Adam Wilcock
test
Dependencies: MODDMA MODSERIAL mbed
main.cpp
- Committer:
- JonathanAshworth
- Date:
- 2015-04-09
- Revision:
- 7:49aba2a2e847
- Parent:
- 6:8ffc6d3a7c1d
File content as of revision 7:49aba2a2e847:
/************************************************************************************
PROJECT IRIS
Multidisciplinary Group Project
ENG701S2 (Year 4 - MENG) at the University of Portsmouth
Filename: main.cpp
Description: Root source file for the project. The protocol's
timeout functionality is achieved using timer3
therefore the use of wait() or similar must be
avoided else the system will not adhere to the
timeout timings (wait and timer both use timer3).
Original Created: 05/04/2015
Original Author: Jonathan Ashworth
--------------------------Change Log--------------------------------
Date | Author | Details
06/04/05 Jonathan Ashworth
************************************************************************************/
#include "main.h"
//************************ For Development Only *************************************
#define DEBUG 0 // When set GGPSSerial becomes a debug output
#define DUMMY 1 // When set botData is filled with random data
#define PC_SERIAL_INIT_DISABLE 0
DigitalOut led1(LED1); // Timeout led - idle
DigitalOut led2(LED2); // rx tacking too long timeout
DigitalOut led3(LED3); // bad write timeout
DigitalOut led4(LED4); // Invalid value requested to servo/motor - construct tx - data too soon
//***********************************************************************************
//************************** Serial DMA Setup ***************************************
MODSERIAL PCSerial( PC_TX_PIN, // MBED's transmit pin to PC
PC_RX_PIN, // MBED's receive pin from PC
PC_TX_BUFFER_SIZE, // Transmiter buffer size
PC_RX_BUFFER_SIZE); // Receiver buffer size
MODSERIAL SSC32Serial(SSC32_TX_PIN, // MBED's transmit pin to SSC32
SSC32_RX_PIN, // MBED's receive pin from SSC32
SSC32_TX_BUFFER_SIZE, // Transmiter buffer size
SSC32_RX_BUFFER_SIZE); // Receiver buffer size
MODSERIAL GPSSerial(GPS_TX_PIN, // MBED's transmit pin to GPS
GPS_RX_PIN, // MBED's receive pin from GPS
GPS_TX_BUFFER_SIZE, // Transmiter buffer size
GPS_RX_BUFFER_SIZE); // Receiver buffer size
MODDMA dma; // Define DMA hardware for MODSERIAL to use
// Storage of whole PC requests (termination detected):
char PCRxData[PC_RX_BUFFER_SIZE] = {0};
// Storage of whole PC responses:
char PCTxData[PC_TX_BUFFER_SIZE] = {0};
// Temporary storage of incomming PC bytes as they arrive:
char incommingPCRxData[PC_RX_BUFFER_SIZE] = {0};
// Tracking the length of the incomming PC data (number of bytes received):
char incommingPCRxDataCount = 0;
// Storage for requested servo position (set by SSC32's rx ISR):
char servoPositionReturn = 0;
// PC serial comms timout timer:
Timer PCTimeoutTimer;
//***********************************************************************************
//************************** ESC Drive Setup ****************************************
PwmOut driveNS(DRIVE_NS_PIN); // Nearside ESC
PwmOut driveOS(DRIVE_OS_PIN); // Offside ESC
//***********************************************************************************
//HCSR04 usound(p17, p18); //trig echo
Flags flags; // Event/status tracking flag structure
botStateHandler botData; // Storage class for all peripherals
// limit pitch 1300 lower limit 2200 max
int main(void) {
initialise();
while(!flags.PCBegin); // Wait for the PC to send the begin phrase
while(1) {
// If the PC serial rx line is idle for too long then timeout:
if (PCTimeoutTimer.read_ms() > PC_TIMEOUT_PERIOD) {
flags.PCTimeout = 1;
led1 = 1;
if(DEBUG) {
GPSSerial.printf("Idle timeout\r\n");
}
}
// If a timeout has been set anywhere then process the timeout function:
if (flags.PCTimeout) {
PCTimeout();
}
if (flags.rxNewData) { // If a new data packet has been received:
parseRequestData(); // Complete the requests
flags.rxNewData = 0; // Data is now old so reset flag
}
if (flags.txNewData) {
sendPCSerialData(); // Return the requested data
flags.txNewData = 0; // Data is now old so reset flag
}
}
}
void initialise(void) {
// Setup serial links and their ISRs:
PCSerial.baud(PC_BAUDRATE);
PCSerial.attach(&dmaPCSerialRx, MODSERIAL::RxIrq); // rx ISR
SSC32Serial.baud(SSC32_BAUDRATE);
SSC32Serial.attach(&dmaSSC32SerialRx, MODSERIAL::RxIrq); // rx ISR
GPSSerial.baud(GPS_BAUDRATE);
GPSSerial.attach(&dmaGPSSerialRx, MODSERIAL::RxIrq); // rx ISR
// Stop and reset the timeout timer:
PCTimeoutTimer.stop();
PCTimeoutTimer.reset();
// Define the PWM period for the drive ESCs:
driveNS.period_ms(20); // Nearside
driveOS.period_ms(20); // Offside
// Set the channels for each servo in field 0 of the bot's DB:
float armBaseChannel = SERVO_ARM_BASE_CHAN;
float armShoulderChannel = SERVO_ARM_SHOULDER_CHAN;
float armElbowChannel = SERVO_ARM_ELBOW_CHAN;
float armWristChannel = SERVO_ARM_WRIST_CHAN;
float armManChannel = SERVO_ARM_MANIPULATOR_CHAN;
float visionPitchChannel = SERVO_VISION_PITCH_CHAN;
float visionYawChannel = SERVO_VISION_YAW_CHAN;
botData.setVal(SID_ARM_BASE, 0, &armBaseChannel);
botData.setVal(SID_ARM_SHOULDER, 0, &armShoulderChannel);
botData.setVal(SID_ARM_ELBOW, 0, &armElbowChannel);
botData.setVal(SID_ARM_WRIST, 0, &armWristChannel);
botData.setVal(SID_ARM_MANIPULATOR, 0, &armManChannel);
botData.setVal(SID_VISION_PITCH, 0, &visionPitchChannel);
botData.setVal(SID_VISION_YAW, 0, &visionYawChannel);
shutdown(); // Ensure the bot is in a safe shutdown mode/position
if (DEBUG) {
GPSSerial.printf("Hello\r\n"); // Aid setup of a terminal to the MBED
}
flags.PCBegin = (bool)PC_SERIAL_INIT_DISABLE; // TODO: remove
}
/*
void updateSensorsData(void) {
int pirAlarm = mbed_PIR();
float temperature = mbed_tempsensor();
float humidity = mbed_humiditysensor();
float compassX = getcompassX();
float compassY = getcompassY();
float compassZ = getcompassZ();
float gyroX = getgyroX();
float gyroY = getgyroY();
float gyroZ = getgyroZ();
int accelX = getaccelX();
int accelY = getaccelY();
int accelZ = getaccelZ();
}
*/
/*
void getServoData(Servo *servo) {
SSC32Serial.printf("QP %d\r", servo->channel);
int temp = 1;
//read reply
//convert char to int
servo->position = temp;
}
*/
long readUltrasound(void) {/*
//Set Mux to sensor->muxChannel
//Pulse trigger pin
//Disable interrupts
//Read PWM on echo pin
//Enable interrupts
//sensor->distanceMillimeters = result;*/
long distance = 0;
//distance = usound.distance(1);
return distance;
}
void constructTxData(char *SIDs, char numberOfSensors) { // TODO: Tidy up
int i, j, lengthOfCurrentSentence;
char sendSentence[PC_LONGESTRESPONSESENTENCE] = {0};
char fieldCount;
int acumulatedLengthOfPreviousSentences = 0;
float accelX, accelY, accelZ;
char accelXArr[BYTELENGTH_OF_A_FLOAT], accelYArr[BYTELENGTH_OF_A_FLOAT], accelZArr[BYTELENGTH_OF_A_FLOAT];
float rotX, rotY, rotZ;
char rotXArr[BYTELENGTH_OF_A_FLOAT], rotYArr[BYTELENGTH_OF_A_FLOAT], rotZArr[BYTELENGTH_OF_A_FLOAT];
float magX, magY, magZ;
char magXArr[BYTELENGTH_OF_A_FLOAT], magYArr[BYTELENGTH_OF_A_FLOAT], magZArr[BYTELENGTH_OF_A_FLOAT];
float lat, lon, spd;
char gpsLatArr[BYTELENGTH_OF_A_FLOAT], gpsLonArr[BYTELENGTH_OF_A_FLOAT], gpsSpdArr[BYTELENGTH_OF_A_FLOAT];
float temperature, humidity, voltage, current, pirAlarm;
char temperatureArr[BYTELENGTH_OF_A_FLOAT], humidityArr[BYTELENGTH_OF_A_FLOAT], voltageArr[BYTELENGTH_OF_A_FLOAT], currentArr[BYTELENGTH_OF_A_FLOAT], pirAlarmArr[BYTELENGTH_OF_A_FLOAT];
float position, speed, time;
char positionArr[BYTELENGTH_OF_A_FLOAT], speedArr[BYTELENGTH_OF_A_FLOAT], timeArr[BYTELENGTH_OF_A_FLOAT];
float demand, distance;
char demandArr[BYTELENGTH_OF_A_FLOAT], distanceArr[BYTELENGTH_OF_A_FLOAT];
float channel;
unsigned int termBigEnd;
memset(PCTxData, 0, PC_TX_BUFFER_SIZE); //reset tx buffer
for (i=0; i<numberOfSensors; i++) {
switch (SIDs[i]) {
case SID_9DOF_ACC: // 9DOF acc
fieldCount = 3;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_X, &accelX);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Y, &accelY);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Z, &accelZ);
if (DUMMY) {
if(accelX == 255.0f) {
accelX = 0.0f;
accelY = 0.0f;
accelZ = 0.0f;
}
else {
accelX++;
accelY++;
accelZ++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_X, &accelX);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Y, &accelY);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Z, &accelZ);
}
memcpy(accelXArr, &accelX, BYTELENGTH_OF_A_FLOAT);
memcpy(accelYArr, &accelY, BYTELENGTH_OF_A_FLOAT);
memcpy(accelZArr, &accelZ, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF accel ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = accelXArr[j];
sendSentence[PC_FLOAT1_LOCATION+j] = accelYArr[j];
sendSentence[PC_FLOAT2_LOCATION+j] = accelZArr[j];
}
lengthOfCurrentSentence = PC_9DOFACC_RESPONSE_LENGTH;
break;
case SID_9DOF_GYRO: // 9DOF gyro
fieldCount = 3;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_X, &rotX);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Y, &rotY);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Z, &rotZ);
if (DUMMY) {
if(rotX == 255.0f) {
rotX = 0.0f;
rotY = 0.0f;
rotZ = 0.0f;
}
else {
rotX++;
rotY++;
rotZ++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_X, &rotX);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Y, &rotY);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Z, &rotZ);
}
memcpy(rotXArr, &rotX, BYTELENGTH_OF_A_FLOAT);
memcpy(rotYArr, &rotY, BYTELENGTH_OF_A_FLOAT);
memcpy(rotZArr, &rotZ, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF gyro ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = rotXArr[j];
sendSentence[PC_FLOAT1_LOCATION+j] = rotYArr[j];
sendSentence[PC_FLOAT2_LOCATION+j] = rotZArr[j];
}
lengthOfCurrentSentence = PC_9DOFGYR_RESPONSE_LENGTH;
break;
case SID_9DOF_MAG: // 9DOF mag
fieldCount = 3;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_X, &magX);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Y, &magY);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Z, &magZ);
if (DUMMY) {
if(magX == 255.0f) {
magX = 0.0f;
magY = 0.0f;
magZ = 0.0f;
}
else {
magX++;
magY++;
magZ++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_X, &magX);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Y, &magY);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Z, &magZ);
}
memcpy(magXArr, &magX, BYTELENGTH_OF_A_FLOAT);
memcpy(magYArr, &magY, BYTELENGTH_OF_A_FLOAT);
memcpy(magZArr, &magZ, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF mag ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = magXArr[j];
sendSentence[PC_FLOAT1_LOCATION+j] = magYArr[j];
sendSentence[PC_FLOAT2_LOCATION+j] = magZArr[j];
}
lengthOfCurrentSentence = PC_9DOFMAG_RESPONSE_LENGTH;
break;
case SID_GPS: // GPS
fieldCount = 3;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LAT, &lat);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LONG, &lon);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_SPEED, &spd);
if (DUMMY) {
if(lat == 255.0f) {
lat = 0.0f;
lon = 0.0f;
spd = 0.0f;
}
else {
lat++;
lon++;
spd++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LAT, &lat);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LONG, &lon);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_SPEED, &spd);
}
memcpy(gpsLatArr, &lat, BYTELENGTH_OF_A_FLOAT);
memcpy(gpsLonArr, &lon, BYTELENGTH_OF_A_FLOAT);
memcpy(gpsSpdArr, &spd, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //GPS ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = gpsLatArr[j];
sendSentence[PC_FLOAT1_LOCATION+j] = gpsLonArr[j];
sendSentence[PC_FLOAT2_LOCATION+j] = gpsSpdArr[j];
}
lengthOfCurrentSentence = PC_GPS_RESPONSE_LENGTH;
break;
case SID_TEMP: // Temperature
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_TEMPERATURE, &temperature);
if (DUMMY) {
if (temperature == 255.0f) {
temperature = 0.0f;
}
else {
temperature++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_TEMPERATURE, &temperature);
}
memcpy(temperatureArr, &temperature, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //Temperature ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = temperatureArr[j];
}
lengthOfCurrentSentence = PC_TEMPERATURE_RESPONSE_LENGTH;
break;
case SID_HUM: // Humidity
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_HUMIDITY, &humidity);
if (DUMMY) {
if (humidity == 255.0f) {
humidity = 0.0f;
}
else {
humidity++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_HUMIDITY, &humidity);
}
memcpy(humidityArr, &humidity, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //Humidity ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = humidityArr[j];
}
lengthOfCurrentSentence = PC_HUMIDITY_RESPONSE_LENGTH;
break;
case SID_BATT_VOL: // Battery Voltage
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_VOLTAGE, &voltage);
if (DUMMY) {
if (voltage == 255.0f) {
voltage = 0.0f;
}
else {
voltage++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_VOLTAGE, &voltage);
}
memcpy(voltageArr, &voltage, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //Voltage sensor ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = voltageArr[j];
}
lengthOfCurrentSentence = PC_BATTVOLTAGE_RESPONSE_LENGTH;
break;
case SID_BATT_CUR: // Battery Current
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_CURRENT, ¤t);
if (DUMMY) {
if (current == 255.0f) {
current = 0.0f;
}
else {
current++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_CURRENT, ¤t);
}
memcpy(currentArr, ¤t, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //Current sensor ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = currentArr[j];
}
lengthOfCurrentSentence = PC_BATTCURRENT_RESPONSE_LENGTH;
break;
case SID_PIR: // PIR
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_PIR, &pirAlarm);
if (DUMMY) {
if (pirAlarm == 255.0f) {
pirAlarm = 0.0f;
}
else {
pirAlarm++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_PIR, &pirAlarm);
}
memcpy(pirAlarmArr, &pirAlarm, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //PIR ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = pirAlarmArr[j];
}
lengthOfCurrentSentence = PC_PIR_RESPONSE_LENGTH;
break;
default: break;
}
if ((SIDs[i] >= SID_FIRSTSERVO_LOCATION)&&
(SIDs[i] <= SID_LASTSERVO_LOCATION)) { //Servos
fieldCount = 3;
channel = 0.0f;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_POSITION, &position);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed);
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_TIME, &time);
if (DUMMY) {
if(position == 255.0f) {
position = 0.0f;
speed = 0.0f;
time = 0.0f;
}
else {
time++;
time++;
time++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_POSITION, &position);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed);
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_TIME, &time);
}
//SSC32Serial.printf("QP #%d \r", (char)channel);
//while(servoPositionReturn == 0) {;}
//position = servoPositionReturn;
//servoPositionReturn = 0;
memcpy(positionArr, &position, BYTELENGTH_OF_A_FLOAT);
memcpy(speedArr, &speed, BYTELENGTH_OF_A_FLOAT);
memcpy(timeArr, &time, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //servo ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = positionArr[j];
sendSentence[PC_FLOAT1_LOCATION+j] = speedArr[j];
sendSentence[PC_FLOAT2_LOCATION+j] = timeArr[j];
}
lengthOfCurrentSentence = PC_SERVO_REQUEST_LENGTH;
}
if ((SIDs[i] >= SID_FIRSTMOTOR_LOCATION)&&
(SIDs[i] <= SID_LASTMOTOR_LOCATION)) { //Motors
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
if (DUMMY) {
if (demand == 255.0f) {
demand = 0.0f;
}
else {
demand++;
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
}
memcpy(demandArr, &demand, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //Motor ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = demandArr[j];
}
lengthOfCurrentSentence = PC_MOTOR_REQUEST_LENGTH;
}
if ((SIDs[i] >= SID_FIRSTUSOUND_LOCATION)&&
(SIDs[i] <= SID_LASTUSOUND_LOCATION)) { //Ultrasounds
fieldCount = 1;
botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_USOUND_DISTANCE, &distance);
if (DUMMY) {
/*if (distance == 255.0f) {
distance = 0.0f;
}
else {
distance++;
}*/
// distance = (float)usound.distance(1);
distance = -1.0f;
if (DEBUG) {
GPSSerial.printf("USOUND Distance (cm): %f\r\n", distance);
}
botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_USOUND_DISTANCE, &distance);
}
memcpy(distanceArr, &distance, BYTELENGTH_OF_A_FLOAT);
sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
sendSentence[PC_SID_LOCATION] = SIDs[i]; //Ultrasound ID
sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
sendSentence[PC_FLOAT0_LOCATION+j] = distanceArr[j];
}
lengthOfCurrentSentence = PC_USOUND_RESPONSE_LENGTH;
}
memcpy(&PCTxData[acumulatedLengthOfPreviousSentences], sendSentence, lengthOfCurrentSentence);
acumulatedLengthOfPreviousSentences = acumulatedLengthOfPreviousSentences + lengthOfCurrentSentence;
}
// Convert from little endian to big endian:
termBigEnd = PC_TERMINATION_PHRASE;
termBigEnd = (termBigEnd>>BITLENGTH_OF_A_BYTE*3) | // Send byte 4 to byte 0
((termBigEnd<<BITLENGTH_OF_A_BYTE) & 0x00ff0000) | // Send byte 2 to byte 3
((termBigEnd>>BITLENGTH_OF_A_BYTE) & 0x0000ff00) | // Send byte 3 to btye 2
(termBigEnd<<BITLENGTH_OF_A_BYTE*3); // Send byte 0 to byte 4
memcpy(&PCTxData[acumulatedLengthOfPreviousSentences], &termBigEnd, BYTELENGTH_OF_AN_INT);
flags.txNewData = 1;
}
void parseRequestData(void) {
char i,j;
int bufferOffset = 0; // Tacking how far into the rx data we are
int lengthOfCurrentSentence = 0;
char CID;
char SID;
char fieldCount;
char requestedSensorIDs[PC_REQUESTEDSID_MAXCOUNT] = {0};
// Servo vars:
float channel;
char posArr[BYTELENGTH_OF_A_FLOAT];
char spdArr[BYTELENGTH_OF_A_FLOAT];
char timArr[BYTELENGTH_OF_A_FLOAT];
float position;
float speed;
float time;
// Motor vars:
unsigned int pwm;
float demand;
char demArr[BYTELENGTH_OF_A_FLOAT];
//bool terminated;
/*
if(1) {
terminated = 0;
//GPSSerial.printf("Echo back...");
for (i=0; i<PC_RX_BUFFER_SIZE; i++) {
if ( (PCRxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) +
(PCRxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) +
(PCRxData[i-2]<<BITLENGTH_OF_A_BYTE) +
PCRxData[i-1] == PC_TERMINATION_PHRASE ) {
terminated = 1;
}
if (!terminated) {
GPSSerial.putc(PCRxData[i]);
}
}
//GPSSerial.printf("...Echo end\r\n");
}
*/
for (j=0; j<((char)PC_SENTENCE_COUNT); j++) {
// Read CID:
CID = PCRxData[bufferOffset+PC_CID_LOCATION];
// Read SID:
SID = PCRxData[bufferOffset+PC_SID_LOCATION];
// Read Field Count:
fieldCount = PCRxData[bufferOffset+PC_FIELDCOUNT_LOCATION];
if(DEBUG) {
GPSSerial.printf("Parsing data for SID: %d\r\n", SID);
GPSSerial.printf("Field count: %d\r\n", fieldCount);
}
if (CID != SID_PC)
break; // Stop as I'm only listening to the PC
if (SID == SID_PC)
break; // Stop as that message is for the PC
if (SID > SID_LASTMOTOR_LOCATION) {
flags.PCTimeout = 1;
led3 = 1;
break; // Stop as you cannot write to a sensor
}
if (SID == SID_MBED) { //Get ready to pump out sensor data:
if(DEBUG)
GPSSerial.printf("Sensor info requested\r\n");
for (i=0; i<fieldCount; i++) { // fieldCount = the number of requested sensors
// Retrieve the SID of 'ith' requested sensor:
requestedSensorIDs[i] = PCRxData[bufferOffset+PC_REQUESTEDSID_BYTE0_LOCATION+i];
if(DEBUG)
GPSSerial.printf("Sensor requested: %d\r\n",requestedSensorIDs[i]);
}
// Send the SIDs to construct a response:
constructTxData(requestedSensorIDs, fieldCount);
// Length of this sentence is:
lengthOfCurrentSentence = fieldCount +
PC_CID_LENGTH +
PC_SID_LENGTH +
PC_FIELDCOUNT_LENGTH;
}
if ((SID >= SID_FIRSTSERVO_LOCATION)&&
(SID <= SID_LASTSERVO_LOCATION)) { // We're writing to a servo
if(DEBUG)
GPSSerial.printf("Servo request\r\n");
// Retrieve byte values representing the position, speed and time floats:
for (i=0; i<BYTELENGTH_OF_A_FLOAT; i++) {
posArr[i] = PCRxData[(bufferOffset)+PC_FLOAT0_LOCATION+i]; // Float Byte 3, 4, 5 & 6
spdArr[i] = PCRxData[(bufferOffset)+PC_FLOAT1_LOCATION+i]; // Float Byte 7, 8, 9 & 10
timArr[i] = PCRxData[(bufferOffset)+PC_FLOAT2_LOCATION+i]; // Float Byte 11, 12, 13 & 14
}
// Cast byte arrays into float values:
position = *(float*)(posArr);
speed = *(float*)(spdArr);
time = *(float*)(timArr);
if (DEBUG) {
GPSSerial.printf("Position: %f\r\n", position);
GPSSerial.printf("Speed: %f\r\n", speed);
GPSSerial.printf("Time: %f\r\n", time);
}
// Find the channel associated with the desired servo:
botData.getVal(SID, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Validate the position request (ensure it is within safe limits):
if ((position >= (float)SERVO_POSITION_MIN)&&
(position <= (float)SERVO_POSITION_MAX)) {
// If no speed or time was specified:
if ((speed == 0.0f)&&(time == 0.0f)) {
SSC32Serial.printf("#%d P%d\r", (char)channel, (int)position);
if(DEBUG) {
GPSSerial.printf("writing to servo... ");
GPSSerial.printf("#%d P%d\r\n", (char)channel, (int)position);
}
}
// If a speed or time were specified:
else {
if (speed > 0.0f) { // Speed was specified:
SSC32Serial.printf("#%d P%d S%d\r", (char)channel, (int)position, (int)speed);
if(DEBUG) {
GPSSerial.printf("writing to servo... ");
GPSSerial.printf("#%d P%d S%d\r\n", (char)channel, (int)position, (int)speed);
}
}
if (time > 0.0f) { // Time was specified:
SSC32Serial.printf("#%d P%d T%d\r", (char)channel, (int)position, (int)time);
if(DEBUG) {
GPSSerial.printf("writing to servo... ");
GPSSerial.printf("#%d P%d T%d\r\n", (char)channel, (int)position, (int)time);
}
}
}
// Update botData with new values:
botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_POSITION, &position);
botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed);
botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_TIME, &time);
}
else {
//led4 = 1; // Indicate an invalid data warning (position outside limits)
}
lengthOfCurrentSentence = PC_SERVO_REQUEST_LENGTH;
}
if ((SID >= SID_FIRSTMOTOR_LOCATION)&&
(SID <= SID_LASTMOTOR_LOCATION)) { // We're writing to a drive motor ESC
//led4 = 0;
// Retrieve byte values representing the demand float:
demArr[0] = PCRxData[bufferOffset+PC_FLOAT0_LOCATION]; // Float Byte 0
demArr[1] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+1)]; // Float Byte 1
demArr[2] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+2)]; // Float Byte 2
demArr[3] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+3)]; // Float Byte 3
demand = *(float*)(demArr); // Cast byte array into a float value
if(DEBUG) {
GPSSerial.printf("Motor request...\r\n");
GPSSerial.printf("Demand: %f\r\n",demand);
}
if ((demand >= -1.0f)&&
(demand <= 1.0f)) { // Ensure demand value is valid
pwm = translateMotorDemandToPWM(demand); // Translate demand into a pwm value
if(DEBUG)
GPSSerial.printf("Pulsewidth (us): %d\r\n",pwm);
// If request is for the nearside drive motor ESC:
if (SID == (char)SID_DRIVE_NS) {
driveNS.pulsewidth_us(pwm); // Write to nearside drive motor ESC
// Update bot's DB with new demand value:
botData.setVal( SID, BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
if(DEBUG) {
GPSSerial.printf("Writing to NS motor (pwm value): ");
GPSSerial.printf("%d \r\n", pwm);
}
}
// If request is for the offside drive motor ESC:
if (SID == (char)SID_DRIVE_OS) {
driveOS.pulsewidth_us(pwm); // Wite to offside drive motor ESC
// Update bot's DB with new demand value:
botData.setVal(SID, BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
if(DEBUG) {
GPSSerial.printf("Writing to OS motor (pwm value): ");
GPSSerial.printf("%d \r\n", pwm);
}
}
}
else {
//led4 = 1; // Indicate an invalid data warning (demand outside limits)
}
lengthOfCurrentSentence = PC_MOTOR_REQUEST_LENGTH;
}
// Update how far into the rx data we are using the current sentence length:
bufferOffset = bufferOffset + lengthOfCurrentSentence;
}
}
unsigned int translateMotorDemandToPWM(float demand) {
/* Valid demand values: -1.0 upto 1.0 From protocol definition
Desired pwm values: 0 upto 1.0 From ESC operation requirements
The values resolve as follow:
Demand: -1.0 | 0 | 1.0
M/S Ratio: 0 | 1 | 2.0
PWM: 700 | 1300 | 2000
Meaning: MaxReverse | Neutral | MaxForward
*/
float pwm = 0;
pwm = demand+1;
pwm = (625.0f*pwm)+700.0f;
return (unsigned int)pwm;
}
void PCTimeout(void) {
// Try telling the PC comms have been lost:
//PCSerial.printf(PC_TIMEOUT_MESSAGE);
if(DEBUG) {
GPSSerial.printf(PC_TIMEOUT_MESSAGE);
}
//led1 = 1; // Indicate the MBED has timedout
shutdown(); // Ensure the bot is in a safe shutdown mode/position
// Wait for PC_TIMEOUT_MESSAGE to be sent before disabling the ISRs
while(!SSC32Serial.txBufferEmpty()) {;}
// Prevent any ISRs from firing:
// (important this comes after the while else the timeout message won't send):
__disable_irq(); // Should disable all ISRs
NVIC_DisableIRQ(UART0_IRQn); // Directly turn off USB (PC)'s DMA ISRs
while(1) {;} // Require a hard reset to resume
}
void shutdown(void) {
float channel; // Temporary storage for locating servo channels
// Set drive motor ESCs to neutral:
driveNS.pulsewidth_us(translateMotorDemandToPWM(DRIVE_DEMAND_NEUTRAL)); // Nearside
driveOS.pulsewidth_us(translateMotorDemandToPWM(DRIVE_DEMAND_NEUTRAL)); // Offside
SSC32Serial.printf(SSC32_STOP); // Stop previous servo command
// Find the channel for the arm's base servo:
botData.getVal(SID_ARM_BASE, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the base in a 'safe' position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_ARM_BASE,
(int)SSC32_STOPPOSITION_TIME);
// Find the channel for the arm's shoulder servo:
botData.getVal(SID_ARM_SHOULDER, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the shoulder in a 'safe' position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_ARM_SHOULDER,
(int)SSC32_STOPPOSITION_TIME);
// Find the channel for the arm's elbow servo:
botData.getVal(SID_ARM_ELBOW, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the elbow in a 'safe' position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_ARM_ELBOW,
(int)SSC32_STOPPOSITION_TIME);
// Find the channel for the arm's wrist servo:
botData.getVal(SID_ARM_WRIST, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the wrist in a 'safe' position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_ARM_WRIST,
(int)SSC32_STOPPOSITION_TIME);
// Find the channel for the arm's manipulator servo:
botData.getVal(SID_ARM_MANIPULATOR, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the manipulator in a 'safe' position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_ARM_MANIPULATOR,
(int)SSC32_STOPPOSITION_TIME);
// Find the channel for the visions's pitch servo:
botData.getVal(SID_VISION_PITCH, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the pitch in a neutral position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_VISION_PITCH,
(int)SSC32_STOPPOSITION_TIME);
// Find the channel for the visions's yaw servo:
botData.getVal(SID_VISION_YAW, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
// Set the yaw in a neutral position:
SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
(int)SSC32_STOPPOSITION_VISION_YAW,
(int)SSC32_STOPPOSITION_TIME);
}
void dmaPCSerialRx(MODSERIAL_IRQ_INFO *q) {
MODSERIAL *serial = q->serial; //Define local serial class
if(flags.PCBegin) {
if (flags.txSentData == 1) {
PCTimeoutTimer.stop();
PCTimeoutTimer.reset();
flags.txSentData = 0;
}
PCTimeoutTimer.start();
if (PCTimeoutTimer.read_ms() < PC_TIMEOUT_PERIOD) {
incommingPCRxData[incommingPCRxDataCount] = serial->getc();
if(DEBUG) {
GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]);
}
if (incommingPCRxDataCount > 2) {
if ((incommingPCRxData[incommingPCRxDataCount-3]<<(BITLENGTH_OF_A_BYTE*3)) +
(incommingPCRxData[incommingPCRxDataCount-2]<<(BITLENGTH_OF_A_BYTE*2)) +
(incommingPCRxData[incommingPCRxDataCount-1]<<BITLENGTH_OF_A_BYTE) +
(incommingPCRxData[incommingPCRxDataCount]) == PC_TERMINATION_PHRASE) {
if (flags.rxNewData) {
flags.PCTimeout = 1;
led4 = 1;
if(DEBUG) {
GPSSerial.printf("Received new data too soon\r\n");
}
PCTimeoutTimer.stop();
PCTimeoutTimer.reset();
}
else {
memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
memcpy(PCRxData, incommingPCRxData, PC_RX_BUFFER_SIZE); //PCRxData = incommingPCRxData;
memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
incommingPCRxDataCount = 0;
flags.rxNewData = 1;
PCTimeoutTimer.stop();
PCTimeoutTimer.reset();
if(DEBUG) {
GPSSerial.printf("\nReceived a termination\r\n");
}
}
}
else {
incommingPCRxDataCount++;
}
}
else {
incommingPCRxDataCount++;
}
}
else {
flags.PCTimeout = 1;
led2 = 1;
if(DEBUG) {
GPSSerial.printf("Rx timeout\r\n");
}
PCTimeoutTimer.stop();
PCTimeoutTimer.reset();
}
}
else {
incommingPCRxData[incommingPCRxDataCount] = serial->getc();
/*
unsigned int lengthOfPhrase = 9;//sizeof(PC_BEGIN_PHRASE);
if (incommingPCRxDataCount >= lengthOfPhrase) {
if (string_compare(&PC_BEGIN_PHRASE, &(incommingPCRxData[incommingPCRxDataCount-(lengthOfPhrase-1)]), lengthOfPhrase)) {
flags.PCBegin = 1;
memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
incommingPCRxDataCount = 0;
}
else {
incommingPCRxDataCount++;
}
}
else {
incommingPCRxDataCount++;
}
*/
if (incommingPCRxDataCount > 7) {
if ( (incommingPCRxData[incommingPCRxDataCount-8] == 'I') &&
(incommingPCRxData[incommingPCRxDataCount-7] == 'R') &&
(incommingPCRxData[incommingPCRxDataCount-6] == 'I') &&
(incommingPCRxData[incommingPCRxDataCount-5] == 'S') &&
(incommingPCRxData[incommingPCRxDataCount-4] == ':') &&
(incommingPCRxData[incommingPCRxDataCount-3] == 'I') &&
(incommingPCRxData[incommingPCRxDataCount-2] == 'N') &&
(incommingPCRxData[incommingPCRxDataCount-1] == 'I') &&
(incommingPCRxData[incommingPCRxDataCount] == 'T') ) {
flags.PCBegin = 1;
memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
incommingPCRxDataCount = 0;
}
else {
incommingPCRxDataCount++;
}
}
else {
incommingPCRxDataCount++;
}
}
}
bool string_compare(char *a, char *b, unsigned int length) {
for (unsigned int i=0; i<length; i++) {
if (*a != *b) {
return 0;
}
}
return 1;
}
void sendPCSerialData(void) {
int i;
bool terminated = 0;
for (i=0; i<PC_TX_BUFFER_SIZE; i++) {
if ( (PCTxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) +
(PCTxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) +
(PCTxData[i-2]<<BITLENGTH_OF_A_BYTE) +
PCTxData[i-1] == PC_TERMINATION_PHRASE ) {
terminated = 1;
}
if (!terminated) {
PCSerial.putc(PCTxData[i]);
}
}
if(DEBUG) {
terminated = 0;
GPSSerial.printf("Response back...");
for (i=0; i<PC_TX_BUFFER_SIZE; i++) {
if ( (PCTxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) +
(PCTxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) +
(PCTxData[i-2]<<BITLENGTH_OF_A_BYTE) +
PCTxData[i-1] == PC_TERMINATION_PHRASE ) {
terminated = 1;
}
if (!terminated) {
GPSSerial.putc(PCTxData[i]);
}
}
GPSSerial.printf("...Response end\r\n");
}
PCTimeoutTimer.stop(); // precautionary - done after receiving
PCTimeoutTimer.reset(); // precautionary - done after receiving
flags.txSentData = 1;
PCTimeoutTimer.start();
}
void dmaSSC32SerialRx(MODSERIAL_IRQ_INFO *q) {
MODSERIAL *serial = q->serial; //Define local serial class
servoPositionReturn = serial->getc();
}
void dmaGPSSerialRx(MODSERIAL_IRQ_INFO *q) {
}