/*******************************************************************************
* Copyright (c) 2016 Julien Louette & Gaƫl Wittorski
*
* This file is part of Raspoid.
*
* Raspoid is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Raspoid is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Raspoid. If not, see <http://www.gnu.org/licenses/>.
******************************************************************************/
package com.raspoid.brickpi;
import static com.raspoid.brickpi.Atmel.CHIP1;
import static com.raspoid.brickpi.Atmel.CHIP2;
import static com.raspoid.brickpi.BrickPi.NB_MOTORS_BY_ATMEGA;
import static com.raspoid.brickpi.BrickPi.NB_SENSORS_BY_ATMEGA;
import java.util.AbstractMap;
import java.util.Map;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;
import com.raspoid.brickpi.uart.BrickPiSerialTransmitter;
import com.raspoid.brickpi.uart.PacketFormatter;
import com.raspoid.brickpi.uart.message.AckMessage;
import com.raspoid.brickpi.uart.message.AckValuesMessage;
import com.raspoid.brickpi.uart.message.Message;
import com.raspoid.brickpi.uart.message.SensorTypeMessage;
import com.raspoid.brickpi.uart.message.TimeoutSettingsMessage;
import com.raspoid.brickpi.uart.message.ValuesMessage;
/**
* BrickPi connector implementing the message exchanges between the
* raspberry pi and the BrickPi. It is implementing the init phase,
* the regular polling of new values and the update of the local BrickPi state.
*
* @author Julien Louette & Gaël Wittorski
* @version 1.0
*/
public class BrickPiConnector {
/**
* How frequently send valuesMessages to get updated values from the brick pi.
* This is in milliseconds.
*/
public static final int DEFAULT_DELAY = 20;
/**
* Contains the timeout after which we assume the response was lost
*/
public static final int RECV_TIMEOUT = 25;
/**
* Timeout to shutdown motors.
* When the brick pi doesn't receive information request
* within the last MOTOR_TIMEOUT ms, the motors are stopped.
*/
public static final int MOTOR_TIMEOUT = 100;
/**
* Single thread pool used for sending messages over the serial link
*/
private ScheduledExecutorService serialExecutor = Executors.newSingleThreadScheduledExecutor();
/**
* Single thread pool used for updating the local state of the BrickPi
*/
private ExecutorService notifyExecutor = Executors.newSingleThreadExecutor();
/**
* Serial transmitter used for exchanging messages with the BrickPi
*/
private BrickPiSerialTransmitter transmitter = new BrickPiSerialTransmitter(RECV_TIMEOUT);
/**
* Values message destined for the 1st Atmel
*/
private ValuesMessage valuesMessage1;
/**
* Values message destined for the 2nd Atmel
*/
private ValuesMessage valuesMessage2;
/**
* Starts the execution of the BrickPi
*/
public void start() {
// Create the values message with the motors configuration
// Each time the motors are needed, we retrieve them through a supplier
// from the BrickPi class. This is because we need to ensure there is no
// lock currently set because we could be in the middle of a transaction
valuesMessage1 = new ValuesMessage(() -> BrickPi.MA, () -> BrickPi.MB);
valuesMessage2 = new ValuesMessage(() -> BrickPi.MC, () -> BrickPi.MD);
// Set the timeout to shutdown the motors
TimeoutSettingsMessage timeoutMsg = new TimeoutSettingsMessage(MOTOR_TIMEOUT);
sendMessage(CHIP1, timeoutMsg);
sendMessage(CHIP2, timeoutMsg);
// Configure the sensors
// We send one packet to each AtMega to configure the corresponding sensors.
Sensor[] sensors = BrickPi.getSensors();
for(int atmelIndex = 0; atmelIndex < BrickPi.NB_ATMEGA_TARGETS; atmelIndex++) {
sendMessage(Atmel.valueOf((byte) (atmelIndex + 1)),
new SensorTypeMessage(sensors[atmelIndex * BrickPi.NB_ATMEGA_TARGETS],
sensors[atmelIndex * BrickPi.NB_ATMEGA_TARGETS + 1]));
}
// Regularly send the values message to the BrickPi
// This will update the BrickPi motor speed if it was changed locally
// It will refresh the local values for motor encoders and sensors values
serialExecutor.scheduleAtFixedRate(() -> {
sendThenNotify(CHIP1, valuesMessage1);
sendThenNotify(CHIP2, valuesMessage2);
} , 0, DEFAULT_DELAY, TimeUnit.MILLISECONDS);
}
/**
* Stops the execution of the BrickPi
*/
public void stop() {
// Flush so that the last values set are transmitted
flush();
serialExecutor.shutdown();
notifyExecutor.shutdown();
}
/**
* Send a message over the serial transmitter, then notify using the notify executor
* @param chip the chip on which to send the message
* @param msg the message to be send
*/
private void sendThenNotify(Atmel chip, Message msg) {
notifyExecutor.submit(() ->
updateBrickPi(transmitter.sendMessage(new AbstractMap.SimpleEntry<>(chip, PacketFormatter.encode(chip, msg))))
);
}
/**
* Send a message to the BrickPi using the serial executor to send the message
* and the notify executor to update the BrickPi state accordingly
* @param chip the chip on which to send the message
* @param msg the message to be send
*/
private void sendMessage(Atmel chip, Message msg) {
serialExecutor.submit(() -> sendThenNotify(chip, msg));
}
/**
* Immediately send the values message to update the BrickPi
* with the values of the local state
*/
public void flush() {
sendMessage(CHIP1, valuesMessage1);
sendMessage(CHIP2, valuesMessage2);
}
/**
* Update the local state of the BrickPi with the values received from the BrickPi
* @param byteChunksRead the byte chunk read along with the Atmel it was received from
*/
private void updateBrickPi(Map.Entry<Atmel, byte[]> byteChunksRead) {
AckMessage ackMsg = PacketFormatter.decode(byteChunksRead);
if (ackMsg instanceof AckValuesMessage) {
AckValuesMessage ackValMsg = (AckValuesMessage) ackMsg;
int chipIndex = ackMsg.getOrigin().getIndex();
/**
* Update the motors encoders 1 Atmel chip, that is 2 motors.
*/
for (int deviceNum = 0; deviceNum < NB_MOTORS_BY_ATMEGA; deviceNum++) {
int deviceIndex = chipIndex + deviceNum;
// Update the motor encoder
Motor currentMotor = BrickPi.getMotors()[deviceIndex];
if (currentMotor != null) {
currentMotor.setEncoderValue(ackValMsg.getMotorEncoderValue(deviceNum));
}
}
/**
* Update the sensors values of 1 Atmel chip, that is 2 sensors.
*/
for (int deviceNum = 0; deviceNum < NB_SENSORS_BY_ATMEGA; deviceNum++) {
int deviceIndex = chipIndex + deviceNum;
// Update the sensor value
BrickPi.getSensors()[deviceIndex].setValue(ackValMsg.getSensorValue(deviceNum));
}
}
}
}