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 deviceTrajectoryHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_GET_ABSOLUTE_POSITION) {
appendAck(outputStream);
updateTrajectoryWithNoThreshold();
append(outputStream, COMMAND_GET_ABSOLUTE_POSITION);
notifyAbsolutePositionWithoutHeader(outputStream);
}
else if (header == COMMAND_DEBUG_GET_ABSOLUTE_POSITION) {
appendAck(outputStream);
updateTrajectoryWithNoThreshold();
append(outputStream, COMMAND_DEBUG_GET_ABSOLUTE_POSITION);
OutputStream* debugOutputStream = getOutputStreamLogger(ALWAYS);
printPosition(debugOutputStream);
}
else if (header == COMMAND_SET_ABSOLUTE_POSITION) {
long newX = readHex4(inputStream);
inputStream->readChar(inputStream);
long newY = readHex4(inputStream);
inputStream->readChar(inputStream);
long newAngle = readHex4(inputStream);
appendAck(outputStream);
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
appendStringAndDec(debugOutputStream, "newX=", newX);
appendStringAndDec(debugOutputStream, ",newY=", newY);
appendStringAndDec(debugOutputStream, ",newAngle=", newAngle);
float fX = (float) newX;
float fY = (float) newY;
float fAngle = PI_DIVIDE_1800 * (float) newAngle;
appendStringAndDecf(debugOutputStream, "fX=", fX);
appendStringAndDecf(debugOutputStream, ",fY=", fY);
appendStringAndDecf(debugOutputStream, ",fAngle=", fAngle);
setPosition(fX, fY, fAngle);
append(outputStream, COMMAND_SET_ABSOLUTE_POSITION);
}
}
signed long readSignedHex4(InputStream* inputStream) {
signed long result = readHex4(inputStream);
if (result > 0x7FFF) {
result -= 0x10000;
}
return result;
}
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 deviceSystemHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
if (header == COMMAND_PING) {
// data
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_PING);
// Read and write in output the pingIndex (to control that it's the right which does the response)
unsigned char pingIndex = readHex2(inputStream);
appendHex2(outputStream, pingIndex);
}
// Last Error
else if (header == COMMAND_GET_LAST_ERROR) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_GET_LAST_ERROR);
unsigned int lastError = getLastError();
appendHex4(outputStream, lastError);
}
else if (header == COMMAND_CLEAR_LAST_ERROR) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_CLEAR_LAST_ERROR);
clearLastError();
}
// Device list
else if (header == COMMAND_DEVICE_LIST) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_DEVICE_LIST);
printDeviceList(getInfoOutputStreamLogger());
// Usage
} else if (header == COMMAND_USAGE) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE);
printDeviceListUsage(getInfoOutputStreamLogger(), false);
} else if (header == COMMAND_USAGE_PROBLEM) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE_PROBLEM);
printDeviceListUsage(getInfoOutputStreamLogger(), true);
}
else if (header == COMMAND_USAGE_SPECIFIC_DEVICE) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_USAGE_SPECIFIC_DEVICE);
char deviceHeader = readBinaryChar(inputStream);
int size = getDeviceCount();
int i;
for (i = 0; i < size; i++) {
Device* device = getDevice(i);
if (deviceHeader == device->deviceInterface->deviceHeader) {
println(getInfoOutputStreamLogger());
printDeviceUsage(getInfoOutputStreamLogger(), device, false);
return;
}
}
appendString(getErrorOutputStreamLogger(), "Device Not Found ! ");
}
else if (header == COMMAND_CLS) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_CLS);
#ifdef PC_COMPILER
system("cls");
#else
appendString(outputStream, "Unsupported Operation");
#endif // PC_COMPILER
}
else if (header == COMMAND_RESET) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_RESET);
#ifdef PC_COMPILER
appendString(outputStream, "Unsupported Operation");
#else
// goto 0;
#endif // PC_COMPILER
}
// Notifications
else if (header == COMMAND_NOTIFICATION) {
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_NOTIFICATION);
printDeviceListNotification(getInfoOutputStreamLogger(), false);
} else if (header == COMMAND_WAIT) {
appendAck(outputStream);
int mSec = readHex4(inputStream);
delaymSec(mSec);
append(outputStream, SYSTEM_DEVICE_HEADER);
append(outputStream, COMMAND_WAIT);
} else if (header == COMMAND_BOARD_NAME) {
appendString(getInfoOutputStreamLogger(), getBoardName());
println(getInfoOutputStreamLogger());
ackCommand(outputStream, SYSTEM_DEVICE_HEADER, COMMAND_BOARD_NAME);
}
}
