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