Adam Wilcock / Mbed 2 deprecated IRIS_MBED

test

Dependencies:   MODDMA MODSERIAL mbed

Fork of IRIS_MBED by IRIS

main.cpp@7:49aba2a2e847, 2015-04-09 (annotated)

Committer:
JonathanAshworth
Date:
Thu Apr 09 20:07:15 2015 +0000
Revision:
7:49aba2a2e847
Parent:
6:8ffc6d3a7c1d 
latest version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonathanAshworth 6:8ffc6d3a7c1d 1 /************************************************************************************
JonathanAshworth 6:8ffc6d3a7c1d 2 PROJECT IRIS
JonathanAshworth 6:8ffc6d3a7c1d 3 Multidisciplinary Group Project
JonathanAshworth 6:8ffc6d3a7c1d 4 ENG701S2 (Year 4 - MENG) at the University of Portsmouth
JonathanAshworth 6:8ffc6d3a7c1d 5
JonathanAshworth 6:8ffc6d3a7c1d 6 Filename: main.cpp
JonathanAshworth 6:8ffc6d3a7c1d 7
JonathanAshworth 6:8ffc6d3a7c1d 8 Description: Root source file for the project. The protocol's
JonathanAshworth 6:8ffc6d3a7c1d 9 timeout functionality is achieved using timer3
JonathanAshworth 6:8ffc6d3a7c1d 10 therefore the use of wait() or similar must be
JonathanAshworth 6:8ffc6d3a7c1d 11 avoided else the system will not adhere to the
JonathanAshworth 6:8ffc6d3a7c1d 12 timeout timings (wait and timer both use timer3).
JonathanAshworth 6:8ffc6d3a7c1d 13
JonathanAshworth 6:8ffc6d3a7c1d 14 Original Created: 05/04/2015
JonathanAshworth 6:8ffc6d3a7c1d 15 Original Author: Jonathan Ashworth
JonathanAshworth 6:8ffc6d3a7c1d 16
JonathanAshworth 6:8ffc6d3a7c1d 17 --------------------------Change Log--------------------------------
JonathanAshworth 6:8ffc6d3a7c1d 18 Date | Author | Details
JonathanAshworth 6:8ffc6d3a7c1d 19 06/04/05 Jonathan Ashworth
JonathanAshworth 6:8ffc6d3a7c1d 20
JonathanAshworth 6:8ffc6d3a7c1d 21 ************************************************************************************/
JonathanAshworth 0:2676c97df44e 22 #include "main.h"
JonathanAshworth 4:1017848d2fe1 23
JonathanAshworth 6:8ffc6d3a7c1d 24 //************************ For Development Only *************************************
JonathanAshworth 6:8ffc6d3a7c1d 25 #define DEBUG 0 // When set GGPSSerial becomes a debug output
JonathanAshworth 6:8ffc6d3a7c1d 26 #define DUMMY 1 // When set botData is filled with random data
JonathanAshworth 6:8ffc6d3a7c1d 27 #define PC_SERIAL_INIT_DISABLE 0
JonathanAshworth 6:8ffc6d3a7c1d 28 DigitalOut led1(LED1); // Timeout led - idle
JonathanAshworth 6:8ffc6d3a7c1d 29 DigitalOut led2(LED2); // rx tacking too long timeout
JonathanAshworth 6:8ffc6d3a7c1d 30 DigitalOut led3(LED3); // bad write timeout
JonathanAshworth 6:8ffc6d3a7c1d 31 DigitalOut led4(LED4); // Invalid value requested to servo/motor - construct tx - data too soon
JonathanAshworth 6:8ffc6d3a7c1d 32 //***********************************************************************************
JonathanAshworth 4:1017848d2fe1 33
JonathanAshworth 6:8ffc6d3a7c1d 34 //************************** Serial DMA Setup ***************************************
JonathanAshworth 6:8ffc6d3a7c1d 35 MODSERIAL PCSerial( PC_TX_PIN, // MBED's transmit pin to PC
JonathanAshworth 6:8ffc6d3a7c1d 36 PC_RX_PIN, // MBED's receive pin from PC
JonathanAshworth 6:8ffc6d3a7c1d 37 PC_TX_BUFFER_SIZE, // Transmiter buffer size
JonathanAshworth 6:8ffc6d3a7c1d 38 PC_RX_BUFFER_SIZE); // Receiver buffer size
JonathanAshworth 6:8ffc6d3a7c1d 39
JonathanAshworth 6:8ffc6d3a7c1d 40 MODSERIAL SSC32Serial(SSC32_TX_PIN, // MBED's transmit pin to SSC32
JonathanAshworth 6:8ffc6d3a7c1d 41 SSC32_RX_PIN, // MBED's receive pin from SSC32
JonathanAshworth 6:8ffc6d3a7c1d 42 SSC32_TX_BUFFER_SIZE, // Transmiter buffer size
JonathanAshworth 6:8ffc6d3a7c1d 43 SSC32_RX_BUFFER_SIZE); // Receiver buffer size
JonathanAshworth 6:8ffc6d3a7c1d 44
JonathanAshworth 6:8ffc6d3a7c1d 45 MODSERIAL GPSSerial(GPS_TX_PIN, // MBED's transmit pin to GPS
JonathanAshworth 6:8ffc6d3a7c1d 46 GPS_RX_PIN, // MBED's receive pin from GPS
JonathanAshworth 6:8ffc6d3a7c1d 47 GPS_TX_BUFFER_SIZE, // Transmiter buffer size
JonathanAshworth 6:8ffc6d3a7c1d 48 GPS_RX_BUFFER_SIZE); // Receiver buffer size
JonathanAshworth 6:8ffc6d3a7c1d 49
JonathanAshworth 6:8ffc6d3a7c1d 50 MODDMA dma; // Define DMA hardware for MODSERIAL to use
JonathanAshworth 4:1017848d2fe1 51
JonathanAshworth 6:8ffc6d3a7c1d 52 // Storage of whole PC requests (termination detected):
JonathanAshworth 6:8ffc6d3a7c1d 53 char PCRxData[PC_RX_BUFFER_SIZE] = {0};
JonathanAshworth 0:2676c97df44e 54
JonathanAshworth 6:8ffc6d3a7c1d 55 // Storage of whole PC responses:
JonathanAshworth 6:8ffc6d3a7c1d 56 char PCTxData[PC_TX_BUFFER_SIZE] = {0};
JonathanAshworth 4:1017848d2fe1 57
JonathanAshworth 6:8ffc6d3a7c1d 58 // Temporary storage of incomming PC bytes as they arrive:
JonathanAshworth 6:8ffc6d3a7c1d 59 char incommingPCRxData[PC_RX_BUFFER_SIZE] = {0};
JonathanAshworth 4:1017848d2fe1 60
JonathanAshworth 6:8ffc6d3a7c1d 61 // Tracking the length of the incomming PC data (number of bytes received):
JonathanAshworth 6:8ffc6d3a7c1d 62 char incommingPCRxDataCount = 0;
JonathanAshworth 4:1017848d2fe1 63
JonathanAshworth 6:8ffc6d3a7c1d 64 // Storage for requested servo position (set by SSC32's rx ISR):
JonathanAshworth 6:8ffc6d3a7c1d 65 char servoPositionReturn = 0;
JonathanAshworth 4:1017848d2fe1 66
JonathanAshworth 6:8ffc6d3a7c1d 67 // PC serial comms timout timer:
JonathanAshworth 6:8ffc6d3a7c1d 68 Timer PCTimeoutTimer;
JonathanAshworth 6:8ffc6d3a7c1d 69 //***********************************************************************************
JonathanAshworth 4:1017848d2fe1 70
JonathanAshworth 6:8ffc6d3a7c1d 71 //************************** ESC Drive Setup ****************************************
JonathanAshworth 6:8ffc6d3a7c1d 72 PwmOut driveNS(DRIVE_NS_PIN); // Nearside ESC
JonathanAshworth 6:8ffc6d3a7c1d 73 PwmOut driveOS(DRIVE_OS_PIN); // Offside ESC
JonathanAshworth 6:8ffc6d3a7c1d 74 //***********************************************************************************
JonathanAshworth 4:1017848d2fe1 75
JonathanAshworth 6:8ffc6d3a7c1d 76 //HCSR04 usound(p17, p18); //trig echo
JonathanAshworth 4:1017848d2fe1 77
JonathanAshworth 6:8ffc6d3a7c1d 78 Flags flags; // Event/status tracking flag structure
JonathanAshworth 6:8ffc6d3a7c1d 79 botStateHandler botData; // Storage class for all peripherals
JonathanAshworth 4:1017848d2fe1 80
JonathanAshworth 6:8ffc6d3a7c1d 81 // limit pitch 1300 lower limit 2200 max
JonathanAshworth 0:2676c97df44e 82
JonathanAshworth 0:2676c97df44e 83 int main(void) {
JonathanAshworth 0:2676c97df44e 84
JonathanAshworth 6:8ffc6d3a7c1d 85 initialise();
JonathanAshworth 0:2676c97df44e 86
JonathanAshworth 6:8ffc6d3a7c1d 87 while(!flags.PCBegin); // Wait for the PC to send the begin phrase
JonathanAshworth 0:2676c97df44e 88
JonathanAshworth 0:2676c97df44e 89 while(1) {
JonathanAshworth 6:8ffc6d3a7c1d 90
JonathanAshworth 6:8ffc6d3a7c1d 91 // If the PC serial rx line is idle for too long then timeout:
JonathanAshworth 6:8ffc6d3a7c1d 92 if (PCTimeoutTimer.read_ms() > PC_TIMEOUT_PERIOD) {
JonathanAshworth 6:8ffc6d3a7c1d 93 flags.PCTimeout = 1;
JonathanAshworth 6:8ffc6d3a7c1d 94 led1 = 1;
JonathanAshworth 6:8ffc6d3a7c1d 95 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 96 GPSSerial.printf("Idle timeout\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 97 }
JonathanAshworth 6:8ffc6d3a7c1d 98 }
JonathanAshworth 4:1017848d2fe1 99
JonathanAshworth 6:8ffc6d3a7c1d 100 // If a timeout has been set anywhere then process the timeout function:
JonathanAshworth 6:8ffc6d3a7c1d 101 if (flags.PCTimeout) {
JonathanAshworth 4:1017848d2fe1 102 PCTimeout();
JonathanAshworth 4:1017848d2fe1 103 }
JonathanAshworth 6:8ffc6d3a7c1d 104
JonathanAshworth 6:8ffc6d3a7c1d 105 if (flags.rxNewData) { // If a new data packet has been received:
JonathanAshworth 6:8ffc6d3a7c1d 106 parseRequestData(); // Complete the requests
JonathanAshworth 6:8ffc6d3a7c1d 107 flags.rxNewData = 0; // Data is now old so reset flag
JonathanAshworth 0:2676c97df44e 108 }
JonathanAshworth 6:8ffc6d3a7c1d 109
JonathanAshworth 6:8ffc6d3a7c1d 110 if (flags.txNewData) {
JonathanAshworth 6:8ffc6d3a7c1d 111 sendPCSerialData(); // Return the requested data
JonathanAshworth 6:8ffc6d3a7c1d 112 flags.txNewData = 0; // Data is now old so reset flag
JonathanAshworth 6:8ffc6d3a7c1d 113 }
JonathanAshworth 6:8ffc6d3a7c1d 114
JonathanAshworth 0:2676c97df44e 115 }
JonathanAshworth 6:8ffc6d3a7c1d 116
JonathanAshworth 0:2676c97df44e 117 }
JonathanAshworth 0:2676c97df44e 118
JonathanAshworth 4:1017848d2fe1 119 void initialise(void) {
JonathanAshworth 4:1017848d2fe1 120
JonathanAshworth 6:8ffc6d3a7c1d 121 // Setup serial links and their ISRs:
JonathanAshworth 6:8ffc6d3a7c1d 122 PCSerial.baud(PC_BAUDRATE);
JonathanAshworth 6:8ffc6d3a7c1d 123 PCSerial.attach(&dmaPCSerialRx, MODSERIAL::RxIrq); // rx ISR
JonathanAshworth 6:8ffc6d3a7c1d 124 SSC32Serial.baud(SSC32_BAUDRATE);
JonathanAshworth 6:8ffc6d3a7c1d 125 SSC32Serial.attach(&dmaSSC32SerialRx, MODSERIAL::RxIrq); // rx ISR
JonathanAshworth 6:8ffc6d3a7c1d 126 GPSSerial.baud(GPS_BAUDRATE);
JonathanAshworth 6:8ffc6d3a7c1d 127 GPSSerial.attach(&dmaGPSSerialRx, MODSERIAL::RxIrq); // rx ISR
JonathanAshworth 4:1017848d2fe1 128
JonathanAshworth 6:8ffc6d3a7c1d 129 // Stop and reset the timeout timer:
JonathanAshworth 4:1017848d2fe1 130 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 131 PCTimeoutTimer.reset();
JonathanAshworth 4:1017848d2fe1 132
JonathanAshworth 6:8ffc6d3a7c1d 133 // Define the PWM period for the drive ESCs:
JonathanAshworth 6:8ffc6d3a7c1d 134 driveNS.period_ms(20); // Nearside
JonathanAshworth 6:8ffc6d3a7c1d 135 driveOS.period_ms(20); // Offside
JonathanAshworth 4:1017848d2fe1 136
JonathanAshworth 6:8ffc6d3a7c1d 137 // Set the channels for each servo in field 0 of the bot's DB:
JonathanAshworth 6:8ffc6d3a7c1d 138 float armBaseChannel = SERVO_ARM_BASE_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 139 float armShoulderChannel = SERVO_ARM_SHOULDER_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 140 float armElbowChannel = SERVO_ARM_ELBOW_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 141 float armWristChannel = SERVO_ARM_WRIST_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 142 float armManChannel = SERVO_ARM_MANIPULATOR_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 143 float visionPitchChannel = SERVO_VISION_PITCH_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 144 float visionYawChannel = SERVO_VISION_YAW_CHAN;
JonathanAshworth 6:8ffc6d3a7c1d 145 botData.setVal(SID_ARM_BASE, 0, &armBaseChannel);
JonathanAshworth 6:8ffc6d3a7c1d 146 botData.setVal(SID_ARM_SHOULDER, 0, &armShoulderChannel);
JonathanAshworth 6:8ffc6d3a7c1d 147 botData.setVal(SID_ARM_ELBOW, 0, &armElbowChannel);
JonathanAshworth 6:8ffc6d3a7c1d 148 botData.setVal(SID_ARM_WRIST, 0, &armWristChannel);
JonathanAshworth 6:8ffc6d3a7c1d 149 botData.setVal(SID_ARM_MANIPULATOR, 0, &armManChannel);
JonathanAshworth 6:8ffc6d3a7c1d 150 botData.setVal(SID_VISION_PITCH, 0, &visionPitchChannel);
JonathanAshworth 6:8ffc6d3a7c1d 151 botData.setVal(SID_VISION_YAW, 0, &visionYawChannel);
JonathanAshworth 4:1017848d2fe1 152
JonathanAshworth 6:8ffc6d3a7c1d 153 shutdown(); // Ensure the bot is in a safe shutdown mode/position
JonathanAshworth 4:1017848d2fe1 154
JonathanAshworth 6:8ffc6d3a7c1d 155 if (DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 156 GPSSerial.printf("Hello\r\n"); // Aid setup of a terminal to the MBED
JonathanAshworth 6:8ffc6d3a7c1d 157 }
JonathanAshworth 4:1017848d2fe1 158
JonathanAshworth 6:8ffc6d3a7c1d 159 flags.PCBegin = (bool)PC_SERIAL_INIT_DISABLE; // TODO: remove
JonathanAshworth 0:2676c97df44e 160 }
JonathanAshworth 6:8ffc6d3a7c1d 161 /*
JonathanAshworth 6:8ffc6d3a7c1d 162 void updateSensorsData(void) {
JonathanAshworth 6:8ffc6d3a7c1d 163 int pirAlarm = mbed_PIR();
JonathanAshworth 6:8ffc6d3a7c1d 164 float temperature = mbed_tempsensor();
JonathanAshworth 6:8ffc6d3a7c1d 165 float humidity = mbed_humiditysensor();
JonathanAshworth 6:8ffc6d3a7c1d 166 float compassX = getcompassX();
JonathanAshworth 6:8ffc6d3a7c1d 167 float compassY = getcompassY();
JonathanAshworth 6:8ffc6d3a7c1d 168 float compassZ = getcompassZ();
JonathanAshworth 6:8ffc6d3a7c1d 169 float gyroX = getgyroX();
JonathanAshworth 6:8ffc6d3a7c1d 170 float gyroY = getgyroY();
JonathanAshworth 6:8ffc6d3a7c1d 171 float gyroZ = getgyroZ();
JonathanAshworth 6:8ffc6d3a7c1d 172 int accelX = getaccelX();
JonathanAshworth 6:8ffc6d3a7c1d 173 int accelY = getaccelY();
JonathanAshworth 6:8ffc6d3a7c1d 174 int accelZ = getaccelZ();
JonathanAshworth 6:8ffc6d3a7c1d 175 }
JonathanAshworth 6:8ffc6d3a7c1d 176 */
JonathanAshworth 6:8ffc6d3a7c1d 177 /*
JonathanAshworth 4:1017848d2fe1 178 void getServoData(Servo *servo) {
JonathanAshworth 4:1017848d2fe1 179 SSC32Serial.printf("QP %d\r", servo->channel);
JonathanAshworth 4:1017848d2fe1 180 int temp = 1;
JonathanAshworth 4:1017848d2fe1 181 //read reply
JonathanAshworth 4:1017848d2fe1 182 //convert char to int
JonathanAshworth 4:1017848d2fe1 183 servo->position = temp;
JonathanAshworth 4:1017848d2fe1 184 }
JonathanAshworth 6:8ffc6d3a7c1d 185 */
JonathanAshworth 4:1017848d2fe1 186
JonathanAshworth 6:8ffc6d3a7c1d 187
JonathanAshworth 6:8ffc6d3a7c1d 188 long readUltrasound(void) {/*
JonathanAshworth 4:1017848d2fe1 189 //Set Mux to sensor->muxChannel
JonathanAshworth 4:1017848d2fe1 190 //Pulse trigger pin
JonathanAshworth 4:1017848d2fe1 191 //Disable interrupts
JonathanAshworth 4:1017848d2fe1 192 //Read PWM on echo pin
JonathanAshworth 4:1017848d2fe1 193 //Enable interrupts
JonathanAshworth 6:8ffc6d3a7c1d 194 //sensor->distanceMillimeters = result;*/
JonathanAshworth 6:8ffc6d3a7c1d 195 long distance = 0;
JonathanAshworth 6:8ffc6d3a7c1d 196 //distance = usound.distance(1);
JonathanAshworth 6:8ffc6d3a7c1d 197 return distance;
JonathanAshworth 4:1017848d2fe1 198 }
JonathanAshworth 4:1017848d2fe1 199
JonathanAshworth 6:8ffc6d3a7c1d 200
JonathanAshworth 6:8ffc6d3a7c1d 201 void constructTxData(char *SIDs, char numberOfSensors) { // TODO: Tidy up
JonathanAshworth 6:8ffc6d3a7c1d 202 int i, j, lengthOfCurrentSentence;
JonathanAshworth 6:8ffc6d3a7c1d 203 char sendSentence[PC_LONGESTRESPONSESENTENCE] = {0};
JonathanAshworth 6:8ffc6d3a7c1d 204 char fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 205 int acumulatedLengthOfPreviousSentences = 0;
JonathanAshworth 4:1017848d2fe1 206
JonathanAshworth 6:8ffc6d3a7c1d 207 float accelX, accelY, accelZ;
JonathanAshworth 6:8ffc6d3a7c1d 208 char accelXArr[BYTELENGTH_OF_A_FLOAT], accelYArr[BYTELENGTH_OF_A_FLOAT], accelZArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 209 float rotX, rotY, rotZ;
JonathanAshworth 6:8ffc6d3a7c1d 210 char rotXArr[BYTELENGTH_OF_A_FLOAT], rotYArr[BYTELENGTH_OF_A_FLOAT], rotZArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 211 float magX, magY, magZ;
JonathanAshworth 6:8ffc6d3a7c1d 212 char magXArr[BYTELENGTH_OF_A_FLOAT], magYArr[BYTELENGTH_OF_A_FLOAT], magZArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 213 float lat, lon, spd;
JonathanAshworth 6:8ffc6d3a7c1d 214 char gpsLatArr[BYTELENGTH_OF_A_FLOAT], gpsLonArr[BYTELENGTH_OF_A_FLOAT], gpsSpdArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 215 float temperature, humidity, voltage, current, pirAlarm;
JonathanAshworth 6:8ffc6d3a7c1d 216 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];
JonathanAshworth 6:8ffc6d3a7c1d 217 float position, speed, time;
JonathanAshworth 6:8ffc6d3a7c1d 218 char positionArr[BYTELENGTH_OF_A_FLOAT], speedArr[BYTELENGTH_OF_A_FLOAT], timeArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 219 float demand, distance;
JonathanAshworth 6:8ffc6d3a7c1d 220 char demandArr[BYTELENGTH_OF_A_FLOAT], distanceArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 5:d0b2b4d8b9ba 221
JonathanAshworth 6:8ffc6d3a7c1d 222 float channel;
JonathanAshworth 6:8ffc6d3a7c1d 223
JonathanAshworth 6:8ffc6d3a7c1d 224 unsigned int termBigEnd;
JonathanAshworth 6:8ffc6d3a7c1d 225
JonathanAshworth 6:8ffc6d3a7c1d 226 memset(PCTxData, 0, PC_TX_BUFFER_SIZE); //reset tx buffer
JonathanAshworth 5:d0b2b4d8b9ba 227
JonathanAshworth 6:8ffc6d3a7c1d 228 for (i=0; i<numberOfSensors; i++) {
JonathanAshworth 6:8ffc6d3a7c1d 229 switch (SIDs[i]) {
JonathanAshworth 6:8ffc6d3a7c1d 230 case SID_9DOF_ACC: // 9DOF acc
JonathanAshworth 6:8ffc6d3a7c1d 231 fieldCount = 3;
JonathanAshworth 6:8ffc6d3a7c1d 232 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_X, &accelX);
JonathanAshworth 6:8ffc6d3a7c1d 233 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Y, &accelY);
JonathanAshworth 6:8ffc6d3a7c1d 234 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Z, &accelZ);
JonathanAshworth 6:8ffc6d3a7c1d 235 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 236 if(accelX == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 237 accelX = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 238 accelY = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 239 accelZ = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 240 }
JonathanAshworth 6:8ffc6d3a7c1d 241 else {
JonathanAshworth 6:8ffc6d3a7c1d 242 accelX++;
JonathanAshworth 6:8ffc6d3a7c1d 243 accelY++;
JonathanAshworth 6:8ffc6d3a7c1d 244 accelZ++;
JonathanAshworth 6:8ffc6d3a7c1d 245 }
JonathanAshworth 6:8ffc6d3a7c1d 246 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_X, &accelX);
JonathanAshworth 6:8ffc6d3a7c1d 247 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Y, &accelY);
JonathanAshworth 6:8ffc6d3a7c1d 248 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_ACCEL_Z, &accelZ);
JonathanAshworth 6:8ffc6d3a7c1d 249 }
JonathanAshworth 6:8ffc6d3a7c1d 250 memcpy(accelXArr, &accelX, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 251 memcpy(accelYArr, &accelY, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 252 memcpy(accelZArr, &accelZ, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 253 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 254 sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF accel ID
JonathanAshworth 6:8ffc6d3a7c1d 255 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 256 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 257 sendSentence[PC_FLOAT0_LOCATION+j] = accelXArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 258 sendSentence[PC_FLOAT1_LOCATION+j] = accelYArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 259 sendSentence[PC_FLOAT2_LOCATION+j] = accelZArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 260 }
JonathanAshworth 6:8ffc6d3a7c1d 261 lengthOfCurrentSentence = PC_9DOFACC_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 262 break;
JonathanAshworth 6:8ffc6d3a7c1d 263 case SID_9DOF_GYRO: // 9DOF gyro
JonathanAshworth 6:8ffc6d3a7c1d 264 fieldCount = 3;
JonathanAshworth 6:8ffc6d3a7c1d 265 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_X, &rotX);
JonathanAshworth 6:8ffc6d3a7c1d 266 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Y, &rotY);
JonathanAshworth 6:8ffc6d3a7c1d 267 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Z, &rotZ);
JonathanAshworth 6:8ffc6d3a7c1d 268 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 269 if(rotX == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 270 rotX = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 271 rotY = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 272 rotZ = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 273 }
JonathanAshworth 6:8ffc6d3a7c1d 274 else {
JonathanAshworth 6:8ffc6d3a7c1d 275 rotX++;
JonathanAshworth 6:8ffc6d3a7c1d 276 rotY++;
JonathanAshworth 6:8ffc6d3a7c1d 277 rotZ++;
JonathanAshworth 6:8ffc6d3a7c1d 278 }
JonathanAshworth 6:8ffc6d3a7c1d 279 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_X, &rotX);
JonathanAshworth 6:8ffc6d3a7c1d 280 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Y, &rotY);
JonathanAshworth 6:8ffc6d3a7c1d 281 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_MAG_Z, &rotZ);
JonathanAshworth 6:8ffc6d3a7c1d 282 }
JonathanAshworth 6:8ffc6d3a7c1d 283 memcpy(rotXArr, &rotX, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 284 memcpy(rotYArr, &rotY, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 285 memcpy(rotZArr, &rotZ, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 286 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 287 sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF gyro ID
JonathanAshworth 6:8ffc6d3a7c1d 288 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 289 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 290 sendSentence[PC_FLOAT0_LOCATION+j] = rotXArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 291 sendSentence[PC_FLOAT1_LOCATION+j] = rotYArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 292 sendSentence[PC_FLOAT2_LOCATION+j] = rotZArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 293 }
JonathanAshworth 6:8ffc6d3a7c1d 294 lengthOfCurrentSentence = PC_9DOFGYR_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 295 break;
JonathanAshworth 6:8ffc6d3a7c1d 296 case SID_9DOF_MAG: // 9DOF mag
JonathanAshworth 6:8ffc6d3a7c1d 297 fieldCount = 3;
JonathanAshworth 6:8ffc6d3a7c1d 298 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_X, &magX);
JonathanAshworth 6:8ffc6d3a7c1d 299 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Y, &magY);
JonathanAshworth 6:8ffc6d3a7c1d 300 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Z, &magZ);
JonathanAshworth 6:8ffc6d3a7c1d 301 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 302 if(magX == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 303 magX = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 304 magY = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 305 magZ = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 306 }
JonathanAshworth 6:8ffc6d3a7c1d 307 else {
JonathanAshworth 6:8ffc6d3a7c1d 308 magX++;
JonathanAshworth 6:8ffc6d3a7c1d 309 magY++;
JonathanAshworth 6:8ffc6d3a7c1d 310 magZ++;
JonathanAshworth 6:8ffc6d3a7c1d 311 }
JonathanAshworth 6:8ffc6d3a7c1d 312 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_X, &magX);
JonathanAshworth 6:8ffc6d3a7c1d 313 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Y, &magY);
JonathanAshworth 6:8ffc6d3a7c1d 314 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_9DOF_GYRO_Z, &magZ);
JonathanAshworth 6:8ffc6d3a7c1d 315 }
JonathanAshworth 6:8ffc6d3a7c1d 316 memcpy(magXArr, &magX, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 317 memcpy(magYArr, &magY, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 318 memcpy(magZArr, &magZ, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 319 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 320 sendSentence[PC_SID_LOCATION] = SIDs[i]; //9DOF mag ID
JonathanAshworth 6:8ffc6d3a7c1d 321 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 322 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 323 sendSentence[PC_FLOAT0_LOCATION+j] = magXArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 324 sendSentence[PC_FLOAT1_LOCATION+j] = magYArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 325 sendSentence[PC_FLOAT2_LOCATION+j] = magZArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 326 }
JonathanAshworth 6:8ffc6d3a7c1d 327 lengthOfCurrentSentence = PC_9DOFMAG_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 328 break;
JonathanAshworth 6:8ffc6d3a7c1d 329 case SID_GPS: // GPS
JonathanAshworth 6:8ffc6d3a7c1d 330 fieldCount = 3;
JonathanAshworth 6:8ffc6d3a7c1d 331 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LAT, &lat);
JonathanAshworth 6:8ffc6d3a7c1d 332 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LONG, &lon);
JonathanAshworth 6:8ffc6d3a7c1d 333 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_SPEED, &spd);
JonathanAshworth 6:8ffc6d3a7c1d 334 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 335 if(lat == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 336 lat = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 337 lon = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 338 spd = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 339 }
JonathanAshworth 6:8ffc6d3a7c1d 340 else {
JonathanAshworth 6:8ffc6d3a7c1d 341 lat++;
JonathanAshworth 6:8ffc6d3a7c1d 342 lon++;
JonathanAshworth 6:8ffc6d3a7c1d 343 spd++;
JonathanAshworth 6:8ffc6d3a7c1d 344 }
JonathanAshworth 6:8ffc6d3a7c1d 345 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LAT, &lat);
JonathanAshworth 6:8ffc6d3a7c1d 346 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_LONG, &lon);
JonathanAshworth 6:8ffc6d3a7c1d 347 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_GPS_SPEED, &spd);
JonathanAshworth 6:8ffc6d3a7c1d 348 }
JonathanAshworth 6:8ffc6d3a7c1d 349 memcpy(gpsLatArr, &lat, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 350 memcpy(gpsLonArr, &lon, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 351 memcpy(gpsSpdArr, &spd, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 352 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 353 sendSentence[PC_SID_LOCATION] = SIDs[i]; //GPS ID
JonathanAshworth 6:8ffc6d3a7c1d 354 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 355 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 356 sendSentence[PC_FLOAT0_LOCATION+j] = gpsLatArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 357 sendSentence[PC_FLOAT1_LOCATION+j] = gpsLonArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 358 sendSentence[PC_FLOAT2_LOCATION+j] = gpsSpdArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 359 }
JonathanAshworth 6:8ffc6d3a7c1d 360 lengthOfCurrentSentence = PC_GPS_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 361 break;
JonathanAshworth 6:8ffc6d3a7c1d 362 case SID_TEMP: // Temperature
JonathanAshworth 6:8ffc6d3a7c1d 363 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 364 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_TEMPERATURE, &temperature);
JonathanAshworth 6:8ffc6d3a7c1d 365 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 366 if (temperature == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 367 temperature = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 368 }
JonathanAshworth 6:8ffc6d3a7c1d 369 else {
JonathanAshworth 6:8ffc6d3a7c1d 370 temperature++;
JonathanAshworth 6:8ffc6d3a7c1d 371 }
JonathanAshworth 6:8ffc6d3a7c1d 372 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_TEMPERATURE, &temperature);
JonathanAshworth 6:8ffc6d3a7c1d 373 }
JonathanAshworth 6:8ffc6d3a7c1d 374 memcpy(temperatureArr, &temperature, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 375 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 376 sendSentence[PC_SID_LOCATION] = SIDs[i]; //Temperature ID
JonathanAshworth 6:8ffc6d3a7c1d 377 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 378 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 379 sendSentence[PC_FLOAT0_LOCATION+j] = temperatureArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 380 }
JonathanAshworth 6:8ffc6d3a7c1d 381 lengthOfCurrentSentence = PC_TEMPERATURE_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 382 break;
JonathanAshworth 6:8ffc6d3a7c1d 383 case SID_HUM: // Humidity
JonathanAshworth 6:8ffc6d3a7c1d 384 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 385 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_HUMIDITY, &humidity);
JonathanAshworth 6:8ffc6d3a7c1d 386 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 387 if (humidity == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 388 humidity = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 389 }
JonathanAshworth 6:8ffc6d3a7c1d 390 else {
JonathanAshworth 6:8ffc6d3a7c1d 391 humidity++;
JonathanAshworth 6:8ffc6d3a7c1d 392 }
JonathanAshworth 6:8ffc6d3a7c1d 393 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_HUMIDITY, &humidity);
JonathanAshworth 6:8ffc6d3a7c1d 394 }
JonathanAshworth 6:8ffc6d3a7c1d 395 memcpy(humidityArr, &humidity, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 396 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 397 sendSentence[PC_SID_LOCATION] = SIDs[i]; //Humidity ID
JonathanAshworth 6:8ffc6d3a7c1d 398 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 399 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 400 sendSentence[PC_FLOAT0_LOCATION+j] = humidityArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 401 }
JonathanAshworth 6:8ffc6d3a7c1d 402 lengthOfCurrentSentence = PC_HUMIDITY_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 403 break;
JonathanAshworth 6:8ffc6d3a7c1d 404 case SID_BATT_VOL: // Battery Voltage
JonathanAshworth 6:8ffc6d3a7c1d 405 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 406 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_VOLTAGE, &voltage);
JonathanAshworth 6:8ffc6d3a7c1d 407 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 408 if (voltage == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 409 voltage = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 410 }
JonathanAshworth 6:8ffc6d3a7c1d 411 else {
JonathanAshworth 6:8ffc6d3a7c1d 412 voltage++;
JonathanAshworth 6:8ffc6d3a7c1d 413 }
JonathanAshworth 6:8ffc6d3a7c1d 414 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_VOLTAGE, &voltage);
JonathanAshworth 6:8ffc6d3a7c1d 415 }
JonathanAshworth 6:8ffc6d3a7c1d 416 memcpy(voltageArr, &voltage, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 417 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 418 sendSentence[PC_SID_LOCATION] = SIDs[i]; //Voltage sensor ID
JonathanAshworth 6:8ffc6d3a7c1d 419 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 420 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 421 sendSentence[PC_FLOAT0_LOCATION+j] = voltageArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 422 }
JonathanAshworth 6:8ffc6d3a7c1d 423 lengthOfCurrentSentence = PC_BATTVOLTAGE_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 424 break;
JonathanAshworth 6:8ffc6d3a7c1d 425 case SID_BATT_CUR: // Battery Current
JonathanAshworth 6:8ffc6d3a7c1d 426 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 427 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_CURRENT, &current);
JonathanAshworth 6:8ffc6d3a7c1d 428 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 429 if (current == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 430 current = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 431 }
JonathanAshworth 6:8ffc6d3a7c1d 432 else {
JonathanAshworth 6:8ffc6d3a7c1d 433 current++;
JonathanAshworth 6:8ffc6d3a7c1d 434 }
JonathanAshworth 6:8ffc6d3a7c1d 435 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_CURRENT, &current);
JonathanAshworth 6:8ffc6d3a7c1d 436 }
JonathanAshworth 6:8ffc6d3a7c1d 437 memcpy(currentArr, &current, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 438 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 439 sendSentence[PC_SID_LOCATION] = SIDs[i]; //Current sensor ID
JonathanAshworth 6:8ffc6d3a7c1d 440 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 441 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 442 sendSentence[PC_FLOAT0_LOCATION+j] = currentArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 443 }
JonathanAshworth 6:8ffc6d3a7c1d 444 lengthOfCurrentSentence = PC_BATTCURRENT_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 445 break;
JonathanAshworth 6:8ffc6d3a7c1d 446 case SID_PIR: // PIR
JonathanAshworth 6:8ffc6d3a7c1d 447 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 448 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_PIR, &pirAlarm);
JonathanAshworth 6:8ffc6d3a7c1d 449 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 450 if (pirAlarm == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 451 pirAlarm = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 452 }
JonathanAshworth 6:8ffc6d3a7c1d 453 else {
JonathanAshworth 6:8ffc6d3a7c1d 454 pirAlarm++;
JonathanAshworth 6:8ffc6d3a7c1d 455 }
JonathanAshworth 6:8ffc6d3a7c1d 456 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_PIR, &pirAlarm);
JonathanAshworth 6:8ffc6d3a7c1d 457 }
JonathanAshworth 6:8ffc6d3a7c1d 458 memcpy(pirAlarmArr, &pirAlarm, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 459 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 460 sendSentence[PC_SID_LOCATION] = SIDs[i]; //PIR ID
JonathanAshworth 6:8ffc6d3a7c1d 461 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 462 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 463 sendSentence[PC_FLOAT0_LOCATION+j] = pirAlarmArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 464 }
JonathanAshworth 6:8ffc6d3a7c1d 465 lengthOfCurrentSentence = PC_PIR_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 466 break;
JonathanAshworth 6:8ffc6d3a7c1d 467 default: break;
JonathanAshworth 6:8ffc6d3a7c1d 468 }
JonathanAshworth 6:8ffc6d3a7c1d 469 if ((SIDs[i] >= SID_FIRSTSERVO_LOCATION)&&
JonathanAshworth 6:8ffc6d3a7c1d 470 (SIDs[i] <= SID_LASTSERVO_LOCATION)) { //Servos
JonathanAshworth 6:8ffc6d3a7c1d 471 fieldCount = 3;
JonathanAshworth 6:8ffc6d3a7c1d 472 channel = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 473 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 474 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_POSITION, &position);
JonathanAshworth 6:8ffc6d3a7c1d 475 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed);
JonathanAshworth 6:8ffc6d3a7c1d 476 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_TIME, &time);
JonathanAshworth 6:8ffc6d3a7c1d 477 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 478 if(position == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 479 position = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 480 speed = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 481 time = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 482 }
JonathanAshworth 6:8ffc6d3a7c1d 483 else {
JonathanAshworth 6:8ffc6d3a7c1d 484 time++;
JonathanAshworth 6:8ffc6d3a7c1d 485 time++;
JonathanAshworth 6:8ffc6d3a7c1d 486 time++;
JonathanAshworth 6:8ffc6d3a7c1d 487 }
JonathanAshworth 6:8ffc6d3a7c1d 488 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_POSITION, &position);
JonathanAshworth 6:8ffc6d3a7c1d 489 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed);
JonathanAshworth 6:8ffc6d3a7c1d 490 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_SERVO_TIME, &time);
JonathanAshworth 6:8ffc6d3a7c1d 491 }
JonathanAshworth 6:8ffc6d3a7c1d 492
JonathanAshworth 6:8ffc6d3a7c1d 493 //SSC32Serial.printf("QP #%d \r", (char)channel);
JonathanAshworth 6:8ffc6d3a7c1d 494 //while(servoPositionReturn == 0) {;}
JonathanAshworth 6:8ffc6d3a7c1d 495 //position = servoPositionReturn;
JonathanAshworth 6:8ffc6d3a7c1d 496 //servoPositionReturn = 0;
JonathanAshworth 6:8ffc6d3a7c1d 497
JonathanAshworth 6:8ffc6d3a7c1d 498 memcpy(positionArr, &position, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 499 memcpy(speedArr, &speed, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 500 memcpy(timeArr, &time, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 501 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 502 sendSentence[PC_SID_LOCATION] = SIDs[i]; //servo ID
JonathanAshworth 6:8ffc6d3a7c1d 503 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 504 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 505 sendSentence[PC_FLOAT0_LOCATION+j] = positionArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 506 sendSentence[PC_FLOAT1_LOCATION+j] = speedArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 507 sendSentence[PC_FLOAT2_LOCATION+j] = timeArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 508
JonathanAshworth 6:8ffc6d3a7c1d 509 }
JonathanAshworth 6:8ffc6d3a7c1d 510 lengthOfCurrentSentence = PC_SERVO_REQUEST_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 511 }
JonathanAshworth 6:8ffc6d3a7c1d 512 if ((SIDs[i] >= SID_FIRSTMOTOR_LOCATION)&&
JonathanAshworth 6:8ffc6d3a7c1d 513 (SIDs[i] <= SID_LASTMOTOR_LOCATION)) { //Motors
JonathanAshworth 6:8ffc6d3a7c1d 514 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 515 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
JonathanAshworth 6:8ffc6d3a7c1d 516 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 517 if (demand == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 518 demand = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 519 }
JonathanAshworth 6:8ffc6d3a7c1d 520 else {
JonathanAshworth 6:8ffc6d3a7c1d 521 demand++;
JonathanAshworth 6:8ffc6d3a7c1d 522 }
JonathanAshworth 6:8ffc6d3a7c1d 523 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
JonathanAshworth 6:8ffc6d3a7c1d 524 }
JonathanAshworth 6:8ffc6d3a7c1d 525 memcpy(demandArr, &demand, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 526 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 527 sendSentence[PC_SID_LOCATION] = SIDs[i]; //Motor ID
JonathanAshworth 6:8ffc6d3a7c1d 528 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 529 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 530 sendSentence[PC_FLOAT0_LOCATION+j] = demandArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 531 }
JonathanAshworth 6:8ffc6d3a7c1d 532 lengthOfCurrentSentence = PC_MOTOR_REQUEST_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 533 }
JonathanAshworth 6:8ffc6d3a7c1d 534 if ((SIDs[i] >= SID_FIRSTUSOUND_LOCATION)&&
JonathanAshworth 6:8ffc6d3a7c1d 535 (SIDs[i] <= SID_LASTUSOUND_LOCATION)) { //Ultrasounds
JonathanAshworth 6:8ffc6d3a7c1d 536 fieldCount = 1;
JonathanAshworth 6:8ffc6d3a7c1d 537 botData.getVal(SIDs[i], BOTDATA_FIELDNUMBER_USOUND_DISTANCE, &distance);
JonathanAshworth 6:8ffc6d3a7c1d 538 if (DUMMY) {
JonathanAshworth 6:8ffc6d3a7c1d 539 /*if (distance == 255.0f) {
JonathanAshworth 6:8ffc6d3a7c1d 540 distance = 0.0f;
JonathanAshworth 6:8ffc6d3a7c1d 541 }
JonathanAshworth 6:8ffc6d3a7c1d 542 else {
JonathanAshworth 6:8ffc6d3a7c1d 543 distance++;
JonathanAshworth 6:8ffc6d3a7c1d 544 }*/
JonathanAshworth 6:8ffc6d3a7c1d 545 // distance = (float)usound.distance(1);
JonathanAshworth 6:8ffc6d3a7c1d 546 distance = -1.0f;
JonathanAshworth 6:8ffc6d3a7c1d 547 if (DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 548 GPSSerial.printf("USOUND Distance (cm): %f\r\n", distance);
JonathanAshworth 6:8ffc6d3a7c1d 549 }
JonathanAshworth 6:8ffc6d3a7c1d 550 botData.setVal(SIDs[i], BOTDATA_FIELDNUMBER_USOUND_DISTANCE, &distance);
JonathanAshworth 6:8ffc6d3a7c1d 551 }
JonathanAshworth 6:8ffc6d3a7c1d 552 memcpy(distanceArr, &distance, BYTELENGTH_OF_A_FLOAT);
JonathanAshworth 6:8ffc6d3a7c1d 553 sendSentence[PC_CID_LOCATION] = (char)SID_MBED;
JonathanAshworth 6:8ffc6d3a7c1d 554 sendSentence[PC_SID_LOCATION] = SIDs[i]; //Ultrasound ID
JonathanAshworth 6:8ffc6d3a7c1d 555 sendSentence[PC_FIELDCOUNT_LOCATION] = fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 556 for (j=0; j<BYTELENGTH_OF_A_FLOAT; j++) {
JonathanAshworth 6:8ffc6d3a7c1d 557 sendSentence[PC_FLOAT0_LOCATION+j] = distanceArr[j];
JonathanAshworth 6:8ffc6d3a7c1d 558 }
JonathanAshworth 6:8ffc6d3a7c1d 559 lengthOfCurrentSentence = PC_USOUND_RESPONSE_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 560 }
JonathanAshworth 6:8ffc6d3a7c1d 561 memcpy(&PCTxData[acumulatedLengthOfPreviousSentences], sendSentence, lengthOfCurrentSentence);
JonathanAshworth 6:8ffc6d3a7c1d 562 acumulatedLengthOfPreviousSentences = acumulatedLengthOfPreviousSentences + lengthOfCurrentSentence;
JonathanAshworth 6:8ffc6d3a7c1d 563 }
JonathanAshworth 6:8ffc6d3a7c1d 564
JonathanAshworth 6:8ffc6d3a7c1d 565 // Convert from little endian to big endian:
JonathanAshworth 6:8ffc6d3a7c1d 566 termBigEnd = PC_TERMINATION_PHRASE;
JonathanAshworth 6:8ffc6d3a7c1d 567 termBigEnd = (termBigEnd>>BITLENGTH_OF_A_BYTE*3) | // Send byte 4 to byte 0
JonathanAshworth 6:8ffc6d3a7c1d 568 ((termBigEnd<<BITLENGTH_OF_A_BYTE) & 0x00ff0000) | // Send byte 2 to byte 3
JonathanAshworth 6:8ffc6d3a7c1d 569 ((termBigEnd>>BITLENGTH_OF_A_BYTE) & 0x0000ff00) | // Send byte 3 to btye 2
JonathanAshworth 6:8ffc6d3a7c1d 570 (termBigEnd<<BITLENGTH_OF_A_BYTE*3); // Send byte 0 to byte 4
JonathanAshworth 6:8ffc6d3a7c1d 571 memcpy(&PCTxData[acumulatedLengthOfPreviousSentences], &termBigEnd, BYTELENGTH_OF_AN_INT);
JonathanAshworth 6:8ffc6d3a7c1d 572 flags.txNewData = 1;
JonathanAshworth 6:8ffc6d3a7c1d 573 }
JonathanAshworth 6:8ffc6d3a7c1d 574
JonathanAshworth 6:8ffc6d3a7c1d 575 void parseRequestData(void) {
JonathanAshworth 5:d0b2b4d8b9ba 576
JonathanAshworth 6:8ffc6d3a7c1d 577 char i,j;
JonathanAshworth 6:8ffc6d3a7c1d 578
JonathanAshworth 6:8ffc6d3a7c1d 579 int bufferOffset = 0; // Tacking how far into the rx data we are
JonathanAshworth 6:8ffc6d3a7c1d 580 int lengthOfCurrentSentence = 0;
JonathanAshworth 6:8ffc6d3a7c1d 581
JonathanAshworth 6:8ffc6d3a7c1d 582 char CID;
JonathanAshworth 6:8ffc6d3a7c1d 583 char SID;
JonathanAshworth 6:8ffc6d3a7c1d 584 char fieldCount;
JonathanAshworth 6:8ffc6d3a7c1d 585
JonathanAshworth 6:8ffc6d3a7c1d 586 char requestedSensorIDs[PC_REQUESTEDSID_MAXCOUNT] = {0};
JonathanAshworth 6:8ffc6d3a7c1d 587
JonathanAshworth 6:8ffc6d3a7c1d 588 // Servo vars:
JonathanAshworth 6:8ffc6d3a7c1d 589 float channel;
JonathanAshworth 6:8ffc6d3a7c1d 590 char posArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 591 char spdArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 592 char timArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 593 float position;
JonathanAshworth 6:8ffc6d3a7c1d 594 float speed;
JonathanAshworth 6:8ffc6d3a7c1d 595 float time;
JonathanAshworth 6:8ffc6d3a7c1d 596
JonathanAshworth 6:8ffc6d3a7c1d 597 // Motor vars:
JonathanAshworth 6:8ffc6d3a7c1d 598 unsigned int pwm;
JonathanAshworth 6:8ffc6d3a7c1d 599 float demand;
JonathanAshworth 6:8ffc6d3a7c1d 600 char demArr[BYTELENGTH_OF_A_FLOAT];
JonathanAshworth 6:8ffc6d3a7c1d 601
JonathanAshworth 6:8ffc6d3a7c1d 602 //bool terminated;
JonathanAshworth 6:8ffc6d3a7c1d 603
JonathanAshworth 6:8ffc6d3a7c1d 604 /*
JonathanAshworth 6:8ffc6d3a7c1d 605 if(1) {
JonathanAshworth 6:8ffc6d3a7c1d 606 terminated = 0;
JonathanAshworth 6:8ffc6d3a7c1d 607 //GPSSerial.printf("Echo back...");
JonathanAshworth 6:8ffc6d3a7c1d 608 for (i=0; i<PC_RX_BUFFER_SIZE; i++) {
JonathanAshworth 6:8ffc6d3a7c1d 609 if ( (PCRxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) +
JonathanAshworth 6:8ffc6d3a7c1d 610 (PCRxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) +
JonathanAshworth 6:8ffc6d3a7c1d 611 (PCRxData[i-2]<<BITLENGTH_OF_A_BYTE) +
JonathanAshworth 6:8ffc6d3a7c1d 612 PCRxData[i-1] == PC_TERMINATION_PHRASE ) {
JonathanAshworth 6:8ffc6d3a7c1d 613 terminated = 1;
JonathanAshworth 6:8ffc6d3a7c1d 614 }
JonathanAshworth 6:8ffc6d3a7c1d 615 if (!terminated) {
JonathanAshworth 6:8ffc6d3a7c1d 616 GPSSerial.putc(PCRxData[i]);
JonathanAshworth 6:8ffc6d3a7c1d 617 }
JonathanAshworth 6:8ffc6d3a7c1d 618 }
JonathanAshworth 6:8ffc6d3a7c1d 619 //GPSSerial.printf("...Echo end\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 620 }
JonathanAshworth 6:8ffc6d3a7c1d 621 */
JonathanAshworth 6:8ffc6d3a7c1d 622
JonathanAshworth 6:8ffc6d3a7c1d 623 for (j=0; j<((char)PC_SENTENCE_COUNT); j++) {
JonathanAshworth 6:8ffc6d3a7c1d 624
JonathanAshworth 6:8ffc6d3a7c1d 625 // Read CID:
JonathanAshworth 6:8ffc6d3a7c1d 626 CID = PCRxData[bufferOffset+PC_CID_LOCATION];
JonathanAshworth 6:8ffc6d3a7c1d 627
JonathanAshworth 6:8ffc6d3a7c1d 628 // Read SID:
JonathanAshworth 6:8ffc6d3a7c1d 629 SID = PCRxData[bufferOffset+PC_SID_LOCATION];
JonathanAshworth 6:8ffc6d3a7c1d 630
JonathanAshworth 6:8ffc6d3a7c1d 631 // Read Field Count:
JonathanAshworth 6:8ffc6d3a7c1d 632 fieldCount = PCRxData[bufferOffset+PC_FIELDCOUNT_LOCATION];
JonathanAshworth 6:8ffc6d3a7c1d 633
JonathanAshworth 6:8ffc6d3a7c1d 634 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 635 GPSSerial.printf("Parsing data for SID: %d\r\n", SID);
JonathanAshworth 6:8ffc6d3a7c1d 636 GPSSerial.printf("Field count: %d\r\n", fieldCount);
JonathanAshworth 6:8ffc6d3a7c1d 637 }
JonathanAshworth 6:8ffc6d3a7c1d 638
JonathanAshworth 6:8ffc6d3a7c1d 639
JonathanAshworth 6:8ffc6d3a7c1d 640 if (CID != SID_PC)
JonathanAshworth 6:8ffc6d3a7c1d 641 break; // Stop as I'm only listening to the PC
JonathanAshworth 4:1017848d2fe1 642
JonathanAshworth 6:8ffc6d3a7c1d 643 if (SID == SID_PC)
JonathanAshworth 6:8ffc6d3a7c1d 644 break; // Stop as that message is for the PC
JonathanAshworth 6:8ffc6d3a7c1d 645
JonathanAshworth 6:8ffc6d3a7c1d 646 if (SID > SID_LASTMOTOR_LOCATION) {
JonathanAshworth 6:8ffc6d3a7c1d 647 flags.PCTimeout = 1;
JonathanAshworth 6:8ffc6d3a7c1d 648 led3 = 1;
JonathanAshworth 6:8ffc6d3a7c1d 649 break; // Stop as you cannot write to a sensor
JonathanAshworth 6:8ffc6d3a7c1d 650 }
JonathanAshworth 6:8ffc6d3a7c1d 651
JonathanAshworth 6:8ffc6d3a7c1d 652 if (SID == SID_MBED) { //Get ready to pump out sensor data:
JonathanAshworth 6:8ffc6d3a7c1d 653
JonathanAshworth 6:8ffc6d3a7c1d 654 if(DEBUG)
JonathanAshworth 6:8ffc6d3a7c1d 655 GPSSerial.printf("Sensor info requested\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 656
JonathanAshworth 6:8ffc6d3a7c1d 657 for (i=0; i<fieldCount; i++) { // fieldCount = the number of requested sensors
JonathanAshworth 6:8ffc6d3a7c1d 658
JonathanAshworth 6:8ffc6d3a7c1d 659 // Retrieve the SID of 'ith' requested sensor:
JonathanAshworth 6:8ffc6d3a7c1d 660 requestedSensorIDs[i] = PCRxData[bufferOffset+PC_REQUESTEDSID_BYTE0_LOCATION+i];
JonathanAshworth 6:8ffc6d3a7c1d 661 if(DEBUG)
JonathanAshworth 6:8ffc6d3a7c1d 662 GPSSerial.printf("Sensor requested: %d\r\n",requestedSensorIDs[i]);
JonathanAshworth 6:8ffc6d3a7c1d 663
JonathanAshworth 6:8ffc6d3a7c1d 664 }
JonathanAshworth 6:8ffc6d3a7c1d 665 // Send the SIDs to construct a response:
JonathanAshworth 6:8ffc6d3a7c1d 666 constructTxData(requestedSensorIDs, fieldCount);
JonathanAshworth 6:8ffc6d3a7c1d 667
JonathanAshworth 6:8ffc6d3a7c1d 668 // Length of this sentence is:
JonathanAshworth 6:8ffc6d3a7c1d 669 lengthOfCurrentSentence = fieldCount +
JonathanAshworth 6:8ffc6d3a7c1d 670 PC_CID_LENGTH +
JonathanAshworth 6:8ffc6d3a7c1d 671 PC_SID_LENGTH +
JonathanAshworth 6:8ffc6d3a7c1d 672 PC_FIELDCOUNT_LENGTH;
JonathanAshworth 5:d0b2b4d8b9ba 673 }
JonathanAshworth 6:8ffc6d3a7c1d 674
JonathanAshworth 6:8ffc6d3a7c1d 675 if ((SID >= SID_FIRSTSERVO_LOCATION)&&
JonathanAshworth 6:8ffc6d3a7c1d 676 (SID <= SID_LASTSERVO_LOCATION)) { // We're writing to a servo
JonathanAshworth 6:8ffc6d3a7c1d 677
JonathanAshworth 6:8ffc6d3a7c1d 678 if(DEBUG)
JonathanAshworth 6:8ffc6d3a7c1d 679 GPSSerial.printf("Servo request\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 680
JonathanAshworth 6:8ffc6d3a7c1d 681 // Retrieve byte values representing the position, speed and time floats:
JonathanAshworth 6:8ffc6d3a7c1d 682 for (i=0; i<BYTELENGTH_OF_A_FLOAT; i++) {
JonathanAshworth 6:8ffc6d3a7c1d 683 posArr[i] = PCRxData[(bufferOffset)+PC_FLOAT0_LOCATION+i]; // Float Byte 3, 4, 5 & 6
JonathanAshworth 6:8ffc6d3a7c1d 684 spdArr[i] = PCRxData[(bufferOffset)+PC_FLOAT1_LOCATION+i]; // Float Byte 7, 8, 9 & 10
JonathanAshworth 6:8ffc6d3a7c1d 685 timArr[i] = PCRxData[(bufferOffset)+PC_FLOAT2_LOCATION+i]; // Float Byte 11, 12, 13 & 14
JonathanAshworth 6:8ffc6d3a7c1d 686 }
JonathanAshworth 6:8ffc6d3a7c1d 687
JonathanAshworth 6:8ffc6d3a7c1d 688 // Cast byte arrays into float values:
JonathanAshworth 6:8ffc6d3a7c1d 689 position = *(float*)(posArr);
JonathanAshworth 6:8ffc6d3a7c1d 690 speed = *(float*)(spdArr);
JonathanAshworth 6:8ffc6d3a7c1d 691 time = *(float*)(timArr);
JonathanAshworth 6:8ffc6d3a7c1d 692
JonathanAshworth 6:8ffc6d3a7c1d 693 if (DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 694 GPSSerial.printf("Position: %f\r\n", position);
JonathanAshworth 6:8ffc6d3a7c1d 695 GPSSerial.printf("Speed: %f\r\n", speed);
JonathanAshworth 6:8ffc6d3a7c1d 696 GPSSerial.printf("Time: %f\r\n", time);
JonathanAshworth 6:8ffc6d3a7c1d 697 }
JonathanAshworth 6:8ffc6d3a7c1d 698
JonathanAshworth 6:8ffc6d3a7c1d 699 // Find the channel associated with the desired servo:
JonathanAshworth 6:8ffc6d3a7c1d 700 botData.getVal(SID, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 701
JonathanAshworth 6:8ffc6d3a7c1d 702 // Validate the position request (ensure it is within safe limits):
JonathanAshworth 6:8ffc6d3a7c1d 703 if ((position >= (float)SERVO_POSITION_MIN)&&
JonathanAshworth 6:8ffc6d3a7c1d 704 (position <= (float)SERVO_POSITION_MAX)) {
JonathanAshworth 6:8ffc6d3a7c1d 705
JonathanAshworth 6:8ffc6d3a7c1d 706 // If no speed or time was specified:
JonathanAshworth 6:8ffc6d3a7c1d 707 if ((speed == 0.0f)&&(time == 0.0f)) {
JonathanAshworth 6:8ffc6d3a7c1d 708 SSC32Serial.printf("#%d P%d\r", (char)channel, (int)position);
JonathanAshworth 6:8ffc6d3a7c1d 709 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 710 GPSSerial.printf("writing to servo... ");
JonathanAshworth 6:8ffc6d3a7c1d 711 GPSSerial.printf("#%d P%d\r\n", (char)channel, (int)position);
JonathanAshworth 6:8ffc6d3a7c1d 712 }
JonathanAshworth 6:8ffc6d3a7c1d 713 }
JonathanAshworth 6:8ffc6d3a7c1d 714 // If a speed or time were specified:
JonathanAshworth 6:8ffc6d3a7c1d 715 else {
JonathanAshworth 6:8ffc6d3a7c1d 716 if (speed > 0.0f) { // Speed was specified:
JonathanAshworth 6:8ffc6d3a7c1d 717 SSC32Serial.printf("#%d P%d S%d\r", (char)channel, (int)position, (int)speed);
JonathanAshworth 6:8ffc6d3a7c1d 718 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 719 GPSSerial.printf("writing to servo... ");
JonathanAshworth 6:8ffc6d3a7c1d 720 GPSSerial.printf("#%d P%d S%d\r\n", (char)channel, (int)position, (int)speed);
JonathanAshworth 6:8ffc6d3a7c1d 721 }
JonathanAshworth 6:8ffc6d3a7c1d 722 }
JonathanAshworth 6:8ffc6d3a7c1d 723 if (time > 0.0f) { // Time was specified:
JonathanAshworth 6:8ffc6d3a7c1d 724 SSC32Serial.printf("#%d P%d T%d\r", (char)channel, (int)position, (int)time);
JonathanAshworth 6:8ffc6d3a7c1d 725 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 726 GPSSerial.printf("writing to servo... ");
JonathanAshworth 6:8ffc6d3a7c1d 727 GPSSerial.printf("#%d P%d T%d\r\n", (char)channel, (int)position, (int)time);
JonathanAshworth 6:8ffc6d3a7c1d 728 }
JonathanAshworth 6:8ffc6d3a7c1d 729 }
JonathanAshworth 6:8ffc6d3a7c1d 730 }
JonathanAshworth 6:8ffc6d3a7c1d 731
JonathanAshworth 6:8ffc6d3a7c1d 732 // Update botData with new values:
JonathanAshworth 6:8ffc6d3a7c1d 733 botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_POSITION, &position);
JonathanAshworth 6:8ffc6d3a7c1d 734 botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_SPEED, &speed);
JonathanAshworth 6:8ffc6d3a7c1d 735 botData.setVal(SID, BOTDATA_FIELDNUMBER_SERVO_TIME, &time);
JonathanAshworth 6:8ffc6d3a7c1d 736
JonathanAshworth 6:8ffc6d3a7c1d 737 }
JonathanAshworth 6:8ffc6d3a7c1d 738 else {
JonathanAshworth 6:8ffc6d3a7c1d 739 //led4 = 1; // Indicate an invalid data warning (position outside limits)
JonathanAshworth 6:8ffc6d3a7c1d 740 }
JonathanAshworth 6:8ffc6d3a7c1d 741 lengthOfCurrentSentence = PC_SERVO_REQUEST_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 742 }
JonathanAshworth 6:8ffc6d3a7c1d 743
JonathanAshworth 6:8ffc6d3a7c1d 744 if ((SID >= SID_FIRSTMOTOR_LOCATION)&&
JonathanAshworth 6:8ffc6d3a7c1d 745 (SID <= SID_LASTMOTOR_LOCATION)) { // We're writing to a drive motor ESC
JonathanAshworth 6:8ffc6d3a7c1d 746
JonathanAshworth 6:8ffc6d3a7c1d 747 //led4 = 0;
JonathanAshworth 6:8ffc6d3a7c1d 748
JonathanAshworth 6:8ffc6d3a7c1d 749 // Retrieve byte values representing the demand float:
JonathanAshworth 6:8ffc6d3a7c1d 750 demArr[0] = PCRxData[bufferOffset+PC_FLOAT0_LOCATION]; // Float Byte 0
JonathanAshworth 6:8ffc6d3a7c1d 751 demArr[1] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+1)]; // Float Byte 1
JonathanAshworth 6:8ffc6d3a7c1d 752 demArr[2] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+2)]; // Float Byte 2
JonathanAshworth 6:8ffc6d3a7c1d 753 demArr[3] = PCRxData[bufferOffset+(PC_FLOAT0_LOCATION+3)]; // Float Byte 3
JonathanAshworth 6:8ffc6d3a7c1d 754 demand = *(float*)(demArr); // Cast byte array into a float value
JonathanAshworth 6:8ffc6d3a7c1d 755
JonathanAshworth 6:8ffc6d3a7c1d 756 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 757 GPSSerial.printf("Motor request...\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 758 GPSSerial.printf("Demand: %f\r\n",demand);
JonathanAshworth 6:8ffc6d3a7c1d 759 }
JonathanAshworth 6:8ffc6d3a7c1d 760
JonathanAshworth 6:8ffc6d3a7c1d 761 if ((demand >= -1.0f)&&
JonathanAshworth 6:8ffc6d3a7c1d 762 (demand <= 1.0f)) { // Ensure demand value is valid
JonathanAshworth 6:8ffc6d3a7c1d 763
JonathanAshworth 6:8ffc6d3a7c1d 764 pwm = translateMotorDemandToPWM(demand); // Translate demand into a pwm value
JonathanAshworth 6:8ffc6d3a7c1d 765
JonathanAshworth 6:8ffc6d3a7c1d 766 if(DEBUG)
JonathanAshworth 6:8ffc6d3a7c1d 767 GPSSerial.printf("Pulsewidth (us): %d\r\n",pwm);
JonathanAshworth 6:8ffc6d3a7c1d 768
JonathanAshworth 6:8ffc6d3a7c1d 769 // If request is for the nearside drive motor ESC:
JonathanAshworth 6:8ffc6d3a7c1d 770 if (SID == (char)SID_DRIVE_NS) {
JonathanAshworth 6:8ffc6d3a7c1d 771 driveNS.pulsewidth_us(pwm); // Write to nearside drive motor ESC
JonathanAshworth 6:8ffc6d3a7c1d 772
JonathanAshworth 6:8ffc6d3a7c1d 773 // Update bot's DB with new demand value:
JonathanAshworth 6:8ffc6d3a7c1d 774 botData.setVal( SID, BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
JonathanAshworth 6:8ffc6d3a7c1d 775
JonathanAshworth 6:8ffc6d3a7c1d 776 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 777 GPSSerial.printf("Writing to NS motor (pwm value): ");
JonathanAshworth 6:8ffc6d3a7c1d 778 GPSSerial.printf("%d \r\n", pwm);
JonathanAshworth 6:8ffc6d3a7c1d 779 }
JonathanAshworth 6:8ffc6d3a7c1d 780
JonathanAshworth 6:8ffc6d3a7c1d 781 }
JonathanAshworth 6:8ffc6d3a7c1d 782
JonathanAshworth 6:8ffc6d3a7c1d 783 // If request is for the offside drive motor ESC:
JonathanAshworth 6:8ffc6d3a7c1d 784 if (SID == (char)SID_DRIVE_OS) {
JonathanAshworth 6:8ffc6d3a7c1d 785 driveOS.pulsewidth_us(pwm); // Wite to offside drive motor ESC
JonathanAshworth 6:8ffc6d3a7c1d 786
JonathanAshworth 6:8ffc6d3a7c1d 787 // Update bot's DB with new demand value:
JonathanAshworth 6:8ffc6d3a7c1d 788 botData.setVal(SID, BOTDATA_FIELDNUMBER_DRIVE_DEMAND, &demand);
JonathanAshworth 6:8ffc6d3a7c1d 789
JonathanAshworth 6:8ffc6d3a7c1d 790 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 791 GPSSerial.printf("Writing to OS motor (pwm value): ");
JonathanAshworth 6:8ffc6d3a7c1d 792 GPSSerial.printf("%d \r\n", pwm);
JonathanAshworth 6:8ffc6d3a7c1d 793 }
JonathanAshworth 6:8ffc6d3a7c1d 794
JonathanAshworth 6:8ffc6d3a7c1d 795 }
JonathanAshworth 6:8ffc6d3a7c1d 796 }
JonathanAshworth 6:8ffc6d3a7c1d 797 else {
JonathanAshworth 6:8ffc6d3a7c1d 798 //led4 = 1; // Indicate an invalid data warning (demand outside limits)
JonathanAshworth 6:8ffc6d3a7c1d 799 }
JonathanAshworth 6:8ffc6d3a7c1d 800 lengthOfCurrentSentence = PC_MOTOR_REQUEST_LENGTH;
JonathanAshworth 6:8ffc6d3a7c1d 801 }
JonathanAshworth 6:8ffc6d3a7c1d 802
JonathanAshworth 6:8ffc6d3a7c1d 803 // Update how far into the rx data we are using the current sentence length:
JonathanAshworth 6:8ffc6d3a7c1d 804 bufferOffset = bufferOffset + lengthOfCurrentSentence;
JonathanAshworth 5:d0b2b4d8b9ba 805 }
JonathanAshworth 4:1017848d2fe1 806
JonathanAshworth 6:8ffc6d3a7c1d 807 }
JonathanAshworth 6:8ffc6d3a7c1d 808
JonathanAshworth 6:8ffc6d3a7c1d 809 unsigned int translateMotorDemandToPWM(float demand) {
JonathanAshworth 6:8ffc6d3a7c1d 810 /* Valid demand values: -1.0 upto 1.0 From protocol definition
JonathanAshworth 6:8ffc6d3a7c1d 811 Desired pwm values: 0 upto 1.0 From ESC operation requirements
JonathanAshworth 6:8ffc6d3a7c1d 812
JonathanAshworth 6:8ffc6d3a7c1d 813 The values resolve as follow:
JonathanAshworth 6:8ffc6d3a7c1d 814 Demand: -1.0 | 0 | 1.0
JonathanAshworth 6:8ffc6d3a7c1d 815 M/S Ratio: 0 | 1 | 2.0
JonathanAshworth 6:8ffc6d3a7c1d 816 PWM: 700 | 1300 | 2000
JonathanAshworth 6:8ffc6d3a7c1d 817 Meaning: MaxReverse | Neutral | MaxForward
JonathanAshworth 6:8ffc6d3a7c1d 818 */
JonathanAshworth 6:8ffc6d3a7c1d 819 float pwm = 0;
JonathanAshworth 6:8ffc6d3a7c1d 820 pwm = demand+1;
JonathanAshworth 6:8ffc6d3a7c1d 821 pwm = (625.0f*pwm)+700.0f;
JonathanAshworth 6:8ffc6d3a7c1d 822 return (unsigned int)pwm;
JonathanAshworth 0:2676c97df44e 823 }
JonathanAshworth 0:2676c97df44e 824
JonathanAshworth 4:1017848d2fe1 825 void PCTimeout(void) {
JonathanAshworth 6:8ffc6d3a7c1d 826
JonathanAshworth 6:8ffc6d3a7c1d 827 // Try telling the PC comms have been lost:
JonathanAshworth 6:8ffc6d3a7c1d 828 //PCSerial.printf(PC_TIMEOUT_MESSAGE);
JonathanAshworth 6:8ffc6d3a7c1d 829 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 830 GPSSerial.printf(PC_TIMEOUT_MESSAGE);
JonathanAshworth 6:8ffc6d3a7c1d 831 }
JonathanAshworth 6:8ffc6d3a7c1d 832
JonathanAshworth 6:8ffc6d3a7c1d 833 //led1 = 1; // Indicate the MBED has timedout
JonathanAshworth 6:8ffc6d3a7c1d 834
JonathanAshworth 6:8ffc6d3a7c1d 835 shutdown(); // Ensure the bot is in a safe shutdown mode/position
JonathanAshworth 6:8ffc6d3a7c1d 836
JonathanAshworth 6:8ffc6d3a7c1d 837 // Wait for PC_TIMEOUT_MESSAGE to be sent before disabling the ISRs
JonathanAshworth 6:8ffc6d3a7c1d 838 while(!SSC32Serial.txBufferEmpty()) {;}
JonathanAshworth 6:8ffc6d3a7c1d 839
JonathanAshworth 6:8ffc6d3a7c1d 840 // Prevent any ISRs from firing:
JonathanAshworth 6:8ffc6d3a7c1d 841 // (important this comes after the while else the timeout message won't send):
JonathanAshworth 6:8ffc6d3a7c1d 842 __disable_irq(); // Should disable all ISRs
JonathanAshworth 6:8ffc6d3a7c1d 843 NVIC_DisableIRQ(UART0_IRQn); // Directly turn off USB (PC)'s DMA ISRs
JonathanAshworth 6:8ffc6d3a7c1d 844
JonathanAshworth 6:8ffc6d3a7c1d 845 while(1) {;} // Require a hard reset to resume
JonathanAshworth 4:1017848d2fe1 846 }
JonathanAshworth 0:2676c97df44e 847
JonathanAshworth 6:8ffc6d3a7c1d 848 void shutdown(void) {
JonathanAshworth 6:8ffc6d3a7c1d 849
JonathanAshworth 6:8ffc6d3a7c1d 850 float channel; // Temporary storage for locating servo channels
JonathanAshworth 6:8ffc6d3a7c1d 851
JonathanAshworth 6:8ffc6d3a7c1d 852 // Set drive motor ESCs to neutral:
JonathanAshworth 6:8ffc6d3a7c1d 853 driveNS.pulsewidth_us(translateMotorDemandToPWM(DRIVE_DEMAND_NEUTRAL)); // Nearside
JonathanAshworth 6:8ffc6d3a7c1d 854 driveOS.pulsewidth_us(translateMotorDemandToPWM(DRIVE_DEMAND_NEUTRAL)); // Offside
JonathanAshworth 6:8ffc6d3a7c1d 855
JonathanAshworth 6:8ffc6d3a7c1d 856 SSC32Serial.printf(SSC32_STOP); // Stop previous servo command
JonathanAshworth 6:8ffc6d3a7c1d 857
JonathanAshworth 6:8ffc6d3a7c1d 858 // Find the channel for the arm's base servo:
JonathanAshworth 6:8ffc6d3a7c1d 859 botData.getVal(SID_ARM_BASE, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 860 // Set the base in a 'safe' position:
JonathanAshworth 6:8ffc6d3a7c1d 861 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 862 (int)SSC32_STOPPOSITION_ARM_BASE,
JonathanAshworth 6:8ffc6d3a7c1d 863 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 6:8ffc6d3a7c1d 864
JonathanAshworth 6:8ffc6d3a7c1d 865 // Find the channel for the arm's shoulder servo:
JonathanAshworth 6:8ffc6d3a7c1d 866 botData.getVal(SID_ARM_SHOULDER, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 867 // Set the shoulder in a 'safe' position:
JonathanAshworth 6:8ffc6d3a7c1d 868 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 869 (int)SSC32_STOPPOSITION_ARM_SHOULDER,
JonathanAshworth 6:8ffc6d3a7c1d 870 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 6:8ffc6d3a7c1d 871
JonathanAshworth 6:8ffc6d3a7c1d 872 // Find the channel for the arm's elbow servo:
JonathanAshworth 6:8ffc6d3a7c1d 873 botData.getVal(SID_ARM_ELBOW, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 874 // Set the elbow in a 'safe' position:
JonathanAshworth 6:8ffc6d3a7c1d 875 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 876 (int)SSC32_STOPPOSITION_ARM_ELBOW,
JonathanAshworth 6:8ffc6d3a7c1d 877 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 6:8ffc6d3a7c1d 878
JonathanAshworth 6:8ffc6d3a7c1d 879 // Find the channel for the arm's wrist servo:
JonathanAshworth 6:8ffc6d3a7c1d 880 botData.getVal(SID_ARM_WRIST, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 881 // Set the wrist in a 'safe' position:
JonathanAshworth 6:8ffc6d3a7c1d 882 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 883 (int)SSC32_STOPPOSITION_ARM_WRIST,
JonathanAshworth 6:8ffc6d3a7c1d 884 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 6:8ffc6d3a7c1d 885
JonathanAshworth 6:8ffc6d3a7c1d 886 // Find the channel for the arm's manipulator servo:
JonathanAshworth 6:8ffc6d3a7c1d 887 botData.getVal(SID_ARM_MANIPULATOR, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 888 // Set the manipulator in a 'safe' position:
JonathanAshworth 6:8ffc6d3a7c1d 889 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 890 (int)SSC32_STOPPOSITION_ARM_MANIPULATOR,
JonathanAshworth 6:8ffc6d3a7c1d 891 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 6:8ffc6d3a7c1d 892
JonathanAshworth 6:8ffc6d3a7c1d 893 // Find the channel for the visions's pitch servo:
JonathanAshworth 6:8ffc6d3a7c1d 894 botData.getVal(SID_VISION_PITCH, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 895 // Set the pitch in a neutral position:
JonathanAshworth 6:8ffc6d3a7c1d 896 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 897 (int)SSC32_STOPPOSITION_VISION_PITCH,
JonathanAshworth 6:8ffc6d3a7c1d 898 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 6:8ffc6d3a7c1d 899
JonathanAshworth 6:8ffc6d3a7c1d 900 // Find the channel for the visions's yaw servo:
JonathanAshworth 6:8ffc6d3a7c1d 901 botData.getVal(SID_VISION_YAW, BOTDATA_FIELDNUMBER_SERVO_CHANNEL, &channel);
JonathanAshworth 6:8ffc6d3a7c1d 902 // Set the yaw in a neutral position:
JonathanAshworth 6:8ffc6d3a7c1d 903 SSC32Serial.printf("#%d P%d T%d \r", (char)channel,
JonathanAshworth 6:8ffc6d3a7c1d 904 (int)SSC32_STOPPOSITION_VISION_YAW,
JonathanAshworth 6:8ffc6d3a7c1d 905 (int)SSC32_STOPPOSITION_TIME);
JonathanAshworth 4:1017848d2fe1 906 }
JonathanAshworth 4:1017848d2fe1 907
JonathanAshworth 4:1017848d2fe1 908 void dmaPCSerialRx(MODSERIAL_IRQ_INFO *q) {
JonathanAshworth 4:1017848d2fe1 909
JonathanAshworth 4:1017848d2fe1 910 MODSERIAL *serial = q->serial; //Define local serial class
JonathanAshworth 6:8ffc6d3a7c1d 911
JonathanAshworth 6:8ffc6d3a7c1d 912 if(flags.PCBegin) {
JonathanAshworth 6:8ffc6d3a7c1d 913
JonathanAshworth 6:8ffc6d3a7c1d 914 if (flags.txSentData == 1) {
JonathanAshworth 6:8ffc6d3a7c1d 915 PCTimeoutTimer.stop();
JonathanAshworth 6:8ffc6d3a7c1d 916 PCTimeoutTimer.reset();
JonathanAshworth 6:8ffc6d3a7c1d 917 flags.txSentData = 0;
JonathanAshworth 6:8ffc6d3a7c1d 918 }
JonathanAshworth 6:8ffc6d3a7c1d 919 PCTimeoutTimer.start();
JonathanAshworth 6:8ffc6d3a7c1d 920
JonathanAshworth 6:8ffc6d3a7c1d 921 if (PCTimeoutTimer.read_ms() < PC_TIMEOUT_PERIOD) {
JonathanAshworth 6:8ffc6d3a7c1d 922 incommingPCRxData[incommingPCRxDataCount] = serial->getc();
JonathanAshworth 6:8ffc6d3a7c1d 923 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 924 GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]);
JonathanAshworth 6:8ffc6d3a7c1d 925 }
JonathanAshworth 6:8ffc6d3a7c1d 926 if (incommingPCRxDataCount > 2) {
JonathanAshworth 6:8ffc6d3a7c1d 927 if ((incommingPCRxData[incommingPCRxDataCount-3]<<(BITLENGTH_OF_A_BYTE*3)) +
JonathanAshworth 6:8ffc6d3a7c1d 928 (incommingPCRxData[incommingPCRxDataCount-2]<<(BITLENGTH_OF_A_BYTE*2)) +
JonathanAshworth 6:8ffc6d3a7c1d 929 (incommingPCRxData[incommingPCRxDataCount-1]<<BITLENGTH_OF_A_BYTE) +
JonathanAshworth 6:8ffc6d3a7c1d 930 (incommingPCRxData[incommingPCRxDataCount]) == PC_TERMINATION_PHRASE) {
JonathanAshworth 6:8ffc6d3a7c1d 931 if (flags.rxNewData) {
JonathanAshworth 6:8ffc6d3a7c1d 932 flags.PCTimeout = 1;
JonathanAshworth 6:8ffc6d3a7c1d 933 led4 = 1;
JonathanAshworth 6:8ffc6d3a7c1d 934 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 935 GPSSerial.printf("Received new data too soon\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 936 }
JonathanAshworth 6:8ffc6d3a7c1d 937 PCTimeoutTimer.stop();
JonathanAshworth 6:8ffc6d3a7c1d 938 PCTimeoutTimer.reset();
JonathanAshworth 6:8ffc6d3a7c1d 939 }
JonathanAshworth 6:8ffc6d3a7c1d 940 else {
JonathanAshworth 6:8ffc6d3a7c1d 941 memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 6:8ffc6d3a7c1d 942 memcpy(PCRxData, incommingPCRxData, PC_RX_BUFFER_SIZE); //PCRxData = incommingPCRxData;
JonathanAshworth 6:8ffc6d3a7c1d 943 memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 6:8ffc6d3a7c1d 944 incommingPCRxDataCount = 0;
JonathanAshworth 6:8ffc6d3a7c1d 945 flags.rxNewData = 1;
JonathanAshworth 6:8ffc6d3a7c1d 946 PCTimeoutTimer.stop();
JonathanAshworth 6:8ffc6d3a7c1d 947 PCTimeoutTimer.reset();
JonathanAshworth 6:8ffc6d3a7c1d 948 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 949 GPSSerial.printf("\nReceived a termination\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 950 }
JonathanAshworth 6:8ffc6d3a7c1d 951 }
JonathanAshworth 6:8ffc6d3a7c1d 952 }
JonathanAshworth 6:8ffc6d3a7c1d 953 else {
JonathanAshworth 6:8ffc6d3a7c1d 954 incommingPCRxDataCount++;
JonathanAshworth 6:8ffc6d3a7c1d 955 }
JonathanAshworth 6:8ffc6d3a7c1d 956 }
JonathanAshworth 6:8ffc6d3a7c1d 957 else {
JonathanAshworth 6:8ffc6d3a7c1d 958 incommingPCRxDataCount++;
JonathanAshworth 6:8ffc6d3a7c1d 959 }
JonathanAshworth 6:8ffc6d3a7c1d 960 }
JonathanAshworth 6:8ffc6d3a7c1d 961 else {
JonathanAshworth 6:8ffc6d3a7c1d 962 flags.PCTimeout = 1;
JonathanAshworth 6:8ffc6d3a7c1d 963 led2 = 1;
JonathanAshworth 6:8ffc6d3a7c1d 964 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 965 GPSSerial.printf("Rx timeout\r\n");
JonathanAshworth 6:8ffc6d3a7c1d 966 }
JonathanAshworth 6:8ffc6d3a7c1d 967 PCTimeoutTimer.stop();
JonathanAshworth 6:8ffc6d3a7c1d 968 PCTimeoutTimer.reset();
JonathanAshworth 6:8ffc6d3a7c1d 969 }
JonathanAshworth 4:1017848d2fe1 970 }
JonathanAshworth 6:8ffc6d3a7c1d 971 else {
JonathanAshworth 4:1017848d2fe1 972 incommingPCRxData[incommingPCRxDataCount] = serial->getc();
JonathanAshworth 6:8ffc6d3a7c1d 973 /*
JonathanAshworth 6:8ffc6d3a7c1d 974 unsigned int lengthOfPhrase = 9;//sizeof(PC_BEGIN_PHRASE);
JonathanAshworth 6:8ffc6d3a7c1d 975 if (incommingPCRxDataCount >= lengthOfPhrase) {
JonathanAshworth 6:8ffc6d3a7c1d 976 if (string_compare(&PC_BEGIN_PHRASE, &(incommingPCRxData[incommingPCRxDataCount-(lengthOfPhrase-1)]), lengthOfPhrase)) {
JonathanAshworth 6:8ffc6d3a7c1d 977 flags.PCBegin = 1;
JonathanAshworth 6:8ffc6d3a7c1d 978 memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 6:8ffc6d3a7c1d 979 incommingPCRxDataCount = 0;
JonathanAshworth 6:8ffc6d3a7c1d 980 }
JonathanAshworth 6:8ffc6d3a7c1d 981 else {
JonathanAshworth 6:8ffc6d3a7c1d 982 incommingPCRxDataCount++;
JonathanAshworth 6:8ffc6d3a7c1d 983 }
JonathanAshworth 6:8ffc6d3a7c1d 984 }
JonathanAshworth 6:8ffc6d3a7c1d 985 else {
JonathanAshworth 6:8ffc6d3a7c1d 986 incommingPCRxDataCount++;
JonathanAshworth 6:8ffc6d3a7c1d 987 }
JonathanAshworth 6:8ffc6d3a7c1d 988 */
JonathanAshworth 6:8ffc6d3a7c1d 989
JonathanAshworth 6:8ffc6d3a7c1d 990 if (incommingPCRxDataCount > 7) {
JonathanAshworth 6:8ffc6d3a7c1d 991 if ( (incommingPCRxData[incommingPCRxDataCount-8] == 'I') &&
JonathanAshworth 6:8ffc6d3a7c1d 992 (incommingPCRxData[incommingPCRxDataCount-7] == 'R') &&
JonathanAshworth 6:8ffc6d3a7c1d 993 (incommingPCRxData[incommingPCRxDataCount-6] == 'I') &&
JonathanAshworth 6:8ffc6d3a7c1d 994 (incommingPCRxData[incommingPCRxDataCount-5] == 'S') &&
JonathanAshworth 6:8ffc6d3a7c1d 995 (incommingPCRxData[incommingPCRxDataCount-4] == ':') &&
JonathanAshworth 6:8ffc6d3a7c1d 996 (incommingPCRxData[incommingPCRxDataCount-3] == 'I') &&
JonathanAshworth 6:8ffc6d3a7c1d 997 (incommingPCRxData[incommingPCRxDataCount-2] == 'N') &&
JonathanAshworth 6:8ffc6d3a7c1d 998 (incommingPCRxData[incommingPCRxDataCount-1] == 'I') &&
JonathanAshworth 6:8ffc6d3a7c1d 999 (incommingPCRxData[incommingPCRxDataCount] == 'T') ) {
JonathanAshworth 6:8ffc6d3a7c1d 1000 flags.PCBegin = 1;
JonathanAshworth 4:1017848d2fe1 1001 memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 4:1017848d2fe1 1002 incommingPCRxDataCount = 0;
JonathanAshworth 0:2676c97df44e 1003 }
JonathanAshworth 4:1017848d2fe1 1004 else {
JonathanAshworth 4:1017848d2fe1 1005 incommingPCRxDataCount++;
JonathanAshworth 1:ccecc816c5e8 1006 }
JonathanAshworth 0:2676c97df44e 1007 }
JonathanAshworth 0:2676c97df44e 1008 else {
JonathanAshworth 4:1017848d2fe1 1009 incommingPCRxDataCount++;
JonathanAshworth 0:2676c97df44e 1010 }
JonathanAshworth 0:2676c97df44e 1011 }
JonathanAshworth 0:2676c97df44e 1012 }
JonathanAshworth 0:2676c97df44e 1013
JonathanAshworth 6:8ffc6d3a7c1d 1014 bool string_compare(char *a, char *b, unsigned int length) {
JonathanAshworth 6:8ffc6d3a7c1d 1015 for (unsigned int i=0; i<length; i++) {
JonathanAshworth 6:8ffc6d3a7c1d 1016 if (*a != *b) {
JonathanAshworth 6:8ffc6d3a7c1d 1017 return 0;
JonathanAshworth 6:8ffc6d3a7c1d 1018 }
JonathanAshworth 6:8ffc6d3a7c1d 1019 }
JonathanAshworth 6:8ffc6d3a7c1d 1020 return 1;
JonathanAshworth 6:8ffc6d3a7c1d 1021 }
JonathanAshworth 6:8ffc6d3a7c1d 1022
JonathanAshworth 6:8ffc6d3a7c1d 1023 void sendPCSerialData(void) {
JonathanAshworth 6:8ffc6d3a7c1d 1024 int i;
JonathanAshworth 5:d0b2b4d8b9ba 1025 bool terminated = 0;
JonathanAshworth 6:8ffc6d3a7c1d 1026 for (i=0; i<PC_TX_BUFFER_SIZE; i++) {
JonathanAshworth 6:8ffc6d3a7c1d 1027
JonathanAshworth 6:8ffc6d3a7c1d 1028 if ( (PCTxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) +
JonathanAshworth 6:8ffc6d3a7c1d 1029 (PCTxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) +
JonathanAshworth 6:8ffc6d3a7c1d 1030 (PCTxData[i-2]<<BITLENGTH_OF_A_BYTE) +
JonathanAshworth 6:8ffc6d3a7c1d 1031 PCTxData[i-1] == PC_TERMINATION_PHRASE ) {
JonathanAshworth 5:d0b2b4d8b9ba 1032 terminated = 1;
JonathanAshworth 5:d0b2b4d8b9ba 1033 }
JonathanAshworth 5:d0b2b4d8b9ba 1034 if (!terminated) {
JonathanAshworth 6:8ffc6d3a7c1d 1035 PCSerial.putc(PCTxData[i]);
JonathanAshworth 5:d0b2b4d8b9ba 1036 }
JonathanAshworth 5:d0b2b4d8b9ba 1037 }
JonathanAshworth 6:8ffc6d3a7c1d 1038 if(DEBUG) {
JonathanAshworth 6:8ffc6d3a7c1d 1039 terminated = 0;
JonathanAshworth 6:8ffc6d3a7c1d 1040 GPSSerial.printf("Response back...");
JonathanAshworth 6:8ffc6d3a7c1d 1041 for (i=0; i<PC_TX_BUFFER_SIZE; i++) {
JonathanAshworth 6:8ffc6d3a7c1d 1042 if ( (PCTxData[i-4]<<(BITLENGTH_OF_A_BYTE*3)) +
JonathanAshworth 6:8ffc6d3a7c1d 1043 (PCTxData[i-3]<<(BITLENGTH_OF_A_BYTE*2)) +
JonathanAshworth 6:8ffc6d3a7c1d 1044 (PCTxData[i-2]<<BITLENGTH_OF_A_BYTE) +
JonathanAshworth 6:8ffc6d3a7c1d 1045 PCTxData[i-1] == PC_TERMINATION_PHRASE ) {
JonathanAshworth 6:8ffc6d3a7c1d 1046 terminated = 1;
JonathanAshworth 6:8ffc6d3a7c1d 1047 }
JonathanAshworth 6:8ffc6d3a7c1d 1048 if (!terminated) {
JonathanAshworth 6:8ffc6d3a7c1d 1049 GPSSerial.putc(PCTxData[i]);
JonathanAshworth 6:8ffc6d3a7c1d 1050 }
JonathanAshworth 5:d0b2b4d8b9ba 1051 }
JonathanAshworth 6:8ffc6d3a7c1d 1052 GPSSerial.printf("...Response end\r\n");
JonathanAshworth 5:d0b2b4d8b9ba 1053 }
JonathanAshworth 4:1017848d2fe1 1054 PCTimeoutTimer.stop(); // precautionary - done after receiving
JonathanAshworth 4:1017848d2fe1 1055 PCTimeoutTimer.reset(); // precautionary - done after receiving
JonathanAshworth 4:1017848d2fe1 1056 flags.txSentData = 1;
JonathanAshworth 4:1017848d2fe1 1057 PCTimeoutTimer.start();
JonathanAshworth 0:2676c97df44e 1058 }
JonathanAshworth 4:1017848d2fe1 1059
JonathanAshworth 4:1017848d2fe1 1060 void dmaSSC32SerialRx(MODSERIAL_IRQ_INFO *q) {
JonathanAshworth 6:8ffc6d3a7c1d 1061 MODSERIAL *serial = q->serial; //Define local serial class
JonathanAshworth 6:8ffc6d3a7c1d 1062 servoPositionReturn = serial->getc();
JonathanAshworth 4:1017848d2fe1 1063 }
JonathanAshworth 4:1017848d2fe1 1064
JonathanAshworth 4:1017848d2fe1 1065 void dmaGPSSerialRx(MODSERIAL_IRQ_INFO *q) {
JonathanAshworth 4:1017848d2fe1 1066
JonathanAshworth 4:1017848d2fe1 1067 }