BOOL sentStrategyRobotPosition(unsigned char status, unsigned int x, unsigned int y, int angleInDeciDegree) {
OutputStream* debugOutputStream = getOutputStreamLogger(INFO);
appendString(debugOutputStream, "sentStrategyRobotPosition:");
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_STRATEGY_SET_ROBOT_POSITION);
appendHex2(outputStream, status);
appendSeparator(outputStream);
appendHex4(outputStream, x);
appendSeparator(outputStream);
appendHex4(outputStream, y);
appendSeparator(outputStream);
appendHex4(outputStream, angleInDeciDegree);
appendStringAndDec(debugOutputStream, "status=", status);
appendStringAndDec(debugOutputStream, ", x=", x);
appendStringAndDec(debugOutputStream, ", y=", y);
appendStringAndDec(debugOutputStream, ", angle=", angleInDeciDegree);
println(debugOutputStream);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
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);
}
}
BOOL motionDriverGotoPositionPulse(float left,
float right,
float a,
float s) {
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_MOTION_GOTO_IN_PULSE);
appendHex4(outputStream, left);
appendHex4(outputStream, right);
appendHex2(outputStream, a);
appendHex2(outputStream, s);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
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);
}
}
BOOL motionDriverRight(float rightDeciDegree) {
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_MOTION_RIGHT_IN_DECI_DEGREE);
appendHex4(outputStream, rightDeciDegree);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
BOOL motionDriverLeftOneWheel(float leftDeciDegree) {
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_MOTION_LEFT_ONE_WHEEL_IN_DECI_DEGREE);
appendHex4(outputStream, leftDeciDegree);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
BOOL motionDriverBackward(float distanceInMM) {
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_MOTION_BACKWARD_IN_MM);
appendHex4(outputStream, distanceInMM);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
BOOL sendStrategyOpponentRobotPosition(Point* opponentRobotPosition) {
OutputStream* debugOutputStream = getOutputStreamLogger(INFO);
appendString(debugOutputStream, "sendStrategyOpponentRobotPosition: ");
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_STRATEGY_SET_OPPONENT_ROBOT_POSITION);
appendHex4(outputStream, opponentRobotPosition->x);
appendSeparator(outputStream);
appendHex4(outputStream, opponentRobotPosition->y);
appendStringAndDec(debugOutputStream, " x=", opponentRobotPosition->x);
appendStringAndDec(debugOutputStream, ", y=", opponentRobotPosition->y);
println(debugOutputStream);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
BOOL motionDriverBSplineAbsolute(float x, float y, float angle, float dist0, float dist1, int accelerationFactor, int speedFactor) {
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_MOTION_SPLINE_ABSOLUTE);
appendHex4(outputStream, x);
appendSeparator(outputStream);
appendHex4(outputStream, y);
appendSeparator(outputStream);
appendHex4(outputStream, angle);
appendSeparator(outputStream);
appendHex2(outputStream, dist0);
appendSeparator(outputStream);
appendHex2(outputStream, dist1);
appendSeparator(outputStream);
appendHex(outputStream, accelerationFactor);
appendHex(outputStream, speedFactor);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
BOOL sendStrategyConfiguration(int configuration) {
appendString(getOutputStreamLogger(INFO), "sendStrategyConfiguration\n");
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_STRATEGY_SET_CONFIG);
appendHex4(outputStream, configuration);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
void deviceConfigHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_CONFIG) {
// Send ack first
appendAck(outputStream);
// can take a little time
refreshConfig();
append(outputStream, COMMAND_CONFIG);
appendHex4(outputStream, config);
}
}
int main(void) {
setBoardName("TITAN ELECTRONICAL MAIN BOARD 32bits V-JJ_7");
i2cMasterInitialize();
//setRobotMustStop(false);
// Open the serial Link for debug
openSerialLink(&debugSerialStreamLink,
&debugInputBuffer,
&debugInputBufferArray,
MAIN_BOARD_DEBUG_INPUT_BUFFER_LENGTH,
&debugOutputBuffer,
&debugOutputBufferArray,
MAIN_BOARD_DEBUG_OUTPUT_BUFFER_LENGTH,
&debugOutputStream,
SERIAL_PORT_DEBUG,
DEFAULT_SERIAL_SPEED);
// Open the serial Link for the PC
openSerialLink(&pcSerialStreamLink,
&pcInputBuffer,
&pcInputBufferArray,
MAIN_BOARD_PC_INPUT_BUFFER_LENGTH,
&pcOutputBuffer,
&pcOutputBufferArray,
MAIN_BOARD_PC_OUTPUT_BUFFER_LENGTH,
&pcOutputStream,
SERIAL_PORT_PC,
DEFAULT_SERIAL_SPEED);
// LCD (LCD03 via Serial interface)
initLCDOutputStream(&lcdOutputStream);
initTimerList(&timerListArray, MAIN_BOARD_TIMER_LENGTH);
// Init the logs
initLog(DEBUG);
addLogHandler(&debugSerialLogHandler, "UART", &debugOutputStream, DEBUG);
addLogHandler(&lcdLogHandler, "LCD", &lcdOutputStream, ERROR);
appendString(getAlwaysOutputStreamLogger(), getBoardName());
println(getAlwaysOutputStreamLogger());
initDevicesDescriptor();
initDriversDescriptor();
i2cMasterInitialize();
// Initialize EEPROM and RTC
initClockPCF8563(&globalClock);
init24C512Eeprom(&eeprom_);
initRobotConfigPic32(&robotConfig);
initStartMatchDetector32(&startMatchDetector);
// Initializes the opponent robot
// initOpponentRobot();
/*
// Creates a composite to notify both PC and Debug
initCompositeOutputStream(&compositePcAndDebugOutputStream);
addOutputStream(&compositePcAndDebugOutputStream, &debugOutputStream);
addOutputStream(&compositePcAndDebugOutputStream, &pcOutputStream);
// Creates a composite to redirect to driverRequest and Debug
initCompositeOutputStream(&compositeDriverAndDebugOutputStream);
addOutputStream(&compositeDriverAndDebugOutputStream, &debugOutputStream);
addOutputStream(&compositeDriverAndDebugOutputStream, getDriverRequestOutputStream());
*/
appendString(&debugOutputStream, "DEBUG\n");
// Start interruptions
//startTimerList(); //////RALENTI FORTEMENT LE PIC!!! PLANTE I2C !!!!!!!!
// Configure data dispatcher
//addLocalDriverDataDispatcher();
// Stream for motorBoard
/*
addI2CDriverDataDispatcher(&motorI2cDispatcher,
"MOTOR_BOARD_DISPATCHER",
&motorBoardInputBuffer,
&motorBoardInputBufferArray,
MAIN_BOARD_LINK_TO_MOTOR_BOARD_BUFFER_LENGTH,
&motorBoardOutputStream,
&motorBoardInputStream,
MOTOR_BOARD_I2C_ADDRESS);
*/
/*
// Stream for Mechanical Board 2
addI2CDriverDataDispatcher(&mechanical2I2cDispatcher,
"MECHANICAL_BOARD_2_DISPATCHER",
&mechanical2BoardInputBuffer,
&mechanical2BoardInputBufferArray,
MAIN_BOARD_LINK_TO_MECA_BOARD_2_BUFFER_LENGTH,
&mechanical2BoardOutputStream,
&mechanical2BoardInputStream,
MECHANICAL_BOARD_2_I2C_ADDRESS);
*/
/* // Stream for Air Conditioning
addI2CDriverDataDispatcher(&airConditioningI2cDispatcher,
"AIR_CONDITIONING_DISPATCHER",
&airConditioningBoardInputBuffer,
&airConditioningBoardInputBufferArray,
MAIN_BOARD_LINK_TO_AIR_CONDITIONING_BOARD_BUFFER_LENGTH,
&airConditioningBoardOutputStream,
&airConditioningBoardInputStream,
AIR_CONDITIONING_BOARD_I2C_ADDRESS);
*/
// I2C Debug
initI2CDebugBuffers(&i2cMasterDebugInputBuffer,
&i2cMasterDebugInputBufferArray,
MAIN_BOARD_I2C_DEBUG_MASTER_IN_BUFFER_LENGTH,
&i2cMasterDebugOutputBuffer,
&i2cMasterDebugOutputBufferArray,
MAIN_BOARD_I2C_DEBUG_MASTER_OUT_BUFFER_LENGTH);
appendStringConfig(&lcdOutputStream);
//pingDriverDataDispatcherList(getDebugOutputStreamLogger());
// Inform PC waiting
showWaitingStart(&debugOutputStream);
// wait other board initialization
delaymSec(1500);
// 2012 VALUE
unsigned int configValue = getConfigValue();
unsigned int homologationIndex = configValue & CONFIG_STRATEGY_MASK;
unsigned int color = configValue & CONFIG_COLOR_BLUE_MASK;
appendString(getAlwaysOutputStreamLogger(), "Homologation:");
appendCRLF(getAlwaysOutputStreamLogger());
appendDec(getAlwaysOutputStreamLogger(), homologationIndex);
appendString(getAlwaysOutputStreamLogger(), "Config:");
appendHex4(getAlwaysOutputStreamLogger(), configValue);
appendCRLF(getAlwaysOutputStreamLogger());
useBalise = configValue & CONFIG_USE_BALISE_MASK;
useInfrared = configValue & CONFIG_USE_SONAR_MASK;
if (color != 0) {
appendString(getAlwaysOutputStreamLogger(), "COLOR:VIOLET\n");
}
else {
appendString(getAlwaysOutputStreamLogger(), "COLOR:RED\n");
}
// TODO 2012 SV motionDriverMaintainPosition();
// wait for start
// setInitialPosition(color);
// notify game strategy
// sendStrategyConfiguration(configValue);
loopUntilStart(&waitForInstruction);
// Enable the sonar Status only after start
//setSonarStatus(configValue & CONFIG_USE_SONAR_MASK);
// mark that match begins
markStartMatch();
// write begin of match
showStarted(&pcOutputStream);
if (homologationIndex == 0) {
// MATCH
while (1) {
waitForInstruction();
//CLOCK Read
if (isEnd()) {
break;
}
}
} else {
setReadyForNextMotion(true);
while (1) {
portableStartI2C(i2cBus);
WaitI2C(i2cBus);
portableMasterWriteI2C(FREE_ADDRESS_2);//0x54
WaitI2C(i2cBus);
portableMasterWriteI2C('H');
WaitI2C(i2cBus);
portableMasterWriteI2C('E');
WaitI2C(i2cBus);
portableMasterWriteI2C('L');
WaitI2C(i2cBus);
portableMasterWriteI2C('L');
WaitI2C(i2cBus);
portableMasterWriteI2C('O');
portableStopI2C(i2cBus);
WaitI2C(i2cBus);
int data1,data2,data3;
while(1){
waitForInstruction();
//globalClock.readClock(&globalClock);
//printClock(&debugOutputStream,&globalClock);
//appendCR(&debugOutputStream);
int data = 0;
portableMasterWaitSendI2C(i2cBus);
portableStartI2C(i2cBus);
IdleI2C1();
portableMasterWriteI2C(FREE_ADDRESS_2 + 1);//0x54
WaitI2C(i2cBus);
data = portableMasterReadI2C(i2cBus);
portableAckI2C(i2cBus);
IdleI2C1();
data1 = portableMasterReadI2C(i2cBus);
portableAckI2C(i2cBus);
IdleI2C1();
data2= portableMasterReadI2C(i2cBus);
portableAckI2C(i2cBus);
IdleI2C1();
data3 = portableMasterReadI2C(i2cBus);
portableNackI2C(i2cBus);
IdleI2C1();
append(&lcdOutputStream,data);
append(&lcdOutputStream,data1);
append(&lcdOutputStream,data2);
append(&lcdOutputStream,data3);
portableStopI2C(i2cBus);
IdleI2C1();
delaymSec(500);
}
homologation(homologationIndex, color);
// We stop if we are in homologation mode
if (isRobotMustStop()) {
motionDriverStop();
break;
}
// ultimate tentative to restart the robot if it is blocked
forceRobotNextStepIfNecessary();
}
}
showEnd(&lcdOutputStream);
while (1) {
// Avoid reboot even at end
}
}
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 writeError(int errorCode) {
OutputStream* outputStream = getErrorOutputStreamLogger();
appendString(outputStream, "ERR CODE:");
appendHex4(outputStream, errorCode);
println(outputStream);
}
void appendHexFloat8(OutputStream* stream, float value, float factorToTrunc) {
signed long longValue = (signed long) (value * factorToTrunc);
appendHex4(stream, longValue);
}
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);
}
}
