test
Dependencies: MODDMA MODSERIAL mbed
Fork of IRIS_MBED by
main.cpp@6:8ffc6d3a7c1d, 2015-04-09 (annotated)
- Committer:
- JonathanAshworth
- Date:
- Thu Apr 09 19:54:08 2015 +0000
- Revision:
- 6:8ffc6d3a7c1d
- Parent:
- 5:d0b2b4d8b9ba
Latest update. 99% working comms. Dummy data as sensor values - value will increase every time you request it then loop back to 0 from 255.
Who changed what in which revision?
User | Revision | Line number | New 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, ¤t); |
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, ¤t); |
JonathanAshworth | 6:8ffc6d3a7c1d | 436 | } |
JonathanAshworth | 6:8ffc6d3a7c1d | 437 | memcpy(currentArr, ¤t, 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 | } |