void deviceLCDHandleRawData(char commandHeader,
InputStream* inputStream,
OutputStream* outputStream) {
if (commandHeader == COMMAND_LCD) {
ackCommand(outputStream, LCD_DEVICE_HEADER, COMMAND_LCD);
// print character
int i;
for (i = 0; i < LCD_CHAR_COUNT_BY_MESSAGE; i++) {
int c = readHex2(inputStream);
// we only valid write data
if (c == '\0') {
break;
}
writeLCDChar(c);
}
}
else if (commandHeader == COMMAND_LCD_TEST) {
ackCommand(outputStream, LCD_DEVICE_HEADER, COMMAND_LCD_TEST);
writeLCDChar('H');
writeLCDChar('E');
writeLCDChar('L');
writeLCDChar('L');
writeLCDChar('O');
writeLCDChar(' ');
writeLCDChar('W');
writeLCDChar('O');
writeLCDChar('R');
writeLCDChar('L');
writeLCDChar('D');
}
else if (commandHeader == COMMAND_CLEAR_LCD) {
ackCommand(outputStream, LCD_DEVICE_HEADER, COMMAND_CLEAR_LCD);
clearScreen();
}
}
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 deviceMotionSimulationHandleRawData(char commandHeader,
InputStream* inputStream,
OutputStream* outputStream) {
if (commandHeader == COMMAND_MOTION_SIMULATION_GET) {
// send acknowledge
ackCommand(outputStream, MOTION_SIMULATION_DEVICE_HEADER, COMMAND_MOTION_SIMULATION_GET);
MotionSimulationParameter* motionSimulationParameter = getMotionSimulationParameter();
appendHex(outputStream, motionSimulationParameter->simulateMotors);
appendHex(outputStream, motionSimulationParameter->simulateCoders);
appendHex(outputStream, motionSimulationParameter->simulateRobotPosition);
}
else if (commandHeader == COMMAND_MOTION_SIMULATION_SET) {
// send acknowledge
ackCommand(outputStream, MOTION_SIMULATION_DEVICE_HEADER, COMMAND_MOTION_SIMULATION_SET);
bool simulateMotors = readHex(inputStream);
bool simulateCoders = readHex(inputStream);
bool simulateRobotPosition = readHex(inputStream);
MotionSimulationParameter* motionSimulationParameter = getMotionSimulationParameter();
motionSimulationParameter->simulateMotors = simulateMotors;
motionSimulationParameter->simulateCoders = simulateCoders;
motionSimulationParameter->simulateRobotPosition = simulateRobotPosition;
}
}
void deviceI2cSlaveDebugHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
// I2C Management
if (header == COMMAND_I2C_DEBUG_SLAVE_DEBUG) {
ackCommand(outputStream, I2C_SLAVE_DEBUG_DEVICE_HEADER, COMMAND_I2C_DEBUG_SLAVE_DEBUG);
printI2cDebugBuffers();
}
else if (header == COMMANG_I2C_DEBUG_SLAVE_ADDRESS) {
ackCommand(outputStream, I2C_SLAVE_DEBUG_DEVICE_HEADER, COMMANG_I2C_DEBUG_SLAVE_ADDRESS);
unsigned char address = slaveDebugDeviceI2cBusConnection->i2cAddress;
appendHex2(outputStream, address);
}
else if (header == COMMAND_I2C_DEBUG_SLAVE_ENABLE_DISABLE) {
ackCommand(outputStream, I2C_SLAVE_DEBUG_DEVICE_HEADER, COMMAND_I2C_DEBUG_SLAVE_ENABLE_DISABLE);
unsigned char enable = readHex2(inputStream);
setDebugI2cEnabled(enable != 0);
}
else if (header == COMMAND_I2C_DEBUG_SLAVE_SEND_CHAR_I2C_TO_MASTER) {
ackCommand(outputStream, I2C_SLAVE_DEBUG_DEVICE_HEADER, COMMAND_I2C_DEBUG_SLAVE_SEND_CHAR_I2C_TO_MASTER);
int c = readHex2(inputStream);
portableSlaveWriteI2C(slaveDebugDeviceI2cBusConnection, c);
}
else if (header == COMMAND_I2C_DEBUG_SLAVE_READ_CHAR_I2C_FROM_MASTER) {
ackCommand(outputStream, I2C_SLAVE_DEBUG_DEVICE_HEADER, COMMAND_I2C_DEBUG_SLAVE_READ_CHAR_I2C_FROM_MASTER);
char c = portableSlaveReadI2C(slaveDebugDeviceI2cBusConnection);
appendHex2(outputStream, c);
}
}
void devicePwmMotorHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_MOVE_MOTOR) {
signed int left = readSignedHex2(inputStream);
signed int right = readSignedHex2(inputStream);
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_MOVE_MOTOR);
setMotorSpeeds(left * 2, right * 2);
}
else if (commandHeader == COMMAND_READ_MOTOR_VALUE) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_READ_MOTOR_VALUE);
signed int left = getLeftSpeed();
signed int right = getRightSpeed();
appendHex2(outputStream, left / 2);
appendHex2(outputStream, right / 2);
}
else if (commandHeader == COMMAND_STOP_MOTOR) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_STOP_MOTOR);
stopMotors();
}
else if (commandHeader == COMMAND_TEST_MOTOR) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_TEST_MOTOR);
appendString(getAlwaysOutputStreamLogger(), "Left Forward\n");
// Left forward
setMotorSpeeds(50, 0);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
// Right forward
setMotorSpeeds(0, 50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Left Backward\n");
// Left backward
setMotorSpeeds(-50, 0);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
// Right backward
setMotorSpeeds(0, -50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Both Forward\n");
// Both forward
setMotorSpeeds(50, 50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Both Backward\n");
// Both backward
setMotorSpeeds(-50, -50);
delaymSec(2000);
stopMotors();
}
}
void deviceTemperatureSensorHandleRawData(unsigned char header, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream){
if (header == COMMAND_READ_TEMPERATURE_SENSOR) {
ackCommand(outputStream, TEMPERATURE_SENSOR_DEVICE_HEADER, COMMAND_READ_TEMPERATURE_SENSOR);
unsigned char value = (unsigned char) temperature->readSensorValue(temperature);
appendHex2(outputStream, value);
} else if (header == COMMAND_SET_TEMPERATURE_SENSOR_ALERT) {
unsigned char temperatureSensorAlert = readHex2(inputStream);
ackCommand(outputStream, TEMPERATURE_SENSOR_DEVICE_HEADER, COMMAND_SET_TEMPERATURE_SENSOR_ALERT);
temperature->writeAlertLimit(temperature, temperatureSensorAlert);
}
}
void deviceStartMatchDetectorHandleRawData(unsigned char commandHeader, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream) {
if (commandHeader == COMMAND_MATCH_IS_STARTED) {
StartMatch* startMatch = getStartMatchDetectorStartMatchObject();
int value = isMatchStarted(startMatch);
ackCommand(outputStream, START_MATCH_DEVICE_HEADER, COMMAND_MATCH_IS_STARTED);
appendHex2(outputStream, value);
}
else if (commandHeader == COMMAND_START_MATCH_DEBUG) {
StartMatch* startMatch = getStartMatchDetectorStartMatchObject();
ackCommand(outputStream, START_MATCH_DEVICE_HEADER, COMMAND_START_MATCH_DEBUG);
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
printStartMatchTable(debugOutputStream, startMatch);
}
}
void deviceBatteryHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream){
_deviceBatteryCheckInitialized();
if (header == COMMAND_READ_BATTERY) {
ackCommand(outputStream, BATTERY_DEVICE_HEADER, COMMAND_READ_BATTERY);
int batteryValue = battery->readBatteryValue(battery);
appendHex4(outputStream, batteryValue);
}
}
void deviceRobotSonarDetectorHandleRawData(char commandHeader,
InputStream* inputStream,
OutputStream* outputStream) {
if (commandHeader == COMMAND_SET_SONAR_STATUS) {
sonarStatus = readHex2(inputStream);
ackCommand(outputStream, ROBOT_SONAR_DETECTOR_DEVICE_HEADER, COMMAND_SET_SONAR_STATUS);
}
}
void deviceDataDispatcherHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) {
// Dispatcher List
if (header == COMMAND_DISPATCHER_LIST) {
ackCommand(outputStream, SYSTEM_DEBUG_DEVICE_HEADER, COMMAND_DISPATCHER_LIST);
DriverDataDispatcherList* dispatcherList = getDispatcherList();
printDriverDataDispatcherList(getAlwaysOutputStreamLogger(), dispatcherList);
}
else if (header == COMMAND_PING_DISPATCHER_INDEX) {
// Handle directly by DriverStreamListener => Throw an error
writeError(DISPATCHER_PING_MUST_BE_HANDLE_IN_DRIVER_STREAM_LISTENER);
}
}
void deviceEndMatchDetectorHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_GET_TIME_LEFT) {
ackCommand(outputStream, END_MATCH_DETECTOR_DEVICE_HEADER, COMMAND_GET_TIME_LEFT);
appendHex2(outputStream, MATCH_DURATION - currentTimeInSecond);
}
}
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);
}
}
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);
}
}
