![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test
Dependencies: MODDMA MODSERIAL mbed
Fork of IRIS_MBED by
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?
User | Revision | Line number | New 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 | } |