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 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 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);
}
}
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 notifyInfraredDetectorDetection(int type) {
Buffer* buffer = getMechanicalBoard2I2CSlaveOutputBuffer();
OutputStream* outputStream = getOutputStream(buffer);
append(outputStream, NOTIFY_INFRARED_DETECTOR_DETECTION);
appendHex2(outputStream, type);
// Debug
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
append(debugOutputStream, NOTIFY_INFRARED_DETECTOR_DETECTION);
appendHex2(debugOutputStream, type);
}
int32_t VL53L0X_read_multi(uint8_t deviceAddress, uint8_t index, uint8_t *pdata, int32_t count) {
I2cBusConnection* i2cBusConnection = getI2cBusConnectionBySlaveAddress(deviceAddress);
I2cBus* i2cBus = i2cBusConnection->i2cBus;
portableMasterWaitSendI2C(i2cBusConnection);
portableMasterStartI2C(i2cBusConnection);
WaitI2C(i2cBus);
portableMasterWriteI2C(i2cBusConnection, deviceAddress);
WaitI2C(i2cBus);
// Write the "index" from which we want to read
portableMasterWriteI2C(i2cBusConnection, index);
WaitI2C(i2cBus);
portableMasterStartI2C(i2cBusConnection);
WaitI2C(i2cBus);
// Enter in "read" mode
portableMasterWriteI2C(i2cBusConnection, deviceAddress | 1);
WaitI2C(i2cBus);
#ifdef VL53L0X_DEBUG
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
appendString(debugOutputStream, "\tReading ");
appendDec(debugOutputStream, count);
appendString(debugOutputStream, " from addr 0x");
appendHex2(debugOutputStream, deviceAddress);
appendString(debugOutputStream, ": ");
#endif
while (count--) {
pdata[0] = portableMasterReadI2C(i2cBusConnection);
// Ack
if (count > 0) {
portableMasterAckI2C(i2cBusConnection);
}
else {
portableMasterNackI2C(i2cBusConnection);
}
WaitI2C(i2cBus);
#ifdef VL53L0X_DEBUG
appendString(debugOutputStream, "0x ");
appendHex2(debugOutputStream, pdata[0]);
appendString(debugOutputStream, ", ");
#endif
pdata++;
}
#ifdef VL53L0X_DEBUG
println(debugOutputStream);
#endif
return VL53L0X_ERROR_NONE;
}
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);
}
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 deviceStartMatchDetectorHandleRawData(unsigned char commandHeader, InputStream* inputStream, OutputStream* outputStream, OutputStream* notificationOutputStream) {
if (commandHeader == COMMAND_MATCH_IS_STARTED) {
StartMatch* startMatch = getStartMatchDetectorStartMatchObject();
int value = isMatchStarted(startMatch);
ackCommand(outputStream, START_MATCH_DEVICE_HEADER, COMMAND_MATCH_IS_STARTED);
appendHex2(outputStream, value);
}
else if (commandHeader == COMMAND_START_MATCH_DEBUG) {
StartMatch* startMatch = getStartMatchDetectorStartMatchObject();
ackCommand(outputStream, START_MATCH_DEVICE_HEADER, COMMAND_START_MATCH_DEBUG);
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
printStartMatchTable(debugOutputStream, startMatch);
}
}
void deviceTrajectoryHandleRawData(char header,
InputStream* inputStream,
OutputStream* outputStream) {
if (header == COMMAND_GET_ABSOLUTE_POSITION) {
appendAck(outputStream);
updateTrajectoryWithNoThreshold();
append(outputStream, COMMAND_GET_ABSOLUTE_POSITION);
notifyAbsolutePositionWithoutHeader(outputStream);
}
else if (header == COMMAND_DEBUG_GET_ABSOLUTE_POSITION) {
appendAck(outputStream);
updateTrajectoryWithNoThreshold();
append(outputStream, COMMAND_DEBUG_GET_ABSOLUTE_POSITION);
OutputStream* debugOutputStream = getOutputStreamLogger(ALWAYS);
printPosition(debugOutputStream);
}
else if (header == COMMAND_SET_ABSOLUTE_POSITION) {
long newX = readHex4(inputStream);
inputStream->readChar(inputStream);
long newY = readHex4(inputStream);
inputStream->readChar(inputStream);
long newAngle = readHex4(inputStream);
appendAck(outputStream);
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
appendStringAndDec(debugOutputStream, "newX=", newX);
appendStringAndDec(debugOutputStream, ",newY=", newY);
appendStringAndDec(debugOutputStream, ",newAngle=", newAngle);
float fX = (float) newX;
float fY = (float) newY;
float fAngle = PI_DIVIDE_1800 * (float) newAngle;
appendStringAndDecf(debugOutputStream, "fX=", fX);
appendStringAndDecf(debugOutputStream, ",fY=", fY);
appendStringAndDecf(debugOutputStream, ",fAngle=", fAngle);
setPosition(fX, fY, fAngle);
append(outputStream, COMMAND_SET_ABSOLUTE_POSITION);
}
}
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;
}
int32_t VL53L0X_write_multi(uint8_t deviceAddress, uint8_t index, uint8_t *pdata, int32_t count) {
I2cBusConnection* i2cBusConnection = getI2cBusConnectionBySlaveAddress(deviceAddress);
I2cBus* i2cBus = i2cBusConnection->i2cBus;
portableMasterWaitSendI2C(i2cBusConnection);
// Wait till Start sequence is completed
WaitI2C(i2cBus);
portableMasterStartI2C(i2cBusConnection);
WaitI2C(i2cBus);
// I2C PICs adress use 8 bits and not 7 bits
portableMasterWriteI2C(i2cBusConnection, deviceAddress);
WaitI2C(i2cBus);
portableMasterWriteI2C(i2cBusConnection, index);
WaitI2C(i2cBus);
#ifdef VL53L0X_DEBUG
OutputStream* debugOutputStream = getDebugOutputStreamLogger();
appendString(debugOutputStream, "\tWriting ");
appendDec(debugOutputStream, count);
appendString(debugOutputStream, " to addr 0x");
appendHex2(debugOutputStream, deviceAddress);
appendString(debugOutputStream, ": ");
#endif
while(count--) {
portableMasterWriteI2C(i2cBusConnection, pdata[0]);
WaitI2C(i2cBus);
#ifdef VL53L0X_DEBUG
appendString(debugOutputStream, "0x ");
appendHex2(debugOutputStream, pdata[0]);
appendString(debugOutputStream, ", ");
#endif
pdata++;
}
#ifdef VL53L0X_DEBUG
println(debugOutputStream);
#endif
portableMasterStopI2C(i2cBusConnection);
WaitI2C(i2cBus);
return VL53L0X_ERROR_NONE;
}
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 mainBoardCommonInitCommonDrivers(void) {
// -> Eeproms
appendString(getDebugOutputStreamLogger(), "EEPROM ...");
eepromI2cBusConnection = addI2cBusConnection(i2cBus, ST24C512_ADDRESS_0, true);
init24C512Eeprom(&eeprom, eepromI2cBusConnection);
appendStringLN(getDebugOutputStreamLogger(), "OK");
// -> Clock
appendString(getDebugOutputStreamLogger(), "CLOCK ...");
clockI2cBusConnection = addI2cBusConnection(i2cBus, PCF8563_WRITE_ADDRESS, true);
initClockPCF8563(&clock, clockI2cBusConnection);
appendStringLN(getDebugOutputStreamLogger(), "OK");
// -> Temperature
appendString(getDebugOutputStreamLogger(), "TEMPERATURE ...");
temperatureI2cBusConnection = addI2cBusConnection(i2cBus, LM75A_ADDRESS, true);
initTemperatureLM75A(&temperature, temperatureI2cBusConnection);
appendStringLN(getDebugOutputStreamLogger(), "OK");
/*
//--> Current
appendString(getDebugOutputStreamLogger(), "CURRENT...");
currentI2CBusConnection = addI2cBusConnection(i2cBus, INA3221_ADDRESS_1, true);
initCurrentINA3221(¤t, currentI2CBusConnection);
appendStringLN(getDebugOutputStreamLogger(), "OK");
*/
// -> Servo
initServoList(&servoList, (Servo(*)[]) &servoListArray, MAIN_BOARD_SERVO_LIST_LENGTH);
addServos_1_2_5(&servoList, "PWM 1", "PWM 2", "PWM 5");
// -> Accelerometer
/*
appendString(getDebugOutputStreamLogger(), "ACCELEROMETER ...");
I2cBusConnection* adxl345BusConnection = addI2cBusConnection(i2cBus, ADXL345_ALT_ADDRESS, true);
initADXL345AsAccelerometer(&accelerometer, &accelerometerData, adxl345BusConnection);
adxl345_setupInterruptOnSingleTapOnInt1(&accelerometer,
8000,
2,
TAP_AXES_ALL_ENABLE,
ADXL345_RATE_400HZ,
ADXL345_RANGE_16G);
appendStringLN(getDebugOutputStreamLogger(), "OK");
*/
// Start interruptions
startTimerList(true);
}
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 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);
}
void debugTrajectoryVariables(char* valueName1, float value1, char* valueName2, float value2) {
OutputStream* outputStream = getDebugOutputStreamLogger();
appendStringAndDecf(outputStream, valueName1, value1);
appendStringAndDecf(outputStream, valueName2, value2);
appendCRLF(outputStream);
}
void runMotorBoardPC(bool singleMode) {
singleModeActivated = singleMode;
setBoardName(MOTOR_BOARD_PC_NAME);
moveConsole(HALF_SCREEN_WIDTH, 0, HALF_SCREEN_WIDTH, CONSOLE_HEIGHT);
// We use http://patorjk.com/software/taag/#p=testall&v=2&f=Acrobatic&t=MOTOR%20BOARD%20PC
// with Font : Jerusalem
printf(" __ __ ___ _____ ___ ____ ____ ___ _ ____ ____ ____ ____\r\n");
printf("| \\/ |/ _ |_ _/ _ \\| _ \\ | __ ) / _ \\ / \\ | _ \\| _ \\ | _ \\ / ___|\r\n");
printf("| |\\/| | | | || || | | | |_) | | _ \\| | | |/ _ \\ | |_) | | | | | |_) | | \r\n");
printf("| | | | |_| || || |_| | _ < | |_) | |_| / ___ \\| _ <| |_| | | __/| |___ \r\n");
printf("|_| |_|\\___/ |_| \\___/|_| \\_\\ |____/ \\___/_/ \\_|_| \\_|____/ |_| \\____|\r\n");
printf("\r\n");
initLogs(DEBUG, (LogHandler(*)[]) &logHandlerListArray, MOTOR_BOARD_PC_LOG_HANDLER_LIST_LENGTH, LOG_HANDLER_CATEGORY_ALL_MASK);
initConsoleInputStream(&consoleInputStream);
initConsoleOutputStream(&consoleOutputStream);
addConsoleLogHandler(DEBUG, LOG_HANDLER_CATEGORY_ALL_MASK);
appendStringCRLF(getDebugOutputStreamLogger(), getBoardName());
initTimerList((Timer(*)[]) &timerListArray, MOTOR_BOARD_PC_TIMER_LENGTH);
initBuffer(&consoleInputBuffer, (char(*)[]) &consoleInputBufferArray, MOTOR_BOARD_PC_CONSOLE_INPUT_BUFFER_LENGTH, "inputConsoleBuffer", "IN");
initBuffer(&consoleOutputBuffer, (char(*)[]) &consoleOutputBufferArray, MOTOR_BOARD_PC_CONSOLE_OUTPUT_BUFFER_LENGTH, "outputConsoleBuffer", "IN");
// Dispatchers
initDriverDataDispatcherList((DriverDataDispatcher(*)[]) &driverDataDispatcherListArray, MOTOR_BOARD_PC_DATA_DISPATCHER_LIST_LENGTH);
addLocalDriverDataDispatcher();
if (!singleModeActivated) {
initI2cBus(&motorI2cBus, I2C_BUS_TYPE_SLAVE, I2C_BUS_PORT_1);
initI2cBusConnection(&motorI2cBusConnection, &motorI2cBus, MOTOR_BOARD_PC_I2C_ADDRESS);
openSlaveI2cStreamLink(&i2cSlaveStreamLink,
&i2cSlaveInputBuffer,
(char(*)[]) &i2cSlaveInputBufferArray,
MOTOR_BOARD_PC_IN_BUFFER_LENGTH,
&i2cSlaveOutputBuffer,
(char(*)[]) &i2cSlaveOutputBufferArray,
MOTOR_BOARD_PC_OUT_BUFFER_LENGTH,
&motorI2cBusConnection
);
// I2C Debug
initI2CDebugBuffers(&i2cSlaveDebugInputBuffer,
(char(*)[]) &i2cSlaveDebugInputBufferArray,
MOTOR_BOARD_PC_I2C_DEBUG_SLAVE_IN_BUFFER_LENGTH,
&i2cSlaveDebugOutputBuffer,
(char(*)[]) &i2cSlaveDebugOutputBufferArray,
MOTOR_BOARD_PC_I2C_DEBUG_SLAVE_OUT_BUFFER_LENGTH);
}
// Eeprom
initEepromPc(&eeprom, "TODO");
// Battery
initBattery(&battery, getBatteryVoltage);
// Clock
initSoftClock(&clock);
// Devices
initDeviceList((Device(*)[]) &deviceListArray, MOTOR_BOARD_PC_DEVICE_LIST_LENGTH);
addLocalDevice(getTestDeviceInterface(), getTestDeviceDescriptor());
addLocalDevice(getI2cSlaveDebugDeviceInterface(), getI2cSlaveDebugDeviceDescriptor(&motorI2cBusConnection));
addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
addLocalDevice(getMotorDeviceInterface(), getMotorDeviceDescriptor());
addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
addLocalDevice(getBatteryDeviceInterface(), getBatteryDeviceDescriptor(&battery));
addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));
addLocalDevice(getRobotKinematicsDeviceInterface(), getRobotKinematicsDeviceDescriptor(&eeprom));
addLocalDevice(getPIDDeviceInterface(), getPIDDeviceDescriptor(&eeprom, true));
addLocalDevice(getMotorDeviceInterface(), getMotorDeviceDescriptor());
addLocalDevice(getCodersDeviceInterface(), getCodersDeviceDescriptor());
addLocalDevice(getTrajectoryDeviceInterface(), getTrajectoryDeviceDescriptor());
addLocalDevice(getMotionDeviceInterface(), getMotionDeviceDescriptor(&eeprom, true));
addLocalDevice(getMotionSimulationDeviceInterface(), getMotionSimulationDeviceDescriptor());
initDevices();
startTimerList();
setDebugI2cEnabled(false);
while (1) {
motorBoardWaitForInstruction();
}
}
void mainBoardCommonTofInitDrivers(I2cBus* i2cBus, I2cBus* i2cBus2, unsigned int tofSensorCount) {
// IO Expander List
appendString(getDebugOutputStreamLogger(), "PCFs ...");
initIOExpanderList(&ioExpanderList, (IOExpander(*)[]) &ioExpanderArray, MAIN_BOARD_IO_EXPANDER_LIST_LENGTH);
// First Expander (Beacon + Fork)
tofIoExpander = getIOExpanderByIndex(&ioExpanderList, 0);
tofIoExpanderBusConnection = addI2cBusConnection(i2cBus, PCF8574_ADDRESS_0, true);
initIOExpanderPCF8574(tofIoExpander, tofIoExpanderBusConnection);
// if (tofSensorCount > 8) {
// Second Expander (Lateral Tofs)
lateralTofIoExpander = getIOExpanderByIndex(&ioExpanderList, 1);
lateralTofIoExpanderBusConnection = addI2cBusConnection(i2cBus2, PCF8574_ADDRESS_0, true);
initIOExpanderPCF8574(lateralTofIoExpander, lateralTofIoExpanderBusConnection);
// }
// End of IOExpanderList
appendStringLN(getDebugOutputStreamLogger(), "OK");
// TOF
if (tofSensorCount > 0) {
tofSensorArray[0].enabled = true;
}
if (tofSensorCount > 1) {
tofSensorArray[1].enabled = true;
}
if (tofSensorCount > 2) {
tofSensorArray[2].enabled = true;
}
if (tofSensorCount > 3) {
tofSensorArray[3].enabled = true;
}
if (tofSensorCount > 4) {
tofSensorArray[4].enabled = true;
}
if (tofSensorCount > 5) {
tofSensorArray[5].enabled = true;
}
if (tofSensorCount > 6) {
tofSensorArray[6].enabled = true;
}
if (tofSensorCount > 7) {
tofSensorArray[7].enabled = true;
}
if (tofSensorCount > 8) {
tofSensorArray[8].enabled = true;
}
if (tofSensorCount > 9) {
tofSensorArray[9].enabled = true;
}
if (tofSensorCount > 10) {
tofSensorArray[10].enabled = true;
}
if (tofSensorCount > 11) {
tofSensorArray[11].enabled = true;
}
appendStringLN(getDebugOutputStreamLogger(), "TOF ...");
initTofSensorListVL53L0X(&tofSensorList,
(TofSensor(*)[]) &tofSensorArray,
(TofSensorVL53L0X(*)[]) &tofSensorVL53L0XArray,
// Size
tofSensorCount,
// Bus
i2cBus,
// Other Bus if we use more than 8 tofs
i2cBus2,
// IO Expander, if null, we will not be able to
// Manage several tof
tofIoExpander,
// Second IO Expander
lateralTofIoExpander,
// debug
true,
// enabledAllSensors
false,
// changeAddressAllSensors
true
);
appendStringLN(getDebugOutputStreamLogger(), "OK");
/*
// -> Color : IMPORTANT !!! : Initialize COLOR after TOF because they share the same address
// And not the same bus, but it uses "getI2cBusConnectionBySlaveAddress" ... which does not distinguish the bus ...
appendStringLN(getDebugOutputStreamLogger(), "COLOR ...");
colorBusConnection = addI2cBusConnection(i2cBus4, TCS34725_ADDRESS, true);
initTcs34725Struct(&tcs34725, colorBusConnection);
initColorSensorTcs34725(&colorSensor, &colorValue, &colorSensorFindColorType2018, &tcs34725);
appendStringLN(getDebugOutputStreamLogger(), "OK");
*/
}
void runMainBoardPC(bool connectToRobotManagerMode) {
connectToRobotManager = connectToRobotManagerMode;
setBoardName(MAIN_BOARD_PC_NAME);
moveConsole(0, 0, HALF_SCREEN_WIDTH, CONSOLE_HEIGHT);
// We use http://patorjk.com/software/taag/#p=testall&v=2&f=Acrobatic&t=MOTOR%20BOARD%20PC
// with Font : Jerusalem
printf(" __ __ _ ___ _ _ ____ ___ _ ____ ____ ____ ____ \r\n");
printf("| \\/ | / \\ |_ _| \\ | | | __ ) / _ \\ / \\ | _ \\| _ \\ | _ \\ / ___|\r\n");
printf("| |\\/| | / _ \\ | || \\| | | _ \\| | | |/ _ \\ | |_) | | | | | |_) | | \r\n");
printf("| | | |/ ___ \\ | || |\\ | | |_) | |_| / ___ \\| _ <| |_| | | __/| |___ \r\n");
printf("|_| |_/_/ \\_|___|_| \\_| |____/ \\___/_/ \\_|_| \\_|____/ |_| \\____|\r\n");
printf("\r\n");
initLogs(DEBUG, (LogHandler(*)[]) &logHandlerListArray, MAIN_BOARD_PC_LOG_HANDLER_LIST_LENGTH, LOG_HANDLER_CATEGORY_ALL_MASK);
initConsoleInputStream(&consoleInputStream);
initConsoleOutputStream(&consoleOutputStream);
addConsoleLogHandler(DEBUG, LOG_HANDLER_CATEGORY_ALL_MASK);
appendStringCRLF(getDebugOutputStreamLogger(), getBoardName());
initTimerList((Timer(*)[]) &timerListArray, MAIN_BOARD_PC_TIMER_LENGTH);
initBuffer(&consoleInputBuffer, (char(*)[]) &consoleInputBufferArray, MAIN_BOARD_PC_CONSOLE_INPUT_BUFFER_LENGTH, "inputConsoleBuffer", "IN");
initBuffer(&consoleOutputBuffer, (char(*)[]) &consoleOutputBufferArray, MAIN_BOARD_PC_CONSOLE_OUTPUT_BUFFER_LENGTH, "outputConsoleBuffer", "IN");
// Dispatchers
initDriverDataDispatcherList((DriverDataDispatcher(*)[]) &driverDataDispatcherListArray, MAIN_BOARD_PC_DATA_DISPATCHER_LIST_LENGTH);
addLocalDriverDataDispatcher();
initI2cBus(&motorBoardI2cBus, I2C_BUS_TYPE_MASTER, I2C_BUS_PORT_1);
initI2cBusConnection(&motorBoardI2cBusConnection, &motorBoardI2cBus, MOTOR_BOARD_PC_I2C_ADDRESS);
addI2CDriverDataDispatcher("MOTOR_BOARD_DISPATCHER",
&motorBoardInputBuffer,
(char(*)[]) &motorBoardInputBufferArray,
MAIN_BOARD_PC_DATA_MOTOR_BOARD_DISPATCHER_BUFFER_LENGTH,
&motorBoardOutputStream,
&motorBoardInputStream,
&motorBoardI2cBusConnection
);
if (connectToRobotManager) {
// Open the serial Link between RobotManager (C# Project) and the MainBoardPc
openSerialLink(&robotManagerSerialStreamLink,
&robotManagerInputBuffer,
(char(*)[]) &robotManagerInputBufferArray,
ROBOT_MANAGER_INPUT_BUFFER_LENGTH,
&robotManagerOutputBuffer,
(char(*)[]) &robotManagerOutputBufferArray,
ROBOT_MANAGER_OUTPUT_BUFFER_LENGTH,
&robotManagerOutputStream,
SERIAL_PORT_ROBOT_MANAGER,
0);
}
// CONFIG
initRobotConfigPc(&robotConfig);
// EEPROM
initEepromPc(&eeprom, "TODO");
initEepromFile(&eeprom);
// Clock
initPcClock(&clock);
// Temperature
initTemperaturePc(&temperature);
// I2C Debug
initI2CDebugBuffers(&i2cMasterDebugInputBuffer,
(char(*)[]) &i2cMasterDebugInputBufferArray,
MAIN_BOARD_PC_I2C_DEBUG_MASTER_IN_BUFFER_LENGTH,
&i2cMasterDebugOutputBuffer,
(char(*)[]) &i2cMasterDebugOutputBufferArray,
MAIN_BOARD_PC_I2C_DEBUG_MASTER_OUT_BUFFER_LENGTH);
// Init the drivers
initDrivers(&driverRequestBuffer, (char(*)[]) &driverRequestBufferArray, MAIN_BOARD_PC_REQUEST_DRIVER_BUFFER_LENGTH,
&driverResponseBuffer, (char(*)[]) &driverResponseBufferArray, MAIN_BOARD_PC_RESPONSE_DRIVER_BUFFER_LENGTH);
// Get test driver for debug purpose
addDriver(testDriverGetDescriptor(), TRANSMIT_LOCAL);
// Devices
initDeviceList((Device(*)[]) &deviceListArray, MAIN_BOARD_PC_DEVICE_LIST_LENGTH);
// addLocalDevice(getTestDeviceInterface(), getTestDeviceDescriptor());
testDevice = addI2cRemoteDevice(getTestDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
testDevice->deviceHandleNotification = mainBoardDeviceHandleNotification;
// LOCAL BOARD
addLocalDevice(getADCDeviceInterface(), getADCDeviceDescriptor());
addLocalDevice(getStrategyDeviceInterface(), getStrategyDeviceDescriptor());
addLocalDevice(getSystemDeviceInterface(), getSystemDeviceDescriptor());
addLocalDevice(getRobotConfigDeviceInterface(), getRobotConfigDeviceDescriptor(&robotConfig));
addLocalDevice(getI2cMasterDebugDeviceInterface(), getI2cMasterDebugDeviceDescriptor());
addLocalDevice(getDataDispatcherDeviceInterface(), getDataDispatcherDeviceDescriptor());
addLocalDevice(getServoDeviceInterface(), getServoDeviceDescriptor());
addLocalDevice(getTimerDeviceInterface(), getTimerDeviceDescriptor());
addLocalDevice(getClockDeviceInterface(), getClockDeviceDescriptor(&clock));
// addLocalDevice(getFileDeviceInterface(), getFileDeviceDescriptor());
addLocalDevice(getEepromDeviceInterface(), getEepromDeviceDescriptor(&eeprom));
addLocalDevice(getLogDeviceInterface(), getLogDeviceDescriptor());
addLocalDevice(getLCDDeviceInterface(), getLCDDeviceDescriptor());
addLocalDevice(getTemperatureSensorDeviceInterface(), getTemperatureSensorDeviceDescriptor(&temperature));
addLocalDevice(getNavigationDeviceInterface(), getNavigationDeviceDescriptor());
initStartMatch(&startMatch, isMatchStartedPc, mainBoardPcWaitForInstruction, &eeprom);
addLocalDevice(getStartMatchDeviceInterface(), getStartMatchDeviceDescriptor(&startMatch));
// MOTOR BOARD
addI2cRemoteDevice(getBatteryDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
// addI2cRemoteDevice(getEepromDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getPIDDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getMotorDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getCodersDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getTrajectoryDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getMotionDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getRobotKinematicsDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
addI2cRemoteDevice(getMotionSimulationDeviceInterface(), MOTOR_BOARD_PC_I2C_ADDRESS);
initDevices();
delaymSec(100);
setDebugI2cEnabled(false);
// Ping
if (!pingDriverDataDispatcherList()) {
printf("PING PROBLEM !");
}
// Set Clock for Motor Board !
// Read Clock
ClockData* clockData = clock.readClock(&clock);
// TODO : Change Dispatcher Index ...
writeDateRemoteClockData(clockData, 0x01);
writeHourRemoteClockData(clockData, 0x01);
// testDriverIntensive(100);
startTimerList();
while (1) {
mainBoardPcWaitForInstruction(&startMatch);
}
// TODO : ShowEnd
}
