void motorBoardWaitForInstruction(void) {
// delaymSec(MOTOR_BOARD_PC_DELAY_CONSOLE_ANALYZE_MILLISECONDS);
// Analyze data from the Console (Specific to PC)
while (consoleInputStream.availableData(&consoleInputStream)) {
unsigned char c = consoleInputStream.readChar(&consoleInputStream);
consoleInputBuffer.outputStream.writeChar(&(consoleInputBuffer.outputStream), c);
if (c == 13) {
appendCRLF(&consoleOutputStream);
}
consoleOutputStream.writeChar(&consoleOutputStream, c);
}
// Analyse from the ConsoleBuffer
handleStreamInstruction(
&consoleInputBuffer,
&consoleOutputBuffer,
&consoleOutputStream,
&filterRemoveCRLF,
NULL);
if (!singleModeActivated) {
// Analyse data from the I2C
handleStreamInstruction(
&i2cSlaveInputBuffer,
&i2cSlaveOutputBuffer,
NULL,
&filterRemoveCRLF,
NULL);
}
handleInstructionAndMotion();
}
void mechanicalBoard1PcWaitForInstruction(void) {
// Analyze data from the Console (Specific to PC)
while (consoleInputStream.availableData(&consoleInputStream)) {
unsigned char c = consoleInputStream.readChar(&consoleInputStream);
consoleInputBuffer.outputStream.writeChar(&(consoleInputBuffer.outputStream), c);
if (c == 13) {
appendCRLF(&consoleOutputStream);
}
consoleOutputStream.writeChar(&consoleOutputStream, c);
}
// Analyse from the ConsoleBuffer
handleStreamInstruction(
&consoleInputBuffer,
&consoleOutputBuffer,
&consoleOutputStream,
&consoleOutputStream,
&filterRemoveCRLF,
NULL);
if (!singleModeActivated) {
// Analyse data from the I2C
handleStreamInstruction(
&i2cSlaveInputBuffer,
&i2cSlaveOutputBuffer,
NULL,
NULL,
&filterRemoveCRLF,
NULL);
}
}
/**
* @private
* Transmit the buffer locally, call the device which write data out to the stream
*/
void _driverDataDispatcherTransmitLocal(DriverDataDispatcher* dispatcher,
Buffer* inputBuffer,
Buffer* outputBuffer,
int dataToTransferCount,
int dataToReceiveCount
) {
handleStreamInstruction(inputBuffer, outputBuffer, NULL, NULL, NULL);
}
void mainBoardCommonHandleStreamInstruction(void) {
// Listen instruction from debugStream->Devices
handleStreamInstruction(
&debugInputBuffer,
&debugOutputBuffer,
&debugOutputStream,
NULL,
&filterRemoveCRLF_255,
NULL);
}
bool mainBoardPcWaitForInstruction(StartMatch* startMatch) {
while (handleNotificationFromDispatcherList(TRANSMIT_I2C)) {
// loop for all notification
// notification handler must avoid to directly information in notification callback
// and never to the call back device
}
// delaymSec(MAIN_BOARD_PC_DELAY_CONSOLE_ANALYZE_MILLISECONDS);
// Fill Console Buffer
while (consoleInputStream.availableData(&consoleInputStream)) {
unsigned char c = consoleInputStream.readChar(&consoleInputStream);
consoleInputBuffer.outputStream.writeChar(&(consoleInputBuffer.outputStream), c);
if (c == 13) {
appendCRLF(&consoleOutputStream);
}
consoleOutputStream.writeChar(&consoleOutputStream, c);
}
// From Text Console
handleStreamInstruction(
&consoleInputBuffer,
&consoleOutputBuffer,
&consoleOutputStream,
&filterRemoveCRLF,
NULL);
if (connectToRobotManager) {
// From RobotManager
handleStreamInstruction(
&robotManagerInputBuffer,
&robotManagerOutputBuffer,
&robotManagerOutputStream,
&filterRemoveCRLF,
NULL);
}
return true;
}
bool mainBoardWaitForInstruction(StartMatch* startMatchParam) {
// BE CAREFUL : UART 1 / UART 2 are physically connected : https://github.com/svanacker/cen-electronic-schema/issues/11
// -> DO NOT USE IT AT THE SAME TIME !!
// Listen instruction from pcStream->Devices
/*
handleStreamInstruction(
&pcInputBuffer,
&pcOutputBuffer,
&pcOutputStream,
&filterRemoveCRLF,
NULL);
*/
/*
if (counter > 0) {
counter++;
if ((counter % 20) == 0) {
appendString(getAlwaysOutputStreamLogger(), "ASK ! \n");
if (robotInfraredDetectorHasObstacle(DETECTOR_GROUP_TYPE_FORWARD)) {
appendString(getAlwaysOutputStreamLogger(), "DETECTED ROBOT !");
// We stop
return false;
}
}
}
*/
// Listen instruction from debugStream->Devices
handleStreamInstruction(
&debugInputBuffer,
&debugOutputBuffer,
&debugOutputStream,
&filterRemoveCRLF,
NULL);
return true;
}
void waitForInstruction() {
// Listen instruction from pcStream->Devices
handleStreamInstruction(
&pcInputBuffer,
&pcOutputBuffer,
&pcOutputStream,
&filterRemoveCRLF,
NULL);
// Listen instruction from debugStream->Devices
handleStreamInstruction(
&debugInputBuffer,
&debugOutputBuffer,
&debugOutputStream,
&filterRemoveCRLF,
NULL);
// Listen instructions from Devices (I2C Slave) -> Main Board (I2C Master)
/*
while (handleNotificationFromDispatcherList(TRANSMIT_I2C)) {
// loop for all notification
// notification handler must avoid to directly information in notification callback
// and never to the call back device
}
*/
/*
// Notify to the strategy board the position of the robot
if (isRobotPositionChanged()) {
sentStrategyRobotPosition(0, getRobotPositionX(), getRobotPositionY(), getRobotAngle());
resetRobotPositionChanged();
}
if (mustNotifyObstacle) {
mustNotifyObstacle = false;
// Obtain robot position
// Ask the robot position from the MOTOR BOARD
trajectoryDriverUpdateRobotPosition();
// compute the obstacle position. If it's outside the table, does nothing
int obstacleDistance = 350.0f;
appendStringAndDec(getInfoOutputStreamLogger(), "\nInstruction Type:", instructionType);
if (instructionType == INSTRUCTION_TYPE_BACKWARD) {
obstacleDistance = -obstacleDistance;
}
if (isObstacleOutsideTheTable(obstacleDistance)) {
appendString(getInfoOutputStreamLogger(), "\nObstacle OUT side the Table!\n");
}
else {
appendString(getInfoOutputStreamLogger(), "\nObstacle !\n");
// Send information to Strategy Board
stopRobotObstacle();
armDriver2012Up(ARM_LEFT);
armDriver2012Up(ARM_RIGHT);
// we are ready for next motion (next loop)
setReadyForNextMotion(true);
}
}
// Update the current Opponent Robot position
if (useBalise) {
updateOpponentRobotIfNecessary();
}
*/
}
bool transmitFromDriverRequestBuffer() {
// Handle redirection
if (redirectFunction != NULL) {
bool result = redirectFunction();
return result;
}
// We do exactly as if the data was written by a end-user
// requestBuffer must be filled before calling this method
Buffer* requestBuffer = getDriverRequestBuffer();
Buffer* responseBuffer = getDriverResponseBuffer();
InputStream* inputStream = getDriverResponseInputStream();
if (inputStream == NULL) {
writeError(DRIVER_INPUT_STREAM_NULL);
return false;
}
// The first char is the device header
unsigned dataDispacherLength = 0;
unsigned char deviceHeader = bufferGetCharAtIndex(requestBuffer, DEVICE_HEADER_INDEX);
if (deviceHeader == DISPATCHER_COMMAND_HEADER) {
dataDispacherLength = DISPATCHER_COMMAND_AND_INDEX_HEADER_LENGTH;
// Reload the real Device Header
deviceHeader = bufferGetCharAtIndex(requestBuffer, dataDispacherLength + DEVICE_HEADER_INDEX);
}
// The second char is the command header
unsigned char commandHeader = bufferGetCharAtIndex(requestBuffer, dataDispacherLength + COMMAND_HEADER_INDEX);
bool result = handleStreamInstruction(
requestBuffer,
responseBuffer,
// Don't copy to an outputStream, because, we
// want to read the content of responseBuffer
NULL,
// TODO : Check why we don't provide any NotificationOutputStream
NULL,
// No Input Filter
NULL,
// No Output Filter
NULL);
// We need ack
result = checkIsAck(inputStream);
if (!result) {
// The buffer is corrupted, but we would like to avoid further problem
clearInputStream(inputStream);
return false;
}
// Device header answer with the same header as the request
checkIsChar(inputStream, deviceHeader);
if (!result) {
// The buffer is corrupted, but we would like to avoid further problem
clearInputStream(inputStream);
return false;
}
// Command header answer with the same header as the request
checkIsChar(inputStream, commandHeader);
if (!result) {
// The buffer is corrupted, but we would like to avoid further problem
clearInputStream(inputStream);
return false;
}
return result;
}
int main(void) {
setBoardName("STRATEGY_BOARD");
initStrategyBoardIO();
openSerialLink( &debugSerialStreamLink,
&debugInputBuffer,
&debugInputBufferArray,
STRATEGY_BOARD_DEBUG_INPUT_BUFFER_LENGTH,
&debugOutputBuffer,
&debugOutputBufferArray,
STRATEGY_BOARD_DEBUG_OUTPUT_BUFFER_LENGTH,
&debugOutputStream,
SERIAL_PORT_DEBUG,
0);
initTimerList(&timerListArray, STRATEGY_BOARD_TIMER_LENGTH);
// Init the logs
initLog(DEBUG);
addLogHandler(&serialLogHandler, "UART", &debugOutputStream, DEBUG);
appendString(getInfoOutputStreamLogger(), getBoardName());
println(getInfoOutputStreamLogger());
openSlaveI2cStreamLink(&i2cSerialStreamLink,
&i2cSlaveInputBuffer,
&i2cSlaveInputBufferArray,
STRATEGY_BOARD_I2C_INPUT_BUFFER_LENGTH,
&i2cSlaveOutputBuffer,
&i2cSlaveOutputBufferArray,
STRATEGY_BOARD_I2C_OUTPUT_BUFFER_LENGTH,
STRATEGY_BOARD_I2C_ADDRESS
);
// init the devices
initDevicesDescriptor();
initDriversDescriptor();
// initStrategy2012(0);
//setColor(COLOR_VIOLET);
// printGameboard(getInfoOutputStreamLogger());
// printStrategyAllDatas(getInfoOutputStreamLogger());
initStrategyHandler();
//addNavigationLocations();
//printDeviceListUsage(getInfoOutputStreamLogger());
while (nextStep());
while (1);
// Init the timers management
startTimerList();
while (1) {
// I2C Stream
handleStreamInstruction(&i2cSlaveInputBuffer,
&i2cSlaveOutputBuffer,
NULL,
&filterRemoveCRLF,
NULL);
// UART Stream
handleStreamInstruction(&debugInputBuffer,
&debugOutputBuffer,
&debugOutputStream,
&filterRemoveCRLF,
NULL);
}
return (0);
}
