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