void writeBSplineHeader(OutputStream* outputStream) {
appendCRLF(outputStream);
appendKeyAndName(outputStream, "t", CSV_SEPARATOR);
appendKeyAndName(outputStream, "p.x", CSV_SEPARATOR);
appendKeyAndName(outputStream, "p.y", CSV_SEPARATOR);
appendKeyAndName(outputStream, "length", CSV_SEPARATOR);
appendKeyAndName(outputStream, "orientation", CSV_SEPARATOR);
appendCRLF(outputStream);
}
bool convertFloatTest2(void) {
signed long value = -40000;
float result = (float) value;
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "convertFloatTest2\n");
appendString(getDebugOutputStreamLogger(), "NORMAL=-40000.0000");
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "RESULT=");
appendDecf(getDebugOutputStreamLogger(), result);
appendCRLF(getDebugOutputStreamLogger());
return false;
}
void i2cSlaveInitialize(I2cBusConnection* i2cBusConnection) {
// Avoid more than one initialization
if (i2cBusConnection->opened) {
writeError(I2C_SLAVE_ALREADY_INITIALIZED);
return;
}
i2cBusConnection->object = &i2cSlaveBusConnectionPc;
appendString(getDebugOutputStreamLogger(), "I2C Slave Write Address=");
appendHex2(getDebugOutputStreamLogger(), i2cBusConnection->i2cAddress);
appendCRLF(getDebugOutputStreamLogger());
i2cSlaveBusConnectionPc.masterToSlaveHandle = initClientPipe(L"\\\\.\\pipe\\mainBoardPipe");
i2cSlaveBusConnectionPc.slaveToMasterHandle = initServerPipe(L"\\\\.\\pipe\\motorBoardPipe");
i2cBusConnection->opened = true;
// Thread : master => slave
i2cSlaveBusConnectionPc.masterToSlaveThreadHandle = createStandardThread(
masterToSlaveCallback, // thread proc
(LPVOID)i2cBusConnection, // thread parameter
&(i2cSlaveBusConnectionPc.masterToSlaveThreadId)); // returns thread ID
// Thread : slave => master
i2cSlaveBusConnectionPc.slaveToMasterThreadHandle = createStandardThread(
slaveToMasterCallback, // thread proc
(LPVOID)i2cBusConnection, // thread parameter
&(i2cSlaveBusConnectionPc.slaveToMasterThreadId)); // returns thread ID
}
void i2cSlaveInitialize(I2cBusConnection* i2cBusConnection) {
// Avoid more than one initialization
if (i2cBusConnection->opened) {
writeError(I2C_SLAVE_ALREADY_INITIALIZED);
return;
}
i2cBusConnection->opened = true;
appendString(getDebugOutputStreamLogger(), "I2C Slave Write Address=");
appendHex2(getDebugOutputStreamLogger(), i2cBusConnection->i2cAddress);
appendCRLF(getDebugOutputStreamLogger());
if (i2cBusConnection == NULL) {
// Enable the I2C module with clock stretching enabled
OpenI2C1(I2C_ON | I2C_7BIT_ADD | I2C_STR_EN, BRG_VAL);
// 7-bit I2C slave address must be initialised here.
// we shift because i2c address is shift to the right
// to manage read and write address
I2C1ADD = i2cBusConnection->i2cAddress >> 1;
I2C1MSK = 0;
// Interruption on I2C Slave
// -> Priority of I2C Slave interruption
mI2C1SetIntPriority(I2C_INT_PRI_3 | I2C_INT_SLAVE);
// -> Enable Interruption Flag => See the same code in interruption
mI2C1SClearIntFlag();
// Enable I2C (MACRO)
EnableIntSI2C1;
}
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();
}
bool absFloatTest(void) {
signed long value = 40000.0;
signed long absValue = fabsf(value);
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "absFloatTest\n");
appendString(getDebugOutputStreamLogger(), "NORMAL=");
appendDecf(getDebugOutputStreamLogger(), value);
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "RESULT=");
appendDecf(getDebugOutputStreamLogger(), absValue);
appendCRLF(getDebugOutputStreamLogger());
return false;
}
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);
}
}
void printSerialLinkBuffer(OutputStream* outputStream, const SerialLink* serialLink) {
if (serialLink != NULL) {
StreamLink* streamLink = serialLink->streamLink;
// Serial Input Buffer
appendCRLF(outputStream);
Buffer* serialInputBuffer = streamLink->inputBuffer;
appendString(outputStream, "SERIAL INPUT BUFFER : ");
printDebugBuffer(outputStream, serialInputBuffer);
// Serial Output Buffer
appendCRLF(outputStream);
Buffer* serialOutputBuffer = streamLink->outputBuffer;
appendString(outputStream, "SERIAL OUTPUT BUFFER : ");
printDebugBuffer(outputStream, serialOutputBuffer);
}
}
void mainBoardDeviceHandleNotification(const Device* device, const char commandHeader, InputStream* inputStream) {
appendString(getDebugOutputStreamLogger(), "Notification ! commandHeader=");
append(getDebugOutputStreamLogger(), commandHeader);
appendCRLF(getDebugOutputStreamLogger());
if (commandHeader == NOTIFY_TEST) {
unsigned char value = readHex2(inputStream);
appendStringAndDec(getDebugOutputStreamLogger(), "value=", value);
}
}
bool absLimitTest(void) {
signed long value1 = 50000;
signed long limitValue = 40000;
signed long result = limit(value1, limitValue);
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "absLimitTest\n");
appendString(getDebugOutputStreamLogger(), "NORMAL=");
appendDec(getDebugOutputStreamLogger(), limitValue);
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "RESULT=");
appendDec(getDebugOutputStreamLogger(), result);
appendCRLF(getDebugOutputStreamLogger());
return false;
}
void printTimerList(OutputStream* outputStream, TimerList* timerList) {
int i;
for (i = 0; i < timerList->size; i++) {
Timer* timer = (Timer*) timerList->timers;
timer += i;
appendCRLF(outputStream);
printTimer(outputStream, timer);
}
}
void printDriverDataDispatcher(OutputStream* outputStream, DriverDataDispatcher* dispatcher) {
appendString(outputStream, "dispatcher=");
appendString(outputStream, dispatcher->name);
appendString(outputStream, ", transmitMode=");
TransmitMode transmitMode = dispatcher->transmitMode;
appendDec(outputStream, transmitMode);
append(outputStream, '(');
appendString(outputStream, getTransmitModeAsString(transmitMode));
append(outputStream, ')');
appendString(outputStream, ", address=0x");
appendHex2(outputStream, dispatcher->address);
appendCRLF(outputStream);
}
void i2cMasterInitialize(I2cBus* i2cBus) {
OutputStream* outputStream = getDebugOutputStreamLogger();
appendString(outputStream, "Initializing I2C ...");
// Avoid more than one initialization
if (i2cBus->initialized) {
printI2cBus(outputStream, i2cBus);
appendCRLF(outputStream);
appendString(getDebugOutputStreamLogger(), "\n!!! ALREADY INITIALIZED !!!\n");
return;
}
i2cBus->initialized = true;
#define I2C_BRG 0xC6 // 100khz for PIC32
// Configure I2C for 7 bit address mode
#define I2C_CON I2C_ON
i2cBus->config = I2C_CON;
if (i2cBus == NULL) {
OpenI2C1(
// Configure I2C for 7 bit address mode.
I2C_CON,
// 100khz for PIC32.
I2C_BRG);
}
else {
I2C_MODULE i2cModule = getI2C_MODULE(i2cBus->port);
I2CConfigure(i2cModule, I2C_ON);
I2CSetFrequency(i2cModule, GetPeripheralClock(), 100000L);
}
WaitI2C(i2cBus);
// Indicates that it's ok !
appendString(outputStream, "OK\n");
printI2cBus(outputStream, i2cBus);
appendCRLF(outputStream);
}
void i2cSlaveInitialize(I2cBusConnection* i2cBusConnection) {
// Avoid more than one initialization
if (i2cBusConnection->opened) {
writeError(I2C_SLAVE_ALREADY_INITIALIZED);
return;
}
i2cBusConnection->opened = true;
I2CCONbits.STREN = 1;
// I2CCONbits.GCEN = 1;
I2CCONbits.DISSLW = 1;
I2CCONbits.SCLREL = 1;
// 7-bit I2C slave address must be initialised here.
// we shift because i2c address is shift to the right
// to manage read and write address
I2CADD = i2cBusConnection->i2cAddress >> 1;
// Interruption on I2C Slave
// -> Priority of I2C Slave interruption
IPC3bits.SI2CIP = 1;
// -> Enable I2C Slave interruption
IEC0bits.SI2CIE = 1;
// -> Enable Interruption Flag => See the same code in interruption
IFS0bits.SI2CIF = 0;
// Enable I2C
I2CCONbits.I2CEN = 1;
appendString(getDebugOutputStreamLogger(), "I2C Slave CONF=");
appendBinary16(getDebugOutputStreamLogger(), I2CCON, 4);
appendCRLF(getDebugOutputStreamLogger());
appendString(getDebugOutputStreamLogger(), "I2C Slave Write Address=");
appendHex2(getDebugOutputStreamLogger(), i2cBusConnection->i2cAddress);
appendCRLF(getDebugOutputStreamLogger());
}
void printBeaconSystemConfiguration(OutputStream* outputStream) {
Point* opponentPoint = getOpponentRobotPosition();
Laser* laser1 = getLaser(LASER_INDEX_1);
Laser* laser2 = getLaser(LASER_INDEX_2);
printLaserStruct(outputStream, laser1);
println(outputStream);
printLaserStruct(outputStream, laser2);
appendCRLF(outputStream);
// Last Detection Time
appendStringAndDecf(outputStream, "lastDetectionTime=", getLastDetectionTimeInMillis());
println(outputStream);
// Obsolete Detection Time Threshold
appendStringAndDecf(outputStream, "obsoleteDetectionTimeThreshold=", getObsoleteDetectionTimeThreshold());
println(outputStream);
// Notify Time Delay
appendStringAndDecf(outputStream, "notifyTimeDelay=", getNotifyTimeDelay());
println(outputStream);
// Opponent Position
appendString(outputStream, "opponentPosition:");
printPoint(outputStream, opponentPoint, " mm");
// Distance between beacon
appendStringAndDecf(outputStream, "distanceBetweenBeacon=", getDistanceBetweenBeacon());
appendString(outputStream, " mm");
println(outputStream);
// Calibration Point
appendString(outputStream, "calibrationPoint:");
printPoint(outputStream, &(beaconSystem.calibrationPoint), " mm");
// Opponent Position
appendString(outputStream, "opponentPosition:");
printPoint(outputStream, opponentPoint, " mm");
}
void printPosition(OutputStream* outputStream) {
// clearScreen();
appendCRLF(outputStream);
Position* p = getPosition();
appendStringAndDec(outputStream, "left=", getCoderValue(CODER_LEFT));
appendStringAndDec(outputStream, " | right=", getCoderValue(CODER_RIGHT));
println(outputStream);
printPoint(outputStream, &(p->pos), " mm");
appendStringAndAngleInDeg(outputStream, "ang:", p->orientation);
appendStringAndAngleInDeg(outputStream, "\r\nang init:", p->initialOrientation);
appendStringAndDecf(outputStream, "\r\nlastLeft:", lastLeft);
appendString(outputStream, " pulse");
appendStringAndDecf(outputStream, "\r\nlastRight:", lastRight);
appendString(outputStream, " pulse");
appendStringAndAngleInDeg(outputStream, "\r\nlastAng:", lastAngle);
println(outputStream);
}
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;
}
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 debugTrajectoryVariables(char* valueName1, float value1, char* valueName2, float value2) {
OutputStream* outputStream = getDebugOutputStreamLogger();
appendStringAndDecf(outputStream, valueName1, value1);
appendStringAndDecf(outputStream, valueName2, value2);
appendCRLF(outputStream);
}
/**
* The interrupt demo timer.
*/
void interruptDemoTimerCallbackFunc(Timer* timer) {
appendStringAndDec(getAlwaysOutputStreamLogger(), "counter=", demoCounter);
appendCRLF(getAlwaysOutputStreamLogger());
demoCounter++;
}
void bSplineMotionUCompute(void) {
PidMotion* pidMotion = getPidMotion();
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
BSplineCurve* curve = &(motionDefinition->curve);
float pidTime = computationValues->pidTime;
MotionInstruction* thetaInst = &(motionDefinition->inst[THETA]);
float normalPosition = computeNormalPosition(thetaInst, pidTime);
// Computes the time of the bSpline in [0.00, 1.00]
float bSplineTime = computeBSplineTimeAtDistance(curve, normalPosition);
Point normalPoint;
// Computes the normal Point where the robot must be
computeBSplinePoint(curve, bSplineTime, &normalPoint);
// Convert normalPoint into mm space
RobotKinematics* robotKinematics = getRobotKinematics();
float wheelAverageLength = robotKinematics->wheelAverageLengthForOnePulse;
normalPoint.x = normalPoint.x * wheelAverageLength;
normalPoint.y = normalPoint.y * wheelAverageLength;
Position* robotPosition = getPosition();
Point robotPoint = robotPosition->pos;
// GET PID
unsigned pidIndex = getIndexOfPid(THETA, thetaInst->pidType);
unsigned char rollingTestMode = getRollingTestMode();
PidParameter* pidParameter = getPidParameter(pidIndex, rollingTestMode);
// ALPHA
PidMotionError* alphaMotionError = &(computationValues->errors[ALPHA]);
float normalAlpha = computeBSplineOrientationWithDerivative(curve, bSplineTime);
float realAlpha = robotPosition->orientation;
// backward management
if (curve->backward) {
realAlpha += PI;
}
float alphaError = (normalAlpha - realAlpha);
// restriction to [-PI, PI]
alphaError = mod2PI(alphaError);
// Convert angleError into pulse equivalent
float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics);
float alphaPulseError = (-wheelsDistanceFromCenter * alphaError) / wheelAverageLength;
// THETA
PidMotionError* thetaMotionError = &(computationValues->errors[THETA]);
// thetaError must be in Pulse and not in MM
float thetaError = distanceBetweenPoints(&robotPoint, &normalPoint) / wheelAverageLength;
float thetaAngle = angleOfVector(&robotPoint, &normalPoint);
if (curve->backward) {
thetaAngle += PI;
}
float alphaAndThetaDiff = thetaAngle - normalAlpha;
// restriction to [-PI, PI]
alphaAndThetaDiff = mod2PI(alphaAndThetaDiff);
float cosAlphaAndThetaDiff = cosf(alphaAndThetaDiff);
float thetaErrorWithCos = thetaError * cosAlphaAndThetaDiff;
float normalSpeed = computeNormalSpeed(thetaInst, pidTime);
float thetaU = computePidCorrection(thetaMotionError, pidParameter, normalSpeed, thetaErrorWithCos);
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
thetaCurrentValues->u = thetaU;
// ALPHA CORRECTION
alphaPulseError *= 5.0f;
float alphaCorrection = -0.00050f * normalSpeed * thetaError * (alphaAndThetaDiff);
// float alphaCorrection = 0.0f;
alphaPulseError += alphaCorrection;
float alphaU = computePidCorrection(alphaMotionError, pidParameter, 0, alphaPulseError);
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
alphaCurrentValues->u = alphaU;
// LOG
OutputStream* out = getDebugOutputStreamLogger();
// appendStringAndDecf(out, "pt=", pidTime);
appendStringAndDecf(out, ",t=", bSplineTime);
// Normal Position
appendStringAndDecf(out, ",nx=", normalPoint.x);
appendStringAndDecf(out, ",ny=", normalPoint.y);
appendStringAndDecf(out, ",na=", normalAlpha);
// Real position
appendStringAndDecf(out, ",rx=", robotPoint.x);
appendStringAndDecf(out, ",ry=", robotPoint.y);
appendStringAndDecf(out, ",ra=", realAlpha);
// ALPHA
appendStringAndDecf(out, ",ta=", thetaAngle);
appendStringAndDecf(out, ",ae=", alphaError);
//appendStringAndDecf(out, ",atdiff=", alphaAndThetaDiff);
//appendStringAndDecf(out, ",catdiff=", cosAlphaAndThetaDiff);
appendStringAndDecf(out, ",au=", alphaU);
appendStringAndDecf(out, ",ac=", alphaCorrection);
// THETA
// appendString(out, ",np=");
// appendDecf(out, normalPosition);
appendStringAndDecf(out, ",te=", thetaError);
appendStringAndDecf(out, ",tec=", thetaErrorWithCos);
appendStringAndDecf(out, ",tu=", thetaU);
appendCRLF(out);
}
/**
* @private
* @return true if it's ok, false if there is an error
*/
bool printMethodOrNotificationMetaData(OutputStream* outputStream, DeviceInterface* deviceInterface, unsigned char commandHeader, char argumentLength, char resultLength, bool notification) {
bool result = true;
if (argumentLength != DEVICE_HEADER_NOT_HANDLED) {
DeviceMethodMetaData* deviceMethodMetaData = getDeviceMethodMetaData();
// Function Header
appendCharTableData(outputStream, commandHeader, DEVICE_USAGE_HEADER_COLUMN_LENGTH);
// functionName
const char* functionName = deviceMethodMetaData->functionName;
appendStringTableData(outputStream, functionName, DEVICE_USAGE_NAME_COLUMN_LENGTH);
// arguments
appendStringTableData(outputStream, "IN", DEVICE_USAGE_IO_COLUMN_LENGTH);
int argumentCount = deviceMethodMetaData->argumentsSize;
int argumentIndex;
int realArgumentLength = 0;
for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
DeviceArgument deviceArgument = deviceMethodMetaData->arguments[argumentIndex];
realArgumentLength += getLengthOfType(deviceArgument.type);
}
appendDecTableData(outputStream, realArgumentLength, DEVICE_USAGE_IO_SIZE_COLUMN_LENGTH);
// param name empty
appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_NAME_COLUMN_LENGTH);
appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_SIZE_COLUMN_LENGTH);
appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_TYPE_COLUMN_LENGTH);
appendTableSeparator(outputStream);
println(outputStream);
for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
DeviceArgument deviceArgument = deviceMethodMetaData->arguments[argumentIndex];
printArgument(outputStream, &deviceArgument, argumentIndex);
}
// TODO : Error must be check at the beginning of the function and not Inside
if (argumentLength != realArgumentLength) {
writeError(DEVICE_INTERFACE_PROBLEM);
result = false;
appendStringLN(outputStream, "Arguments Definition ERROR !!!!!!");
println(outputStream);
appendStringAndDec(outputStream, "argumentCount=", argumentCount);
println(outputStream);
appendStringAndDec(outputStream, "argumentLength=", argumentLength);
println(outputStream);
appendStringAndDec(outputStream, "realArgumentLength=", realArgumentLength);
println(outputStream);
}
// results
if (!notification) {
appendStringTableData(outputStream, "", DEVICE_USAGE_HEADER_COLUMN_LENGTH);
appendStringTableData(outputStream, "", DEVICE_USAGE_NAME_COLUMN_LENGTH);
appendStringTableData(outputStream, "OUT", DEVICE_USAGE_IO_COLUMN_LENGTH);
int resultCount = deviceMethodMetaData->resultsSize;
int realResultLength = 0;
int resultIndex;
for (resultIndex = 0; resultIndex < resultCount; resultIndex++) {
DeviceArgument resultArgument = deviceMethodMetaData->results[resultIndex];
realResultLength += getLengthOfType(resultArgument.type);
}
appendDecTableData(outputStream, realResultLength, DEVICE_USAGE_IO_SIZE_COLUMN_LENGTH);
appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_NAME_COLUMN_LENGTH);
appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_SIZE_COLUMN_LENGTH);
appendStringTableData(outputStream, "", DEVICE_USAGE_PARAM_TYPE_COLUMN_LENGTH);
appendTableSeparator(outputStream);
println(outputStream);
for (resultIndex = 0; resultIndex < resultCount; resultIndex++) {
DeviceArgument resultArgument = deviceMethodMetaData->results[resultIndex];
printArgument(outputStream, &resultArgument, resultIndex);
}
if (resultLength != realResultLength) {
result = false;
writeError(DEVICE_INTERFACE_PROBLEM);
appendString(outputStream, "Result Parameters Definition ERROR !!!!!!");
appendCRLF(outputStream);
appendStringAndDec(outputStream, "resultCount=", resultCount);
appendCRLF(outputStream);
appendStringAndDec(outputStream, "resultLength=", resultLength);
appendCRLF(outputStream);
appendStringAndDec(outputStream, "realResultLength=", realResultLength);
appendCRLF(outputStream);
}
}
appendTableHeaderSeparatorLine(outputStream);
}
return result;
}
void appendStringCRLF(OutputStream* outputStream, const char* s) {
appendString(outputStream, s);
appendCRLF(outputStream);
}
/**
* @private
* @return true if it's ok, false if there is an error
*/
bool printMethodOrNotificationMetaData(OutputStream* outputStream, DeviceInterface* deviceInterface, char commandHeader, char argumentLength, char resultLength, bool notification) {
bool result = true;
if (argumentLength != DEVICE_HEADER_NOT_HANDLED) {
DeviceMethodMetaData* deviceMethodMetaData = getDeviceMethodMetaData();
char deviceHeader = deviceInterface->deviceHeader;
// DeviceName
const char* deviceName = deviceInterface->deviceGetName();
appendString(outputStream, deviceName);
append(outputStream, DEVICE_NAME_AND_HEADER_SEPARATOR);
// Header
append(outputStream, deviceHeader);
append(outputStream, commandHeader);
append(outputStream, DEVICE_HEADER_AND_TYPE_SEPARATOR);
// functionName
appendString(outputStream, deviceMethodMetaData->functionName);
append(outputStream, ARGUMENTS_START_CHAR);
// arguments
int argumentCount = deviceMethodMetaData->argumentsSize;
int argumentIndex;
int realArgumentLength = 0;
for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
DeviceArgument deviceArgument = deviceMethodMetaData->arguments[argumentIndex];
realArgumentLength += printArgument(outputStream, &deviceArgument, argumentIndex);
}
append(outputStream, ARGUMENTS_STOP_CHAR);
if (argumentLength != realArgumentLength) {
writeError(DEVICE_INTERFACE_PROBLEM);
result = false;
appendString(outputStream, "Arguments Definition ERROR !!!!!!\n");
appendCRLF(outputStream);
appendStringAndDec(outputStream, "argumentCount=", argumentCount);
appendCRLF(outputStream);
appendStringAndDec(outputStream, "argumentLength=", argumentLength);
appendCRLF(outputStream);
appendStringAndDec(outputStream, "realArgumentLength=", realArgumentLength);
appendCRLF(outputStream);
}
// results
if (!notification) {
appendString(outputStream, ARGUMENTS_AND_RESULTS_SEPARATOR);
append(outputStream, ARGUMENTS_START_CHAR);
int resultCount = deviceMethodMetaData->resultsSize;
int realResultLength = 0;
int resultIndex;
for (resultIndex = 0; resultIndex < resultCount; resultIndex++) {
DeviceArgument resultArgument = deviceMethodMetaData->results[resultIndex];
realResultLength += printArgument(outputStream, &resultArgument, resultIndex);
}
if (resultLength != realResultLength) {
result = false;
writeError(DEVICE_INTERFACE_PROBLEM);
appendString(outputStream, "Result Parameters Definition ERROR !!!!!!");
appendCRLF(outputStream);
appendStringAndDec(outputStream, "resultCount=", resultCount);
appendCRLF(outputStream);
appendStringAndDec(outputStream, "resultLength=", resultLength);
appendCRLF(outputStream);
appendStringAndDec(outputStream, "realResultLength=", realResultLength);
appendCRLF(outputStream);
}
if (resultCount == 0) {
appendString(outputStream, "void");
}
}
append(outputStream, ARGUMENTS_STOP_CHAR);
appendString(outputStream, ":");
appendDec(outputStream, argumentLength);
appendString(outputStream, "=>");
appendDec(outputStream, resultLength);
println(outputStream);
}
return result;
}
