void deviceColorSensorHandleRawData(unsigned char commandHeader, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream) {
if (commandHeader == COMMAND_COLOR_SENSOR_READ) {
ackCommand(outputStream, COLOR_SENSOR_DEVICE_HEADER, COMMAND_COLOR_SENSOR_READ);
Color* color = colorSensor->colorSensorReadValue(colorSensor);
appendHex4(outputStream, color->R);
appendSeparator(outputStream);
appendHex4(outputStream, color->G);
appendSeparator(outputStream);
appendHex4(outputStream, color->B);
}
else if (commandHeader == COMMAND_COLOR_SENSOR_READ_TYPE) {
ackCommand(outputStream, COLOR_SENSOR_DEVICE_HEADER, COMMAND_COLOR_SENSOR_READ_TYPE);
enum ColorType colorType = colorSensor->colorSensorFindColorType(colorSensor);
appendHex2(outputStream, colorType);
}
else if (commandHeader == COMMAND_COLOR_SENSOR_DEBUG) {
ackCommand(outputStream, COLOR_SENSOR_DEVICE_HEADER, COMMAND_COLOR_SENSOR_DEBUG);
OutputStream* debugOutputStream = getInfoOutputStreamLogger();
printColorSensorTable(debugOutputStream, colorSensor);
}
/** Only for PC */
else if (commandHeader == COMMAND_COLOR_SENSOR_WRITE) {
ackCommand(outputStream, COLOR_SENSOR_DEVICE_HEADER, COMMAND_COLOR_SENSOR_WRITE);
colorSensor->color->R = readHex4(inputStream);
checkIsSeparator(inputStream);
colorSensor->color->G = readHex4(inputStream);
checkIsSeparator(inputStream);
colorSensor->color->B = readHex4(inputStream);
}
}
void updateNewPositionFromNotification(InputStream* inputStream) {
float x = readHexFloat4(inputStream, POSITION_DIGIT_MM_PRECISION);
checkIsSeparator(inputStream);
float y = readHexFloat4(inputStream, POSITION_DIGIT_MM_PRECISION);
checkIsSeparator(inputStream);
float angleDegree = readHexFloat4(inputStream, ANGLE_DIGIT_DEGREE_PRECISION);
gameStrategyContext->robotPosition->x = x;
gameStrategyContext->robotPosition->y = y;
gameStrategyContext->robotAngleRadian = degToRad(angleDegree);
/*
printPoint(getDebugOutputStreamLogger(), gameStrategyContext->robotPosition, "");
println(getDebugOutputStreamLogger());
*/
}
void mainBoardDeviceHandleMotionDeviceNotification(const Device* device, const unsigned char commandHeader, InputStream* notificationInputStream) {
if (device->deviceInterface->deviceHeader == MOTION_DEVICE_HEADER) {
if (
commandHeader == NOTIFY_MOTION_STATUS_FAILED
|| commandHeader == NOTIFY_MOTION_STATUS_MOVING
|| commandHeader == NOTIFY_MOTION_STATUS_OBSTACLE
|| commandHeader == NOTIFY_MOTION_STATUS_REACHED
|| commandHeader == NOTIFY_MOTION_STATUS_BLOCKED
|| commandHeader == NOTIFY_MOTION_STATUS_SHOCKED) {
updateNewPositionFromNotification(notificationInputStream);
// FAKE DATA To Align with TrajectoryDevice
checkIsSeparator(notificationInputStream);
checkIsChar(notificationInputStream, 'F');
gameStrategyContext->trajectoryType = TRAJECTORY_TYPE_NONE;
appendStringCRLF(getDebugOutputStreamLogger(), "Motion Device Notification !");
}
else {
writeError(NOTIFICATION_BAD_DEVICE_COMMAND_HANDLER_NOT_HANDLE);
appendString(getAlwaysOutputStreamLogger(), "header");
append(getAlwaysOutputStreamLogger(), commandHeader);
println(getAlwaysOutputStreamLogger());
}
}
else {
writeError(NOTIFICATION_BAD_DEVICE);
}
}
void mainBoardDeviceHandleTrajectoryDeviceNotification(const Device* device, const unsigned char commandHeader, InputStream* notificationInputStream) {
// append(getDebugOutputStreamLogger(), device->deviceInterface->deviceHeader);
// println(getDebugOutputStreamLogger());
if (device->deviceInterface->deviceHeader == TRAJECTORY_DEVICE_HEADER) {
if (commandHeader == NOTIFY_TRAJECTORY_CHANGED) {
/*
append(getDebugOutputStreamLogger(), commandHeader);
println(getDebugOutputStreamLogger());
*/
updateNewPositionFromNotification(notificationInputStream);
checkIsSeparator(notificationInputStream);
enum TrajectoryType trajectoryType = readHex(notificationInputStream);
gameStrategyContext->trajectoryType = trajectoryType;
}
else {
writeError(NOTIFICATION_BAD_DEVICE_COMMAND_HANDLER_NOT_HANDLE);
appendString(getAlwaysOutputStreamLogger(), "header");
append(getAlwaysOutputStreamLogger(), commandHeader);
println(getAlwaysOutputStreamLogger());
}
}
else {
writeError(NOTIFICATION_BAD_DEVICE);
}
}
void deviceServoHandleRawData(unsigned char commandHeader, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream) {
// WRITE COMMANDS
if (commandHeader == SERVO_COMMAND_WRITE) {
ServoList* servoList = getServoDeviceServoList();
unsigned int servoIndex = readHex2(inputStream);
checkIsSeparator(inputStream);
unsigned int servoSpeed = readHex2(inputStream);
checkIsSeparator(inputStream);
unsigned int servoValue = readHex4(inputStream);
if (servoIndex == SERVO_ALL_INDEX) {
pwmServoAll(servoList, servoSpeed, servoValue);
} else {
Servo* servo = getServo(servoList, servoIndex);
pwmServo(servo, servoSpeed, servoValue, false);
}
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE);
}
else if (commandHeader == SERVO_COMMAND_WRITE_MAX_SPEED_UNDER_LOAD) {
ServoList* servoList = getServoDeviceServoList();
unsigned int servoIndex = readHex2(inputStream);
checkIsSeparator(inputStream);
int servoMaxSpeedUnderLoad = readHex2(inputStream);
Servo* servo = getServo(servoList, servoIndex);
servo->maxSpeedUnderLoad = servoMaxSpeedUnderLoad;
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE_MAX_SPEED_UNDER_LOAD);
}
else if (commandHeader == SERVO_COMMAND_WRITE_COMPACT) {
unsigned int servoValue = readHex4(inputStream);
ServoList* servoList = getServoDeviceServoList();
pwmServoAll(servoList, PWM_SERVO_SPEED_MAX, servoValue);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE_COMPACT);
}
// ENABLE / DISABLE
if (commandHeader == SERVO_COMMAND_WRITE_ENABLE_DISABLE) {
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_WRITE_ENABLE_DISABLE);
ServoList* servoList = getServoDeviceServoList();
unsigned int servoIndex = readHex2(inputStream);
checkIsSeparator(inputStream);
bool enabled = readBool(inputStream);
if (servoIndex == SERVO_ALL_INDEX) {
servoEnableAll(servoList, enabled);
}
else {
Servo* servo = getServo(servoList, servoIndex);
pwmServoSetEnabled(servo, enabled);
}
}
else if (commandHeader == SERVO_COMMAND_ENABLE_DISABLE_ALL) {
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_ENABLE_DISABLE_ALL);
ServoList* servoList = getServoDeviceServoList();
bool enabled = readBool(inputStream);
servoEnableAll(servoList, enabled);
}
// READ COMMANDS
else if (commandHeader == SERVO_COMMAND_GET_COUNT) {
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_GET_COUNT);
ServoList* servoList = getServoDeviceServoList();
unsigned int servoCount = getServoCount(servoList);
appendHex2(outputStream, servoCount);
}
else if (commandHeader == SERVO_COMMAND_READ) {
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ);
unsigned int servoIndex = readHex2(inputStream);
ServoList* servoList = getServoDeviceServoList();
Servo* servo = getServo(servoList, servoIndex);
unsigned int speed = pwmServoReadTargetSpeed(servo);
unsigned int currentPosition = pwmServoReadCurrentPosition(servo);
unsigned int targetPosition = pwmServoReadTargetPosition(servo);
appendHex2(outputStream, servoIndex);
appendSeparator(outputStream);
appendHex2(outputStream, speed);
appendSeparator(outputStream);
appendHex4(outputStream, currentPosition);
appendSeparator(outputStream);
appendHex4(outputStream, targetPosition);
}
else if (commandHeader == SERVO_COMMAND_READ_SPEED) {
unsigned int servoIndex = readHex2(inputStream);
ServoList* servoList = getServoDeviceServoList();
Servo* servo = getServo(servoList, servoIndex);
unsigned int speed = pwmServoReadTargetSpeed(servo);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_SPEED);
appendHex2(outputStream, speed);
}
else if (commandHeader == SERVO_COMMAND_READ_MAX_SPEED_UNDER_LOAD) {
unsigned int servoIndex = readHex2(inputStream);
ServoList* servoList = getServoDeviceServoList();
Servo* servo = getServo(servoList, servoIndex);
unsigned int maxSpeedUnderLoad = pwmServoReadMaxSpeedUnderLoad(servo);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_MAX_SPEED_UNDER_LOAD);
appendHex2(outputStream, maxSpeedUnderLoad);
}
else if (commandHeader == SERVO_COMMAND_READ_CURRENT_POSITION) {
unsigned int servoIndex = readHex2(inputStream);
ServoList* servoList = getServoDeviceServoList();
Servo* servo = getServo(servoList, servoIndex);
int currentPosition = pwmServoReadCurrentPosition(servo);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_CURRENT_POSITION);
appendHex4(outputStream, currentPosition);
}
else if (commandHeader == SERVO_COMMAND_READ_TARGET_POSITION) {
unsigned int servoIndex = readHex2(inputStream);
ServoList* servoList = getServoDeviceServoList();
Servo* servo = getServo(servoList, servoIndex);
int targetPosition = pwmServoReadTargetPosition(servo);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_READ_TARGET_POSITION);
appendHex4(outputStream, targetPosition);
}
// DEBUG COMMANDS
else if (commandHeader == SERVO_COMMAND_TEST) {
ServoList* servoList = getServoDeviceServoList();
testAllPwmServos(servoList);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_TEST);
}
else if (commandHeader == SERVO_COMMAND_GET_TIME_TO_REACH_UNDER_LOAD) {
unsigned int servoIndex = readHex2(inputStream);
checkIsSeparator(inputStream);
unsigned int servoTargetPosition = readHex4(inputStream);
ServoList* servoList = getServoDeviceServoList();
Servo* servo = getServo(servoList, servoIndex);
unsigned int timeToReachUnderLoad = pwmServoComputeTimeMilliSecondsToReachTargetPosition(servo, servoTargetPosition);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_GET_TIME_TO_REACH_UNDER_LOAD);
appendHex4(outputStream, timeToReachUnderLoad);
}
else if (commandHeader == SERVO_COMMAND_DEBUG) {
ServoList* servoList = getServoDeviceServoList();
printServoList(getInfoOutputStreamLogger(), servoList);
ackCommand(outputStream, SERVO_DEVICE_HEADER, SERVO_COMMAND_DEBUG);
}
}
void deviceEepromHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
_deviceEepromCheckInitialized();
if (commandHeader == COMMAND_RELOAD_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_RELOAD_EEPROM);
eeprom_->eepromLoad(eeprom_);
}
else if (commandHeader == COMMAND_DUMP_TO_FILE_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_DUMP_TO_FILE_EEPROM);
eeprom_->eepromDump(eeprom_);
}
else if (commandHeader == COMMAND_DUMP_TO_LOG_OUTPUT_STREAM_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_DUMP_TO_LOG_OUTPUT_STREAM_EEPROM);
OutputStream* debugOutputStream = getInfoOutputStreamLogger();
dumpEepromToOutputStream(eeprom_, debugOutputStream, 0, eeprom_->maxIndex);
}
else if (commandHeader == COMMAND_DUMP_PARTIAL_CONTENT_TO_LOG_OUTPUT_STREAM_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_DUMP_PARTIAL_CONTENT_TO_LOG_OUTPUT_STREAM_EEPROM);
unsigned long startAddress = readHex4(inputStream);
checkIsSeparator(inputStream);
unsigned long length = readHex4(inputStream);
OutputStream* debugOutputStream = getInfoOutputStreamLogger();
dumpEepromToOutputStream(eeprom_, debugOutputStream, startAddress, startAddress + length);
}
else if (commandHeader == COMMAND_CLEAR_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_CLEAR_EEPROM);
unsigned long startAddress = readHex4(inputStream);
checkIsSeparator(inputStream);
unsigned long endAddress = readHex4(inputStream);
clearEeprom(eeprom_, startAddress, endAddress);
}
else if (commandHeader == COMMAND_READ_BYTE_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_READ_BYTE_EEPROM);
unsigned long address = readHex4(inputStream);
char value = eeprom_->eepromReadChar(eeprom_, address);
appendHex2(outputStream, value);
}
else if (commandHeader == COMMAND_READ_INT_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_READ_INT_EEPROM);
unsigned long address = readHex4(inputStream);
int value = eepromReadInt(eeprom_, address);
appendHex4(outputStream, value);
}
else if (commandHeader == COMMAND_WRITE_BYTE_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_WRITE_BYTE_EEPROM);
unsigned long address = readHex4(inputStream);
checkIsSeparator(inputStream);
char data = readHex2(inputStream);
eeprom_->eepromWriteChar(eeprom_, address, data);
}
else if (commandHeader == COMMAND_WRITE_INT_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_WRITE_INT_EEPROM);
unsigned long address = readHex4(inputStream);
checkIsSeparator(inputStream);
int data = readHex4(inputStream);
eepromWriteInt(eeprom_, address, data);
}
else if (commandHeader == COMMAND_READ_BLOCK_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_READ_BLOCK_EEPROM);
unsigned long address = readHex4(inputStream);
int index;
for (index = 0; index < EEPROM_DEVICE_READ_BLOCK_LENGTH; index++) {
char value = eeprom_->eepromReadChar(eeprom_, address + index);
if (index > 0) {
appendSeparator(outputStream);
}
appendHex2(outputStream, value);
}
}
else if (commandHeader == COMMAND_WRITE_BLOCK_EEPROM) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_WRITE_BLOCK_EEPROM);
unsigned long address = readHex4(inputStream);
char data;
int index;
for (index = 0; index < EEPROM_DEVICE_WRITE_BLOCK_LENGTH; index++) {
checkIsSeparator(inputStream);
data = readHex2(inputStream);
eeprom_->eepromWriteChar(eeprom_, address + index, data);
}
}
else if (commandHeader == COMMAND_INTENSIVE_TEST) {
ackCommand(outputStream, EEPROM_DEVICE_HEADER, COMMAND_INTENSIVE_TEST);
unsigned long address = readHex4(inputStream);
checkIsSeparator(inputStream);
unsigned long length = readHex4(inputStream);
unsigned int errorCount = 0;
unsigned int index;
// Writes
for (index = 0; index < length; index++) {
unsigned char value = (unsigned char) index;
eeprom_->eepromWriteChar(eeprom_, address + index, value);
}
// Reads
for (index = 0; index < length; index++) {
unsigned char value = (unsigned char) eeprom_->eepromReadChar(eeprom_, address + index);
if (value != (unsigned char)index) {
if (errorCount < 255) {
errorCount++;
}
}
}
appendHex4(outputStream, errorCount);
}
}
void deviceTimerHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_TIMER_LIST) {
printTimerList(getInfoOutputStreamLogger(), getTimerList());
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_LIST);
}
else if (commandHeader == COMMAND_TIMER_COUNT) {
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_COUNT);
unsigned timerCount = getTimerCount();
appendHex2(outputStream, timerCount);
}
else if (commandHeader == COMMAND_TIMER_READ) {
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_READ);
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
appendHex2(outputStream, timerIndex);
appendSeparator(outputStream);
appendHex2(outputStream, timer->timerCode);
appendSeparator(outputStream);
appendHex4(outputStream, timer->timeDiviser);
appendSeparator(outputStream);
appendHex4(outputStream, timer->timeInternalCounter);
appendSeparator(outputStream);
appendHex6(outputStream, timer->time);
appendSeparator(outputStream);
appendHex6(outputStream, timer->markTime);
appendSeparator(outputStream);
appendBool(outputStream, timer->enabled);
}
// Enable / Tisable
else if (commandHeader == COMMAND_TIMER_ENABLE_DISABLE) {
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
checkIsSeparator(inputStream);
unsigned enableAsChar = readHex(inputStream);
bool enabled = enableAsChar != 0;
timer->enabled = enabled;
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_ENABLE_DISABLE);
}
// Mark
else if (commandHeader == COMMAND_TIMER_MARK) {
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
unsigned long time = markTimer(timer);
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_MARK);
appendHex6(outputStream, time);
}
else if (commandHeader == COMMAND_TIMER_TIME_SINCE_LAST_MARK) {
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
unsigned long value = getTimeSinceLastMark(timer);
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_TIME_SINCE_LAST_MARK);
appendHex6(outputStream, value);
}
else if (commandHeader == COMMAND_TIMER_TIMEOUT) {
unsigned char timerIndex = readHex2(inputStream);
checkIsSeparator(inputStream);
unsigned long timeToCheck = (unsigned long) readHex6(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
bool value = timeout(timer, timeToCheck);
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_TIMEOUT);
appendHex2(outputStream, timerIndex);
appendSeparator(outputStream);
appendBool(outputStream, value);
}
// Demo
else if (commandHeader == COMMAND_TIMER_DEMO) {
Timer* timer = getTimerByCode(DEMO_TIMER_INDEX);
if (timer == NULL) {
timer = addTimerDemo();
}
// Timer could be null when adding the timerDemo because of limit, we don't want any crash !
if (timer != NULL) {
unsigned enableAsChar = readHex(inputStream);
bool enabled = enableAsChar != 0;
timer->enabled = enabled;
}
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_DEMO);
}
}
