Adam Wilcock / Mbed 2 deprecated IRIS_MBED

test

Dependencies:   MODDMA MODSERIAL mbed

Fork of IRIS_MBED by IRIS

main.cpp@5:d0b2b4d8b9ba, 2015-03-26 (annotated)

Committer:
JonathanAshworth
Date:
Thu Mar 26 16:26:35 2015 +0000
Revision:
5:d0b2b4d8b9ba
Parent:
4:1017848d2fe1
Child:
6:8ffc6d3a7c1d
tested working at end of play 26/03/2015 with 0xefffffff termination

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonathanAshworth 0:2676c97df44e 1 #include "mbed.h"
JonathanAshworth 0:2676c97df44e 2 #include "main.h"
JonathanAshworth 5:d0b2b4d8b9ba 3 #include "MODDMA/MODDMA.h"
JonathanAshworth 5:d0b2b4d8b9ba 4 #include "MODSERIAL/MODSERIAL.h"
JonathanAshworth 5:d0b2b4d8b9ba 5 #include "botHandler/botStateHandler.h"
JonathanAshworth 4:1017848d2fe1 6
JonathanAshworth 4:1017848d2fe1 7 // *** For development only ***
JonathanAshworth 4:1017848d2fe1 8 #define PC_TEST_REQUEST_1 "0,1.01,1,600"
JonathanAshworth 4:1017848d2fe1 9 #define PC_TEST_REQUEST_2 "0,1.01,1,2300"
JonathanAshworth 4:1017848d2fe1 10 DigitalOut led1(LED1);
JonathanAshworth 4:1017848d2fe1 11 DigitalOut led2(LED2);
JonathanAshworth 5:d0b2b4d8b9ba 12 DigitalOut led3(LED3);
JonathanAshworth 4:1017848d2fe1 13 // ****************************
JonathanAshworth 4:1017848d2fe1 14
JonathanAshworth 4:1017848d2fe1 15 #define PC_TX_BUFFER_SIZE 1024
JonathanAshworth 4:1017848d2fe1 16 #define PC_RX_BUFFER_SIZE 1024
JonathanAshworth 4:1017848d2fe1 17 #define PC_TX_PIN USBTX //Serial tx
JonathanAshworth 4:1017848d2fe1 18 #define PC_RX_PIN USBRX //Serial rx
JonathanAshworth 4:1017848d2fe1 19 #define PC_TIMEOUT_MESSAGE "MBED detected a timeout - eStop!"
JonathanAshworth 5:d0b2b4d8b9ba 20 #define PC_TERMINATION_BYTE0 0xef //0xcd //Packet termination sequence
JonathanAshworth 5:d0b2b4d8b9ba 21 #define PC_TERMINATION_BYTE1 0xff //0xcc
JonathanAshworth 5:d0b2b4d8b9ba 22 #define PC_TERMINATION_BYTE2 0xff //0x8c
JonathanAshworth 5:d0b2b4d8b9ba 23 #define PC_TERMINATION_BYTE3 0xff //0xbf
JonathanAshworth 4:1017848d2fe1 24
JonathanAshworth 4:1017848d2fe1 25 #define SSC32_TX_BUFFER_SIZE 512
JonathanAshworth 4:1017848d2fe1 26 #define SSC32_RX_BUFFER_SIZE 512
JonathanAshworth 4:1017848d2fe1 27 #define SSC32_TX_PIN p13 //Serial tx
JonathanAshworth 4:1017848d2fe1 28 #define SSC32_RX_PIN p14 //Serial rx
JonathanAshworth 0:2676c97df44e 29
JonathanAshworth 4:1017848d2fe1 30 #define GPS_TX_BUFFER_SIZE 512
JonathanAshworth 4:1017848d2fe1 31 #define GPS_RX_BUFFER_SIZE 512
JonathanAshworth 4:1017848d2fe1 32 #define GPS_TX_PIN p28 //Serial tx
JonathanAshworth 4:1017848d2fe1 33 #define GPS_RX_PIN p27 //Serial rx
JonathanAshworth 4:1017848d2fe1 34
JonathanAshworth 4:1017848d2fe1 35 /* XBEES NOT INCLUDED IN SOLUTION (Recommend a pro version with I2C support)
JonathanAshworth 4:1017848d2fe1 36 #define XBEE_TX_BUFFER_SIZE 512
JonathanAshworth 4:1017848d2fe1 37 #define XBEE_RX_BUFFER_SIZE 512
JonathanAshworth 4:1017848d2fe1 38 #define XBEE_TX_PIN 0 //PIN UNALLOCATED
JonathanAshworth 4:1017848d2fe1 39 #define XBEE_RX_PIN 0 //PIN UNALLOCATED
JonathanAshworth 4:1017848d2fe1 40 */
JonathanAshworth 4:1017848d2fe1 41
JonathanAshworth 4:1017848d2fe1 42 #define ULTRASOUND_TRIGGER_PIN p11//Digital out
JonathanAshworth 4:1017848d2fe1 43 #define ULTRASOUND_ECHO_PIN p12 //Digital (PWM) in (from mux common)
JonathanAshworth 4:1017848d2fe1 44 #define BATTERY_VOLTAGE_PIN p16 //Analogue in
JonathanAshworth 4:1017848d2fe1 45 #define BATTERY_CURRENT_PIN p15 //Analogue in
JonathanAshworth 4:1017848d2fe1 46 #define MUX_D0_PIN p5 //Digital out
JonathanAshworth 4:1017848d2fe1 47 #define MUX_D1_PIN p6 //Digital out
JonathanAshworth 4:1017848d2fe1 48 #define MUX_D2_PIN p7 //Digital out
JonathanAshworth 4:1017848d2fe1 49 #define MUX_D3_PIN p8 //Digital out
JonathanAshworth 4:1017848d2fe1 50 #define I2C_SDA p9 //I2C data line
JonathanAshworth 4:1017848d2fe1 51 #define I2C_SCL p10 //I2C clock line
JonathanAshworth 4:1017848d2fe1 52 #define PIR_PIN p20 //Digital in
JonathanAshworth 4:1017848d2fe1 53 #define DRIVE_NS_PIN p21 //PWM out
JonathanAshworth 4:1017848d2fe1 54 #define DRIVE_OS_PIN p22 //PWM out
JonathanAshworth 0:2676c97df44e 55
JonathanAshworth 4:1017848d2fe1 56 MODSERIAL PCSerial(PC_TX_PIN, PC_RX_PIN, PC_TX_BUFFER_SIZE, PC_RX_BUFFER_SIZE);
JonathanAshworth 4:1017848d2fe1 57 MODSERIAL SSC32Serial(SSC32_TX_PIN, SSC32_RX_PIN, SSC32_TX_BUFFER_SIZE, SSC32_RX_BUFFER_SIZE);
JonathanAshworth 4:1017848d2fe1 58 MODSERIAL GPSSerial(GPS_TX_PIN, GPS_RX_PIN, GPS_TX_BUFFER_SIZE, GPS_RX_BUFFER_SIZE);
JonathanAshworth 4:1017848d2fe1 59 MODDMA dma;
JonathanAshworth 4:1017848d2fe1 60
JonathanAshworth 0:2676c97df44e 61 Flags flags;
JonathanAshworth 0:2676c97df44e 62
JonathanAshworth 4:1017848d2fe1 63 Timer PCTimeoutTimer;
JonathanAshworth 4:1017848d2fe1 64
JonathanAshworth 4:1017848d2fe1 65 Servo armBase;
JonathanAshworth 4:1017848d2fe1 66 Servo armShoulder;
JonathanAshworth 4:1017848d2fe1 67 Servo armElbow;
JonathanAshworth 4:1017848d2fe1 68 Servo armWrist;
JonathanAshworth 4:1017848d2fe1 69 Servo armManipulator;
JonathanAshworth 4:1017848d2fe1 70 Servo visionPitch;
JonathanAshworth 4:1017848d2fe1 71 Servo visionYaw;
JonathanAshworth 4:1017848d2fe1 72
JonathanAshworth 4:1017848d2fe1 73 DriveMotor driveNearside;
JonathanAshworth 4:1017848d2fe1 74 DriveMotor driveOffside;
JonathanAshworth 4:1017848d2fe1 75
JonathanAshworth 4:1017848d2fe1 76 AccelerometerSensor accelerometer;
JonathanAshworth 4:1017848d2fe1 77 GyroSensor gyro;
JonathanAshworth 4:1017848d2fe1 78 MagnetometerSensor magnetometer;
JonathanAshworth 4:1017848d2fe1 79
JonathanAshworth 4:1017848d2fe1 80 GPSSensor gps;
JonathanAshworth 4:1017848d2fe1 81
JonathanAshworth 4:1017848d2fe1 82 TemperatureSensor temperature;
JonathanAshworth 4:1017848d2fe1 83 HumiditySensor humidity;
JonathanAshworth 4:1017848d2fe1 84
JonathanAshworth 4:1017848d2fe1 85 BatteryVoltageSensor batteryVoltage;
JonathanAshworth 4:1017848d2fe1 86 BatteryCurrentSensor batteryCurrent;
JonathanAshworth 4:1017848d2fe1 87
JonathanAshworth 4:1017848d2fe1 88 PIRSensor PIR;
JonathanAshworth 4:1017848d2fe1 89
JonathanAshworth 4:1017848d2fe1 90 UltrasoundSensor ultrasound1 , ultrasound2, ultrasound3, ultrasound4, ultrasound5,
JonathanAshworth 4:1017848d2fe1 91 ultrasound6, ultrasound7, ultrasound8, ultrasound9, ultrasound10;
JonathanAshworth 4:1017848d2fe1 92
JonathanAshworth 4:1017848d2fe1 93 botStateHandler botData;
JonathanAshworth 4:1017848d2fe1 94
JonathanAshworth 4:1017848d2fe1 95 void *childIDLookup[30];
JonathanAshworth 4:1017848d2fe1 96
JonathanAshworth 4:1017848d2fe1 97 char PCRxData[PC_RX_BUFFER_SIZE] = {0}; //Complete PC requests (termination detected)
JonathanAshworth 4:1017848d2fe1 98 char incommingPCRxData[PC_RX_BUFFER_SIZE] = {0}; //Used to temp store incomming PC bytes
JonathanAshworth 4:1017848d2fe1 99
JonathanAshworth 4:1017848d2fe1 100 char incommingPCRxDataCount = 0;
JonathanAshworth 4:1017848d2fe1 101 char PCRxTerminationCount = 0;
JonathanAshworth 0:2676c97df44e 102
JonathanAshworth 0:2676c97df44e 103 int main(void) {
JonathanAshworth 0:2676c97df44e 104
JonathanAshworth 4:1017848d2fe1 105 //((Servo*)childIDLookup[0])->channel = 0;
JonathanAshworth 0:2676c97df44e 106
JonathanAshworth 4:1017848d2fe1 107 initialise();
JonathanAshworth 0:2676c97df44e 108
JonathanAshworth 0:2676c97df44e 109 while(1) {
JonathanAshworth 4:1017848d2fe1 110 /*SSC32Serial.printf("#0 P1500 T50 \r");
JonathanAshworth 4:1017848d2fe1 111
JonathanAshworth 4:1017848d2fe1 112 wait_ms(1000);
JonathanAshworth 4:1017848d2fe1 113
JonathanAshworth 4:1017848d2fe1 114 SSC32Serial.printf("#0 P600 T25 \r");
JonathanAshworth 4:1017848d2fe1 115
JonathanAshworth 4:1017848d2fe1 116 wait_ms(1000);
JonathanAshworth 4:1017848d2fe1 117
JonathanAshworth 4:1017848d2fe1 118 SSC32Serial.printf("#0 P2300 T500 \r");
JonathanAshworth 4:1017848d2fe1 119
JonathanAshworth 4:1017848d2fe1 120 wait_ms(1000);*/
JonathanAshworth 4:1017848d2fe1 121 //LPC_TIM2->TCR = 0x02;
JonathanAshworth 4:1017848d2fe1 122 if (PCTimeoutTimer.read_ms() > 100) {
JonathanAshworth 4:1017848d2fe1 123 flags.PCTimeout = 1;
JonathanAshworth 5:d0b2b4d8b9ba 124 GPSSerial.printf("Idle timeout\n");
JonathanAshworth 0:2676c97df44e 125 }
JonathanAshworth 4:1017848d2fe1 126 if (flags.PCTimeout == 1) {
JonathanAshworth 4:1017848d2fe1 127 PCTimeout();
JonathanAshworth 4:1017848d2fe1 128 }
JonathanAshworth 4:1017848d2fe1 129 if (flags.rxNewData == 1) {
JonathanAshworth 4:1017848d2fe1 130 sendSerialData();
JonathanAshworth 0:2676c97df44e 131 flags.rxNewData = 0;
JonathanAshworth 0:2676c97df44e 132 }
JonathanAshworth 0:2676c97df44e 133 }
JonathanAshworth 0:2676c97df44e 134 }
JonathanAshworth 0:2676c97df44e 135
JonathanAshworth 4:1017848d2fe1 136 void initialise(void) {
JonathanAshworth 4:1017848d2fe1 137
JonathanAshworth 4:1017848d2fe1 138 //wait_ms(10000);
JonathanAshworth 4:1017848d2fe1 139
JonathanAshworth 4:1017848d2fe1 140 PCSerial.baud(115200);
JonathanAshworth 4:1017848d2fe1 141 PCSerial.attach(&dmaPCSerialRx, MODSERIAL::RxIrq);
JonathanAshworth 4:1017848d2fe1 142 SSC32Serial.baud(115200);
JonathanAshworth 4:1017848d2fe1 143 //SSC32Serial.attach(&dmaSSC32SerialRx, MODSERIAL::RxIrq);
JonathanAshworth 4:1017848d2fe1 144 GPSSerial.baud(115200);
JonathanAshworth 4:1017848d2fe1 145 //GPSSerial.attach(&dmaGPSSerialRx, MODSERIAL::RxIrq);
JonathanAshworth 4:1017848d2fe1 146
JonathanAshworth 4:1017848d2fe1 147 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 148 PCTimeoutTimer.reset();
JonathanAshworth 4:1017848d2fe1 149
JonathanAshworth 4:1017848d2fe1 150 //PCTimeoutTimer.attach(&PCTimeout, 0.1);
JonathanAshworth 4:1017848d2fe1 151 //LPC_TIM2->TCR = 0x00; //Stop timer
JonathanAshworth 4:1017848d2fe1 152 //LPC_TIM2->TCR = 0x02; //Reset timer
JonathanAshworth 4:1017848d2fe1 153
JonathanAshworth 4:1017848d2fe1 154 /*LPC_SC->PCONP |= 1 << 1; //Power up Timer 0
JonathanAshworth 4:1017848d2fe1 155 LPC_SC->PCLKSEL0 |= 1 << 2; // Clock for timer = CCLK
JonathanAshworth 4:1017848d2fe1 156 LPC_TIM0->MR0 = 1 << 23; // Give a value suitable for the LED blinking frequency based on the clock frequency
JonathanAshworth 4:1017848d2fe1 157 LPC_TIM0->MCR |= 1 << 0; // Interrupt on Match0 compare
JonathanAshworth 4:1017848d2fe1 158 LPC_TIM0->MCR |= 1 << 1; // Reset timer on Match 0.
JonathanAshworth 4:1017848d2fe1 159 LPC_TIM0->TCR |= 1 << 1; // Manually Reset Timer0 ( forced )
JonathanAshworth 4:1017848d2fe1 160 LPC_TIM0->TCR &= ~(1 << 1); // stop resetting the timer.
JonathanAshworth 4:1017848d2fe1 161 NVIC_EnableIRQ(TIMER0_IRQn); // Enable timer interrupt
JonathanAshworth 4:1017848d2fe1 162 LPC_TIM0->TCR |= 1 << 0; // Start timer
JonathanAshworth 4:1017848d2fe1 163 */
JonathanAshworth 4:1017848d2fe1 164 NVIC_SetPriority(DMA_IRQn, 1);
JonathanAshworth 4:1017848d2fe1 165 NVIC_SetPriority(USB_IRQn, 1);
JonathanAshworth 4:1017848d2fe1 166 //NVIC_SetPriority(dmaGPSSerial, 2);
JonathanAshworth 4:1017848d2fe1 167 //NVIC_SetPriority(dmaSSC32Serial, 3);
JonathanAshworth 4:1017848d2fe1 168 NVIC_SetPriority(RIT_IRQn, 1);
JonathanAshworth 4:1017848d2fe1 169 NVIC_SetPriority(TIMER0_IRQn, 0);
JonathanAshworth 4:1017848d2fe1 170 NVIC_SetPriority(TIMER1_IRQn, 0);
JonathanAshworth 4:1017848d2fe1 171 NVIC_SetPriority(TIMER2_IRQn, 0);
JonathanAshworth 4:1017848d2fe1 172 NVIC_SetPriority(TIMER3_IRQn, 0);
JonathanAshworth 4:1017848d2fe1 173 //NVIC_SetPriority(timer, 0);
JonathanAshworth 4:1017848d2fe1 174
JonathanAshworth 4:1017848d2fe1 175 armBase.channel = 0;
JonathanAshworth 4:1017848d2fe1 176 armShoulder.channel = 1;
JonathanAshworth 4:1017848d2fe1 177 armElbow.channel = 2;
JonathanAshworth 4:1017848d2fe1 178 armWrist.channel = 3;
JonathanAshworth 4:1017848d2fe1 179 armManipulator.channel = 4;
JonathanAshworth 4:1017848d2fe1 180 visionPitch.channel = 5;
JonathanAshworth 4:1017848d2fe1 181 visionYaw.channel = 6;
JonathanAshworth 4:1017848d2fe1 182
JonathanAshworth 4:1017848d2fe1 183 childIDLookup[0] = &armBase; // 1.01
JonathanAshworth 4:1017848d2fe1 184 childIDLookup[1] = &ultrasound1; // 1.02
JonathanAshworth 4:1017848d2fe1 185 childIDLookup[2] = &armShoulder; // 1.02
JonathanAshworth 4:1017848d2fe1 186 childIDLookup[3] = &armElbow; // 1.03
JonathanAshworth 4:1017848d2fe1 187 childIDLookup[4] = &armWrist; // 1.04
JonathanAshworth 4:1017848d2fe1 188 childIDLookup[5] = &armManipulator; // 1.05
JonathanAshworth 4:1017848d2fe1 189 childIDLookup[6] = &visionPitch; // 1.06
JonathanAshworth 4:1017848d2fe1 190 childIDLookup[7] = &visionYaw; // 1.07
JonathanAshworth 4:1017848d2fe1 191 childIDLookup[8] = &driveNearside; // 1.08
JonathanAshworth 4:1017848d2fe1 192 childIDLookup[9] = &driveOffside; // 1.09
JonathanAshworth 4:1017848d2fe1 193 childIDLookup[10] = 0; // 1.10 RESERVED
JonathanAshworth 4:1017848d2fe1 194 childIDLookup[11] = &accelerometer; // 1.11
JonathanAshworth 4:1017848d2fe1 195 childIDLookup[12] = &gyro; // 1.12
JonathanAshworth 4:1017848d2fe1 196 childIDLookup[13] = &magnetometer; // 1.13
JonathanAshworth 4:1017848d2fe1 197 childIDLookup[14] = &gps; // 1.14
JonathanAshworth 4:1017848d2fe1 198 childIDLookup[15] = &temperature; // 1.15
JonathanAshworth 4:1017848d2fe1 199 childIDLookup[16] = &humidity; // 1.16
JonathanAshworth 4:1017848d2fe1 200 childIDLookup[17] = &batteryVoltage; // 1.17
JonathanAshworth 4:1017848d2fe1 201 childIDLookup[18] = &batteryCurrent; // 1.18
JonathanAshworth 4:1017848d2fe1 202 childIDLookup[19] = &PIR; // 1.19
JonathanAshworth 4:1017848d2fe1 203 childIDLookup[20] = &ultrasound1; // 1.20
JonathanAshworth 4:1017848d2fe1 204 childIDLookup[21] = &ultrasound2; // 1.21
JonathanAshworth 4:1017848d2fe1 205 childIDLookup[22] = &ultrasound3; // 1.22
JonathanAshworth 4:1017848d2fe1 206 childIDLookup[23] = &ultrasound4; // 1.23
JonathanAshworth 4:1017848d2fe1 207 childIDLookup[24] = &ultrasound5; // 1.24
JonathanAshworth 4:1017848d2fe1 208 childIDLookup[25] = &ultrasound6; // 1.25
JonathanAshworth 4:1017848d2fe1 209 childIDLookup[26] = &ultrasound7; // 1.26
JonathanAshworth 4:1017848d2fe1 210 childIDLookup[27] = &ultrasound8; // 1.27
JonathanAshworth 4:1017848d2fe1 211 childIDLookup[28] = &ultrasound9; // 1.28
JonathanAshworth 4:1017848d2fe1 212 childIDLookup[29] = &ultrasound10; // 1.29
JonathanAshworth 4:1017848d2fe1 213
JonathanAshworth 4:1017848d2fe1 214 getServoData(&armBase);
JonathanAshworth 4:1017848d2fe1 215 getServoData(&armShoulder);
JonathanAshworth 4:1017848d2fe1 216 getServoData(&armElbow);
JonathanAshworth 4:1017848d2fe1 217 getServoData(&armWrist);
JonathanAshworth 4:1017848d2fe1 218 getServoData(&armManipulator);
JonathanAshworth 4:1017848d2fe1 219 getServoData(&visionPitch);
JonathanAshworth 4:1017848d2fe1 220 getServoData(&visionYaw);
JonathanAshworth 4:1017848d2fe1 221
JonathanAshworth 5:d0b2b4d8b9ba 222 GPSSerial.printf("hi");
JonathanAshworth 5:d0b2b4d8b9ba 223
JonathanAshworth 0:2676c97df44e 224 }
JonathanAshworth 0:2676c97df44e 225
JonathanAshworth 4:1017848d2fe1 226 void getServoData(Servo *servo) {
JonathanAshworth 4:1017848d2fe1 227 SSC32Serial.printf("QP %d\r", servo->channel);
JonathanAshworth 4:1017848d2fe1 228 int temp = 1;
JonathanAshworth 4:1017848d2fe1 229 //read reply
JonathanAshworth 4:1017848d2fe1 230 //convert char to int
JonathanAshworth 4:1017848d2fe1 231 servo->position = temp;
JonathanAshworth 4:1017848d2fe1 232 }
JonathanAshworth 4:1017848d2fe1 233
JonathanAshworth 4:1017848d2fe1 234 void readUltrasound(UltrasoundSensor *sensor) {
JonathanAshworth 4:1017848d2fe1 235 //Set Mux to sensor->muxChannel
JonathanAshworth 4:1017848d2fe1 236 //Pulse trigger pin
JonathanAshworth 4:1017848d2fe1 237 //Disable interrupts
JonathanAshworth 4:1017848d2fe1 238 //Read PWM on echo pin
JonathanAshworth 4:1017848d2fe1 239 //Enable interrupts
JonathanAshworth 4:1017848d2fe1 240 //sensor->distanceMillimeters = result;
JonathanAshworth 0:2676c97df44e 241 }
JonathanAshworth 0:2676c97df44e 242
JonathanAshworth 4:1017848d2fe1 243 void validateRequest(void) {
JonathanAshworth 4:1017848d2fe1 244 /*
JonathanAshworth 4:1017848d2fe1 245
JonathanAshworth 4:1017848d2fe1 246 BYTE 0 = CID = 0 ONLY
JonathanAshworth 4:1017848d2fe1 247 BYTE 1 = COMMA ONLY
JonathanAshworth 4:1017848d2fe1 248 BYTE 2 = SID = 1 | 1.01 upto 1.30
JonathanAshworth 4:1017848d2fe1 249 BYTE 3 = COMMA ONLY
JonathanAshworth 4:1017848d2fe1 250 BYTE 4 = FC = 1 upto 30
JonathanAshworth 4:1017848d2fe1 251 BYTE 5 = COMMA ONLY
JonathanAshworth 4:1017848d2fe1 252
JonathanAshworth 4:1017848d2fe1 253
JonathanAshworth 4:1017848d2fe1 254 */
JonathanAshworth 4:1017848d2fe1 255 }
JonathanAshworth 4:1017848d2fe1 256
JonathanAshworth 5:d0b2b4d8b9ba 257 void parseRequestData(char *request) {
JonathanAshworth 4:1017848d2fe1 258 /*
JonathanAshworth 4:1017848d2fe1 259
JonathanAshworth 5:d0b2b4d8b9ba 260 requestedSensorIDs[512] = {0};
JonathanAshworth 5:d0b2b4d8b9ba 261
JonathanAshworth 5:d0b2b4d8b9ba 262 char posArr[4];
JonathanAshworth 5:d0b2b4d8b9ba 263 char spdArr[4];
JonathanAshworth 5:d0b2b4d8b9ba 264 char timArr[4];
JonathanAshworth 5:d0b2b4d8b9ba 265
JonathanAshworth 5:d0b2b4d8b9ba 266 CID = request[0]; // Byte 0 Read CID
JonathanAshworth 5:d0b2b4d8b9ba 267 SID = request[1]; // Byte 1 Read SID
JonathanAshworth 5:d0b2b4d8b9ba 268 fieldCount = request[2]; // Byte 2 Read Field Count
JonathanAshworth 5:d0b2b4d8b9ba 269
JonathanAshworth 5:d0b2b4d8b9ba 270 If CID != 0
JonathanAshworth 5:d0b2b4d8b9ba 271 return; // Leave as I'm only listening to the PC
JonathanAshworth 5:d0b2b4d8b9ba 272
JonathanAshworth 5:d0b2b4d8b9ba 273 If SID == 0
JonathanAshworth 5:d0b2b4d8b9ba 274 return; // Leave as that message is for the PC
JonathanAshworth 4:1017848d2fe1 275
JonathanAshworth 5:d0b2b4d8b9ba 276 If SID == 1 {
JonathanAshworth 5:d0b2b4d8b9ba 277 Get ready to pump out sensor data
JonathanAshworth 5:d0b2b4d8b9ba 278 for (i=0; i++; i<fieldCount) {
JonathanAshworth 5:d0b2b4d8b9ba 279 requestedSensorIDs[i] = request[3+i];
JonathanAshworth 5:d0b2b4d8b9ba 280 }
JonathanAshworth 5:d0b2b4d8b9ba 281 }
JonathanAshworth 4:1017848d2fe1 282
JonathanAshworth 5:d0b2b4d8b9ba 283 If (SID > 1) && (SID < 0x09) { //We're writing to a servo
JonathanAshworth 5:d0b2b4d8b9ba 284 for (i=0; i<3; i++) {
JonathanAshworth 5:d0b2b4d8b9ba 285 posArr[i] = request[3+i]; //byte 3, 4, 5 & 6
JonathanAshworth 5:d0b2b4d8b9ba 286 spdArr[i] = request[7+i]; //byte 7, 8, 9 & 10
JonathanAshworth 5:d0b2b4d8b9ba 287 timArr[i] = request[11+i]; //byte 11, 12, 13 & 14
JonathanAshworth 5:d0b2b4d8b9ba 288 }
JonathanAshworth 5:d0b2b4d8b9ba 289 position = (float)posArr;
JonathanAshworth 5:d0b2b4d8b9ba 290 speed = (float)spdArr;
JonathanAshworth 5:d0b2b4d8b9ba 291 time =(float)timArr;
JonathanAshworth 5:d0b2b4d8b9ba 292 if ((position > 600)&&(position < 2300)&&(speed > -1)&&(time > -1)) {
JonathanAshworth 5:d0b2b4d8b9ba 293 SSC32Serial.printf("#%d P%d S%d T%d \r", SID-2, position, speed, time); //Set servo
JonathanAshworth 5:d0b2b4d8b9ba 294 botData.setVal(SID, 0, &position); //update botData with new value
JonathanAshworth 5:d0b2b4d8b9ba 295 botData.setVal(SID, 1, &speed);
JonathanAshworth 5:d0b2b4d8b9ba 296 botData.setVal(SID, 2, &time);
JonathanAshworth 5:d0b2b4d8b9ba 297 }
JonathanAshworth 5:d0b2b4d8b9ba 298 else {
JonathanAshworth 5:d0b2b4d8b9ba 299 led4 = 1; //indicate an invalid data warning
JonathanAshworth 5:d0b2b4d8b9ba 300 }
JonathanAshworth 5:d0b2b4d8b9ba 301 }
JonathanAshworth 5:d0b2b4d8b9ba 302 return;
JonathanAshworth 4:1017848d2fe1 303 */
JonathanAshworth 0:2676c97df44e 304 }
JonathanAshworth 0:2676c97df44e 305
JonathanAshworth 4:1017848d2fe1 306 void PCTimeout(void) {
JonathanAshworth 4:1017848d2fe1 307 __disable_irq();
JonathanAshworth 4:1017848d2fe1 308 PCSerial.printf(PC_TIMEOUT_MESSAGE);
JonathanAshworth 5:d0b2b4d8b9ba 309 GPSSerial.printf(PC_TIMEOUT_MESSAGE);
JonathanAshworth 4:1017848d2fe1 310 led1 = 1;
JonathanAshworth 5:d0b2b4d8b9ba 311 led2 = flags.rxNewData;
JonathanAshworth 5:d0b2b4d8b9ba 312 led3 = flags.txSentData;
JonathanAshworth 4:1017848d2fe1 313 while(1);
JonathanAshworth 4:1017848d2fe1 314 }
JonathanAshworth 0:2676c97df44e 315
JonathanAshworth 4:1017848d2fe1 316 void timerISR(void) {
JonathanAshworth 4:1017848d2fe1 317 // Priority 0
JonathanAshworth 4:1017848d2fe1 318 // Fires when timer gets to 110ms
JonathanAshworth 4:1017848d2fe1 319 //LPC_TIM2->TCR = 0x00; //Stop timer
JonathanAshworth 4:1017848d2fe1 320 flags.PCTimeout = 1; // Set timeout flag
JonathanAshworth 4:1017848d2fe1 321 // Leave
JonathanAshworth 4:1017848d2fe1 322 }
JonathanAshworth 4:1017848d2fe1 323
JonathanAshworth 4:1017848d2fe1 324 void dmaPCSerialRx(MODSERIAL_IRQ_INFO *q) {
JonathanAshworth 4:1017848d2fe1 325 /*
JonathanAshworth 4:1017848d2fe1 326
JonathanAshworth 4:1017848d2fe1 327 Priority 1
JonathanAshworth 0:2676c97df44e 328
JonathanAshworth 4:1017848d2fe1 329 Start timer
JonathanAshworth 4:1017848d2fe1 330 If timer is < 100ms
JonathanAshworth 4:1017848d2fe1 331 Copy dma buffer into local buffer
JonathanAshworth 4:1017848d2fe1 332 Look for termination in local buffer
JonathanAshworth 4:1017848d2fe1 333 If found then stop/reset/(start again once ive sent) timer and set new data flag
JonathanAshworth 4:1017848d2fe1 334 Else leave isr
JonathanAshworth 4:1017848d2fe1 335 Else report timeout and leave
JonathanAshworth 4:1017848d2fe1 336
JonathanAshworth 4:1017848d2fe1 337 LPC_TIM2->TCR = 0x01; //Start timer
JonathanAshworth 4:1017848d2fe1 338 LPC_TIM2->TCR = 0x00; //Stop timer
JonathanAshworth 4:1017848d2fe1 339 LPC_TIM2->TCR = 0x02; //Reset timer
JonathanAshworth 4:1017848d2fe1 340 */
JonathanAshworth 4:1017848d2fe1 341
JonathanAshworth 4:1017848d2fe1 342 MODSERIAL *serial = q->serial; //Define local serial class
JonathanAshworth 4:1017848d2fe1 343 if (flags.txSentData == 1) {
JonathanAshworth 4:1017848d2fe1 344 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 345 PCTimeoutTimer.reset();
JonathanAshworth 4:1017848d2fe1 346 flags.txSentData = 0;
JonathanAshworth 4:1017848d2fe1 347 }
JonathanAshworth 4:1017848d2fe1 348 PCTimeoutTimer.start();
JonathanAshworth 4:1017848d2fe1 349
JonathanAshworth 4:1017848d2fe1 350
JonathanAshworth 4:1017848d2fe1 351 //LPC_TIM2->TCR = 0x01; //Start timer
JonathanAshworth 4:1017848d2fe1 352 if (PCTimeoutTimer.read_ms() < 100) {
JonathanAshworth 4:1017848d2fe1 353 incommingPCRxData[incommingPCRxDataCount] = serial->getc();
JonathanAshworth 5:d0b2b4d8b9ba 354 GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]);
JonathanAshworth 4:1017848d2fe1 355 if (incommingPCRxDataCount > 2) {
JonathanAshworth 4:1017848d2fe1 356 if ((incommingPCRxData[incommingPCRxDataCount-3] == (char)PC_TERMINATION_BYTE0) &&
JonathanAshworth 4:1017848d2fe1 357 (incommingPCRxData[incommingPCRxDataCount-2] == (char)PC_TERMINATION_BYTE1) &&
JonathanAshworth 4:1017848d2fe1 358 (incommingPCRxData[incommingPCRxDataCount-1] == (char)PC_TERMINATION_BYTE2) &&
JonathanAshworth 4:1017848d2fe1 359 (incommingPCRxData[incommingPCRxDataCount] == (char)PC_TERMINATION_BYTE3)) {
JonathanAshworth 4:1017848d2fe1 360 //LPC_TIM2->TCR = 0x00; //Stop timer
JonathanAshworth 4:1017848d2fe1 361 //LPC_TIM2->TCR = 0x02; //Reset timer
JonathanAshworth 4:1017848d2fe1 362 memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 4:1017848d2fe1 363 memcpy(PCRxData, incommingPCRxData, PC_RX_BUFFER_SIZE); //PCRxData = incommingPCRxData;
JonathanAshworth 4:1017848d2fe1 364 memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 4:1017848d2fe1 365 incommingPCRxDataCount = 0;
JonathanAshworth 4:1017848d2fe1 366 flags.rxNewData = 1;
JonathanAshworth 4:1017848d2fe1 367 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 368 PCTimeoutTimer.reset();
JonathanAshworth 5:d0b2b4d8b9ba 369 GPSSerial.printf("\nReceived a termination\n");
JonathanAshworth 0:2676c97df44e 370 }
JonathanAshworth 4:1017848d2fe1 371 else {
JonathanAshworth 4:1017848d2fe1 372 incommingPCRxDataCount++;
JonathanAshworth 1:ccecc816c5e8 373 }
JonathanAshworth 0:2676c97df44e 374 }
JonathanAshworth 0:2676c97df44e 375 else {
JonathanAshworth 4:1017848d2fe1 376 incommingPCRxDataCount++;
JonathanAshworth 0:2676c97df44e 377 }
JonathanAshworth 0:2676c97df44e 378 }
JonathanAshworth 0:2676c97df44e 379 else {
JonathanAshworth 4:1017848d2fe1 380 flags.PCTimeout = 1;
JonathanAshworth 5:d0b2b4d8b9ba 381 GPSSerial.printf("Rx timeout\n");
JonathanAshworth 4:1017848d2fe1 382 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 383 PCTimeoutTimer.reset();
JonathanAshworth 4:1017848d2fe1 384 }
JonathanAshworth 4:1017848d2fe1 385
JonathanAshworth 4:1017848d2fe1 386 /* //code for a \n termination
JonathanAshworth 4:1017848d2fe1 387
JonathanAshworth 4:1017848d2fe1 388 if (PCTimeoutTimer.read_ms() < 100) {
JonathanAshworth 4:1017848d2fe1 389 incommingPCRxData[incommingPCRxDataCount] = serial->getc();
JonathanAshworth 4:1017848d2fe1 390 if (incommingPCRxData[incommingPCRxDataCount] == 0x0A) {
JonathanAshworth 4:1017848d2fe1 391 PCRxTerminationCount++;
JonathanAshworth 4:1017848d2fe1 392 if (PCRxTerminationCount == 4) {
JonathanAshworth 4:1017848d2fe1 393 //LPC_TIM2->TCR = 0x00; //Stop timer
JonathanAshworth 4:1017848d2fe1 394 //LPC_TIM2->TCR = 0x02; //Reset timer
JonathanAshworth 4:1017848d2fe1 395 memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
JonathanAshworth 4:1017848d2fe1 396 memcpy(PCRxData, incommingPCRxData, incommingPCRxDataCount); //PCRxData = incommingPCRxData;
JonathanAshworth 4:1017848d2fe1 397 incommingPCRxDataCount = 0;
JonathanAshworth 4:1017848d2fe1 398 PCRxTerminationCount = 0;
JonathanAshworth 4:1017848d2fe1 399 flags.rxNewData = 1;
JonathanAshworth 4:1017848d2fe1 400 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 401 PCTimeoutTimer.reset();
JonathanAshworth 4:1017848d2fe1 402 }
JonathanAshworth 4:1017848d2fe1 403 }
JonathanAshworth 4:1017848d2fe1 404 incommingPCRxDataCount++;
JonathanAshworth 0:2676c97df44e 405 }
JonathanAshworth 4:1017848d2fe1 406 else {
JonathanAshworth 4:1017848d2fe1 407 flags.PCTimeout = 1;
JonathanAshworth 4:1017848d2fe1 408 PCTimeoutTimer.stop();
JonathanAshworth 4:1017848d2fe1 409 PCTimeoutTimer.reset();
JonathanAshworth 4:1017848d2fe1 410 } */
JonathanAshworth 0:2676c97df44e 411 }
JonathanAshworth 0:2676c97df44e 412
JonathanAshworth 4:1017848d2fe1 413 void sendSerialData(void) {
JonathanAshworth 5:d0b2b4d8b9ba 414 int i = 0;
JonathanAshworth 5:d0b2b4d8b9ba 415 bool terminated = 0;
JonathanAshworth 5:d0b2b4d8b9ba 416 //PCSerial.printf(PCRxData);
JonathanAshworth 5:d0b2b4d8b9ba 417 for (i=0; i<PC_RX_BUFFER_SIZE; i++) {
JonathanAshworth 5:d0b2b4d8b9ba 418 if ( (PCRxData[i-4] == (char)PC_TERMINATION_BYTE0) &&
JonathanAshworth 5:d0b2b4d8b9ba 419 (PCRxData[i-3] == (char)PC_TERMINATION_BYTE1) &&
JonathanAshworth 5:d0b2b4d8b9ba 420 (PCRxData[i-2] == (char)PC_TERMINATION_BYTE2) &&
JonathanAshworth 5:d0b2b4d8b9ba 421 (PCRxData[i-1] == (char)PC_TERMINATION_BYTE3)) {
JonathanAshworth 5:d0b2b4d8b9ba 422 terminated = 1;
JonathanAshworth 5:d0b2b4d8b9ba 423 }
JonathanAshworth 5:d0b2b4d8b9ba 424 if (!terminated) {
JonathanAshworth 5:d0b2b4d8b9ba 425 PCSerial.putc(PCRxData[i]);
JonathanAshworth 5:d0b2b4d8b9ba 426 }
JonathanAshworth 5:d0b2b4d8b9ba 427 }
JonathanAshworth 5:d0b2b4d8b9ba 428 terminated = 0;
JonathanAshworth 5:d0b2b4d8b9ba 429 GPSSerial.printf("Echoed back...\n");
JonathanAshworth 5:d0b2b4d8b9ba 430 for (i=0; i<PC_RX_BUFFER_SIZE; i++) {
JonathanAshworth 5:d0b2b4d8b9ba 431 if ( (PCRxData[i-4] == (char)PC_TERMINATION_BYTE0) &&
JonathanAshworth 5:d0b2b4d8b9ba 432 (PCRxData[i-3] == (char)PC_TERMINATION_BYTE1) &&
JonathanAshworth 5:d0b2b4d8b9ba 433 (PCRxData[i-2] == (char)PC_TERMINATION_BYTE2) &&
JonathanAshworth 5:d0b2b4d8b9ba 434 (PCRxData[i-1] == (char)PC_TERMINATION_BYTE3)) {
JonathanAshworth 5:d0b2b4d8b9ba 435 terminated = 1;
JonathanAshworth 5:d0b2b4d8b9ba 436 }
JonathanAshworth 5:d0b2b4d8b9ba 437 if (!terminated) {
JonathanAshworth 5:d0b2b4d8b9ba 438 GPSSerial.putc(PCRxData[i]);
JonathanAshworth 5:d0b2b4d8b9ba 439 }
JonathanAshworth 5:d0b2b4d8b9ba 440 }
JonathanAshworth 5:d0b2b4d8b9ba 441 GPSSerial.printf("...Echo end\n");
JonathanAshworth 4:1017848d2fe1 442 PCTimeoutTimer.stop(); // precautionary - done after receiving
JonathanAshworth 4:1017848d2fe1 443 PCTimeoutTimer.reset(); // precautionary - done after receiving
JonathanAshworth 4:1017848d2fe1 444 //GPSSerial.printf(PCRxData);
JonathanAshworth 4:1017848d2fe1 445 flags.txSentData = 1;
JonathanAshworth 4:1017848d2fe1 446 PCTimeoutTimer.start();
JonathanAshworth 4:1017848d2fe1 447 //LPC_TIM2->TCR = 0x01; //Start timer
JonathanAshworth 0:2676c97df44e 448 }
JonathanAshworth 4:1017848d2fe1 449
JonathanAshworth 4:1017848d2fe1 450 void dmaSSC32SerialRx(MODSERIAL_IRQ_INFO *q) {
JonathanAshworth 4:1017848d2fe1 451
JonathanAshworth 4:1017848d2fe1 452 }
JonathanAshworth 4:1017848d2fe1 453
JonathanAshworth 4:1017848d2fe1 454 void dmaGPSSerialRx(MODSERIAL_IRQ_INFO *q) {
JonathanAshworth 4:1017848d2fe1 455
JonathanAshworth 4:1017848d2fe1 456 }