Adam Wilcock / Mbed 2 deprecated IRIS_MBED

test

Dependencies:   MODDMA MODSERIAL mbed

Fork of IRIS_MBED by IRIS

main.cpp@4:1017848d2fe1, 2015-03-26 (annotated)

Committer:
JonathanAshworth
Date:
Thu Mar 26 12:02:10 2015 +0000
Revision:
4:1017848d2fe1
Parent:
3:074db974b47a
Child:
5:d0b2b4d8b9ba
Tested working version using dma ISRs

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