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); } }