int appendDecf(OutputStream* stream, float value) {
int result = 0;
unsigned long decimalValue;
if (value < 0) {
decimalValue = (value - (long) value) * -1000;
append(stream, '-');
result++;
result += appendDec(stream, -(long) value);
} else {
decimalValue = (value - (long) value) * 1000;
result += appendDec(stream, (long) value);
}
append(stream, DECIMAL_SEPARATOR);
result++;
if (decimalValue < 100) {
append(stream, '0');
if (decimalValue < 10) {
append(stream, '0');
}
if (decimalValue < 1) {
append(stream, '0');
}
}
result += appendDec(stream, decimalValue);
return result;
}
/**
* @private
*/
void printArgumentList(OutputStream* outputStream, DeviceInterface* deviceInterface, char header, char headerLength, char inputMode) {
if (headerLength != DEVICE_HEADER_NOT_HANDLED) {
DeviceArgumentList* deviceArgumentList = getDeviceArgumentList();
const char* deviceName = deviceInterface->deviceGetName();
int argumentCount = deviceArgumentList->size;
append(outputStream, inputMode);
append(outputStream, ARGUMENT_LIST_SEPARATOR);
appendString(outputStream, deviceName);
append(outputStream, ARGUMENT_LIST_SEPARATOR);
append(outputStream, header);
int argumentIndex;
append(outputStream, ARGUMENT_LIST_SEPARATOR);
appendString(outputStream, deviceArgumentList->functionName);
for (argumentIndex = 0; argumentIndex < argumentCount; argumentIndex++) {
DeviceArgument deviceArgument = deviceArgumentList->args[argumentIndex];
append(outputStream, ARGUMENT_LIST_SEPARATOR);
char* argumentName = deviceArgument.name;
appendString(outputStream, argumentName);
append(outputStream, ARGUMENT_SEPARATOR);
char type = deviceArgument.type;
char argumentLength = (type >> 1) & 0xFE;
appendDec(outputStream, argumentLength);
append(outputStream, ARGUMENT_SEPARATOR);
// last bit used for signed / unsigned
if ((type & 1) != 0) {
append(outputStream, 's');
} else {
append(outputStream, 'u');
}
}
println(outputStream);
}
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_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 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);
}
int appendDecf(OutputStream* stream, float value) {
int result = 0;
float decimalValue;
long decimalValueLong;
if (value < 0) {
decimalValue = ((value - (long) value) * -1000);
append(stream, '-');
result++;
result += appendDec(stream, -(long) value);
} else {
decimalValue = ((value - (long) value) * 1000);
result += appendDec(stream, (long) value);
}
decimalValueLong = (long) decimalValue;
if (decimalValue - decimalValueLong > 0.5f) {
decimalValueLong++;
}
append(stream, DECIMAL_SEPARATOR);
result++;
if (decimalValueLong < 100) {
append(stream, '0');
if (decimalValueLong < 10) {
append(stream, '0');
}
if (decimalValueLong < 1 && decimalValueLong > 0) {
append(stream, '0');
}
}
result += appendDec(stream, decimalValueLong);
return result;
}
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;
}
void appendStringAndDec(OutputStream* stream, const char* s, long value) {
appendString(stream, s);
appendDec(stream, value);
}
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
}
}
/**
* @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;
}
