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