void notifyWaitingStart(StartMatch* startMatch, OutputStream* pcOutputStream) { appendString(pcOutputStream, NOTIFY_TO_PC_RESET); appendString(getAlwaysOutputStreamLogger(), "CFG:"); appendBinary16(getAlwaysOutputStreamLogger(), getConfigValue(startMatch->robotConfig), 4); println(getAlwaysOutputStreamLogger()); appendString(getAlwaysOutputStreamLogger(), "Waiting start:"); }
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 loopUntilStart(StartMatch* startMatch) { if (startMatch == NULL) { writeError(ROBOT_START_MATCH_DETECTOR_PC_NULL); return; } if (startMatch->waitForStart) { appendString(getAlwaysOutputStreamLogger(), "WAIT START..."); while (!startMatch->isMatchStartedFunction(startMatch)) { startMatch->loopUntilStartHandleFunction(startMatch); } appendString(getAlwaysOutputStreamLogger(), "OK"); println(getAlwaysOutputStreamLogger()); markStartMatch(startMatch->endMatch); } else { appendString(getAlwaysOutputStreamLogger(), "GO !"); println(getAlwaysOutputStreamLogger()); } }
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 mainBoardCommonMainInit(RobotConfig* robotConfig) { // LOG the BoardName appendStringCRLF(getAlwaysOutputStreamLogger(), getBoardName()); // Increase the Log Level to DEBUG if the config bit are set Logger* logger = getLoggerInstance(); if (isConfigSet(robotConfig, CONFIG_DEBUG)) { logger->globalLogLevel = LOG_LEVEL_DEBUG; } appendString(getInfoOutputStreamLogger(), "GLOBAL LEVEL : "); appendLevelAsString(getInfoOutputStreamLogger(), logger->globalLogLevel); println(getInfoOutputStreamLogger()); }
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(); } }
/** * @private * Try to clear the buffer if it contains some 'z' char. Very usefull when we have made a mistake taping an instruction. * @return true if it was cleared (=> buffer will be cleared), false else */ bool clearBufferIfNeeded(Buffer* inputBuffer) { int i; int inputBufferCount = getBufferElementsCount(inputBuffer); for (i = 0; i < inputBufferCount; i++) { char bufferElement = bufferGetCharAtIndex(inputBuffer, i); if (bufferElement == HEADER_CLEAR_INPUT_STREAM) { deepClearBuffer(inputBuffer); return true; } // remove all informations to the latest char if (bufferElement == HEADER_WRITE_CONTENT_AND_DEEP_CLEAR_BUFFER) { printDebugBuffer(getAlwaysOutputStreamLogger(), inputBuffer); deepClearBuffer(inputBuffer); return true; } } return false; }
void mainBoardCommonStrategyMainLoop(void) { StartMatch* startMatch = mainBoardCommonMatchGetStartMatch(); EndMatch* endMatch = mainBoardCommonMatchGetEndMatch(); mainBoardCommonMatchLoopUntilStart(); while (true) { if (!startMatch->matchHandleInstructionFunction(startMatch)) { break; } // Protection against the start before the match if (gameStrategyContext->loopTargetAndActions) { nextTargetOrNextStep(gameStrategyContext); } // After each instruction => Export the score to endMatch Device endMatch->scoreToShow = gameStrategyContext->score; if (showEndAndScoreIfNeeded(endMatch, getAlwaysOutputStreamLogger())) { break; } } }
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 notifyStarted(StartMatch* startMatch, OutputStream* pcOutputStream) { // Notify the pc that the match started appendString(pcOutputStream, NOTIFY_TO_PC_START); appendStringCRLF(getAlwaysOutputStreamLogger(), "Go !"); }
/** * The interrupt demo timer. */ void interruptDemoTimerCallbackFunc(Timer* timer) { appendStringAndDec(getAlwaysOutputStreamLogger(), "counter=", demoCounter); appendCRLF(getAlwaysOutputStreamLogger()); demoCounter++; }
int main(void) { setBoardName("MAIN BOARD"); setRobotMustStop(false); // LOG Global Configuration initLogs(DEBUG, &logHandlerListArray, MAIN_BOARD_LOG_HANDLER_LIST_LENGTH, LOG_HANDLER_CATEGORY_ALL_MASK); // LCD (LCD via Parallel interface) initLCDOutputStream(&lcdOutputStream); addLogHandler("LCD", &lcdOutputStream, LOG_LEVEL_ERROR, LOG_HANDLER_CATEGORY_ALL_MASK); // CONFIG initRobotConfigPic32(&robotConfig); // Open the serial Link for debug and LOG ! 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); // Serial Debug LOG addLogHandler("UART", &debugOutputStream, DEBUG, LOG_HANDLER_CATEGORY_ALL_MASK); // LOG the BoardName appendString(getAlwaysOutputStreamLogger(), getBoardName()); println(getAlwaysOutputStreamLogger()); // SERIAL // Open the serial Link for the Motor Board openSerialLink(&motorSerialStreamLink, &motorInputBuffer, &motorInputBufferArray, MAIN_BOARD_MOTOR_INPUT_BUFFER_LENGTH, &motorOutputBuffer, &motorOutputBufferArray, MAIN_BOARD_MOTOR_OUTPUT_BUFFER_LENGTH, &motorOutputStream, SERIAL_PORT_MOTOR, DEFAULT_SERIAL_SPEED); // Open the serial Link for the Mechanical Board openSerialLink(&mechanicalBoard2SerialStreamLink, &mechanicalBoard2InputBuffer, &mechanicalBoard2InputBufferArray, MAIN_BOARD_MECA2_INPUT_BUFFER_LENGTH, &mechanicalBoard2OutputBuffer, &mechanicalBoard2OutputBufferArray, MAIN_BOARD_MECA2_OUTPUT_BUFFER_LENGTH, &mechanicalBoard2OutputStream, SERIAL_PORT_MECA2, 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); // MAIN I2C initI2cBus(&i2cBus, I2C_BUS_TYPE_MASTER, I2C_BUS_PORT_1); i2cMasterInitialize(&i2cBus); initI2cBusConnection(&motorI2cBusConnection, &i2cBus, MOTOR_BOARD_I2C_ADDRESS); initI2cBusConnection(&mechanicalBoard2I2cBusConnection, &i2cBus, MECHANICAL_BOARD_2_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); setDebugI2cEnabled(false); // -> Eeproms initI2cBusConnection(&eepromI2cBusConnection, &i2cBus, ST24C512_ADDRESS_0); init24C512Eeprom(&eeprom, &eepromI2cBusConnection); // -> Clock initI2cBusConnection(&clockI2cBusConnection, &i2cBus, PCF8563_WRITE_ADDRESS); initClockPCF8563(&clock, &clockI2cBusConnection); // -> Temperature initI2cBusConnection(&temperatureI2cBusConnection, &i2cBus, LM75A_ADDRESS); initTemperatureLM75A(&temperature, &temperatureI2cBusConnection); // TIMERS initTimerList(&timerListArray, MAIN_BOARD_TIMER_LENGTH); // DEVICES, DRIVERS, DISPATCHERS initMainBoardDevicesDescriptor(); initMainBoardDriversDescriptor(); initMainBoardDriverDataDispatcherList(); // Start interruptions startTimerList(); loopUntilStart(&startMatch); counter = 1; clearBuffer(&mechanicalBoard2InputBuffer); // enableNotificationRobotInfraredDetector(DETECTOR_GROUP_TYPE_FORWARD); while (1) { if (!mainBoardWaitForInstruction(&startMatch)) { break; } } showEnd(getAlwaysOutputStreamLogger()); while (1) { // Avoid reboot even at end } }