Adam Wilcock
test
Dependencies: MODDMA MODSERIAL mbed
Diff: main.cpp
- Revision:
- 4:1017848d2fe1
- Parent:
- 3:074db974b47a
- Child:
- 5:d0b2b4d8b9ba
diff -r 074db974b47a -r 1017848d2fe1 main.cpp
--- a/main.cpp Mon Mar 16 15:38:37 2015 +0000
+++ b/main.cpp Thu Mar 26 12:02:10 2015 +0000
@@ -1,196 +1,416 @@
#include "mbed.h"
#include "main.h"
-#include "RIT.h"
+#include "MODDMA.h"
+#include "MODSERIAL.h"
+#include "botStateHandler.h"
+
+// *** For development only ***
+#define PC_TEST_REQUEST_1 "0,1.01,1,600"
+#define PC_TEST_REQUEST_2 "0,1.01,1,2300"
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+// ****************************
+
+#define PC_TX_BUFFER_SIZE 1024
+#define PC_RX_BUFFER_SIZE 1024
+#define PC_TX_PIN USBTX //Serial tx
+#define PC_RX_PIN USBRX //Serial rx
+#define PC_TIMEOUT_MESSAGE "MBED detected a timeout - eStop!"
+#define PC_TERMINATION_BYTE0 0x0a//0xcd //Packet termination sequence
+#define PC_TERMINATION_BYTE1 0x0a//0xcc
+#define PC_TERMINATION_BYTE2 0x0a//0x8c
+#define PC_TERMINATION_BYTE3 0x0a//0xbf
+
+#define SSC32_TX_BUFFER_SIZE 512
+#define SSC32_RX_BUFFER_SIZE 512
+#define SSC32_TX_PIN p13 //Serial tx
+#define SSC32_RX_PIN p14 //Serial rx
-#define TIMEOUT_MESSAGE "MBED detected a timeout - eStop!\n\n\n\n"
-#define USB_BUFFER_SIZE 1024
+#define GPS_TX_BUFFER_SIZE 512
+#define GPS_RX_BUFFER_SIZE 512
+#define GPS_TX_PIN p28 //Serial tx
+#define GPS_RX_PIN p27 //Serial rx
+
+/* XBEES NOT INCLUDED IN SOLUTION (Recommend a pro version with I2C support)
+#define XBEE_TX_BUFFER_SIZE 512
+#define XBEE_RX_BUFFER_SIZE 512
+#define XBEE_TX_PIN 0 //PIN UNALLOCATED
+#define XBEE_RX_PIN 0 //PIN UNALLOCATED
+*/
+
+#define ULTRASOUND_TRIGGER_PIN p11//Digital out
+#define ULTRASOUND_ECHO_PIN p12 //Digital (PWM) in (from mux common)
+#define BATTERY_VOLTAGE_PIN p16 //Analogue in
+#define BATTERY_CURRENT_PIN p15 //Analogue in
+#define MUX_D0_PIN p5 //Digital out
+#define MUX_D1_PIN p6 //Digital out
+#define MUX_D2_PIN p7 //Digital out
+#define MUX_D3_PIN p8 //Digital out
+#define I2C_SDA p9 //I2C data line
+#define I2C_SCL p10 //I2C clock line
+#define PIR_PIN p20 //Digital in
+#define DRIVE_NS_PIN p21 //PWM out
+#define DRIVE_OS_PIN p22 //PWM out
-Serial serial(USBTX, USBRX);
-Timer usbTimer;
-RIT usbRIT(110);
+MODSERIAL PCSerial(PC_TX_PIN, PC_RX_PIN, PC_TX_BUFFER_SIZE, PC_RX_BUFFER_SIZE);
+MODSERIAL SSC32Serial(SSC32_TX_PIN, SSC32_RX_PIN, SSC32_TX_BUFFER_SIZE, SSC32_RX_BUFFER_SIZE);
+MODSERIAL GPSSerial(GPS_TX_PIN, GPS_RX_PIN, GPS_TX_BUFFER_SIZE, GPS_RX_BUFFER_SIZE);
+MODDMA dma;
+
Flags flags;
-char txBuffer[USB_BUFFER_SIZE] = {0};
-char rxBuffer[USB_BUFFER_SIZE] = {0};
+Timer PCTimeoutTimer;
+
+Servo armBase;
+Servo armShoulder;
+Servo armElbow;
+Servo armWrist;
+Servo armManipulator;
+Servo visionPitch;
+Servo visionYaw;
+
+DriveMotor driveNearside;
+DriveMotor driveOffside;
+
+AccelerometerSensor accelerometer;
+GyroSensor gyro;
+MagnetometerSensor magnetometer;
+
+GPSSensor gps;
+
+TemperatureSensor temperature;
+HumiditySensor humidity;
+
+BatteryVoltageSensor batteryVoltage;
+BatteryCurrentSensor batteryCurrent;
+
+PIRSensor PIR;
+
+UltrasoundSensor ultrasound1 , ultrasound2, ultrasound3, ultrasound4, ultrasound5,
+ ultrasound6, ultrasound7, ultrasound8, ultrasound9, ultrasound10;
+
+botStateHandler botData;
+
+void *childIDLookup[30];
+
+char PCRxData[PC_RX_BUFFER_SIZE] = {0}; //Complete PC requests (termination detected)
+char incommingPCRxData[PC_RX_BUFFER_SIZE] = {0}; //Used to temp store incomming PC bytes
+
+char incommingPCRxDataCount = 0;
+char PCRxTerminationCount = 0;
int main(void) {
-
- flags.rxNewData = 0;
- flags.txSentData = 0;
- flags.usbTimeout = 0;
-
- serial.baud(115200);
-
- //NVIC_SetPriority(TIMER3_IRQn, 1);//priority 1
- //NVIC_SetPriority(USB_IRQn, 2);//priority 2
-
- NVIC_SetPriority( NonMaskableInt_IRQn, 100 );
- NVIC_SetPriority(MemoryManagement_IRQn, 100);
-
- NVIC_SetPriority(BusFault_IRQn, 100);
- NVIC_SetPriority(UsageFault_IRQn, 100);
- NVIC_SetPriority(SVCall_IRQn, 100);
- NVIC_SetPriority(DebugMonitor_IRQn, 100);
- NVIC_SetPriority(PendSV_IRQn, 100);
- NVIC_SetPriority(SysTick_IRQn, 50);
- NVIC_SetPriority(WDT_IRQn, 100);
- NVIC_SetPriority(TIMER0_IRQn, 85);
- NVIC_SetPriority(TIMER1_IRQn, 85);
- NVIC_SetPriority(TIMER2_IRQn, 85);
- NVIC_SetPriority(TIMER3_IRQn, 85);
- NVIC_SetPriority(UART0_IRQn, 100);
- NVIC_SetPriority(UART1_IRQn, 100);
- NVIC_SetPriority(UART2_IRQn, 100);
- NVIC_SetPriority(UART3_IRQn, 100);
- NVIC_SetPriority(PWM1_IRQn, 100);
- NVIC_SetPriority(I2C0_IRQn, 100);
- NVIC_SetPriority(I2C1_IRQn, 100);
- NVIC_SetPriority(I2C2_IRQn, 100);
- NVIC_SetPriority(SPI_IRQn, 100);
- NVIC_SetPriority(SSP0_IRQn, 100);
- NVIC_SetPriority(SSP1_IRQn, 100);
- NVIC_SetPriority(PLL0_IRQn, 100);
- NVIC_SetPriority(RTC_IRQn, 100);
- NVIC_SetPriority(EINT0_IRQn, 100);
- NVIC_SetPriority(EINT1_IRQn, 100);
+ //((Servo*)childIDLookup[0])->channel = 0;
- NVIC_SetPriority(EINT2_IRQn, 100);
- NVIC_SetPriority(EINT3_IRQn, 100);
- NVIC_SetPriority(ADC_IRQn, 100);
- NVIC_SetPriority(BOD_IRQn, 100);
- NVIC_SetPriority(USB_IRQn, 100);
- NVIC_SetPriority(CAN_IRQn, 100);
- NVIC_SetPriority(DMA_IRQn, 100);
-
- NVIC_SetPriority(I2S_IRQn, 100);
- NVIC_SetPriority(ENET_IRQn, 100);
- NVIC_SetPriority(RIT_IRQn, 1);
- NVIC_SetPriority(MCPWM_IRQn, 100);
- NVIC_SetPriority(QEI_IRQn, 100);
- NVIC_SetPriority(PLL1_IRQn, 100);
-
- serial.attach(&serialRx_ISR, Serial::RxIrq);
- usbRIT.append(&usbTimer_ISR);
- usbRIT.disable();
-
- char responseData[USB_BUFFER_SIZE] = {0};
- char requestData[USB_BUFFER_SIZE] = {0};
-
- //LPC_TIMER3->;
-
- //setup ISRs
+ initialise();
while(1) {
- if (flags.usbTimeout) {
- usbTimeout();
+ /*SSC32Serial.printf("#0 P1500 T50 \r");
+
+ wait_ms(1000);
+
+ SSC32Serial.printf("#0 P600 T25 \r");
+
+ wait_ms(1000);
+
+ SSC32Serial.printf("#0 P2300 T500 \r");
+
+ wait_ms(1000);*/
+ //LPC_TIM2->TCR = 0x02;
+ if (PCTimeoutTimer.read_ms() > 100) {
+ flags.PCTimeout = 1;
+ led2 = 1;
}
- if (flags.rxNewData) {
- memcpy(requestData, rxBuffer, USB_BUFFER_SIZE); //requestData = rxBuffer;
- memset(rxBuffer, 0, USB_BUFFER_SIZE); //reset rx buffer
-
- //deconstruct and validate request
-
- //construct response:
- memcpy(responseData, requestData, USB_BUFFER_SIZE); //just echo request back for now
-
- memcpy(txBuffer, responseData, USB_BUFFER_SIZE); //txBuffer = responseData;
-
- serialTx();
-
+ if (flags.PCTimeout == 1) {
+ PCTimeout();
+ }
+ if (flags.rxNewData == 1) {
+ sendSerialData();
flags.rxNewData = 0;
}
}
}
-void serialTx (void) {
-__disable_irq();
- char buffPntr;
- bool terminated = 0;
- for(buffPntr=0; buffPntr<4; buffPntr++) {
- serial.putc(txBuffer[buffPntr]);
- }
- while ((!terminated)&&(buffPntr != USB_BUFFER_SIZE-1)) {
- if ((txBuffer[buffPntr-3] == 0x0A)&&(txBuffer[buffPntr-2] == 0x0A)&&(txBuffer[buffPntr-1] == 0x0A)&&(txBuffer[buffPntr] == 0x0A)) {
- terminated = 1;
- }
- else {
- serial.putc(txBuffer[buffPntr]);
- }
- buffPntr++;
- }
- //send data from tx buffer
- memset(txBuffer, 0, USB_BUFFER_SIZE); //reset tx buffer
- flags.txSentData = 1;
-__enable_irq();
+void initialise(void) {
+
+ //wait_ms(10000);
+
+ PCSerial.baud(115200);
+ PCSerial.attach(&dmaPCSerialRx, MODSERIAL::RxIrq);
+ SSC32Serial.baud(115200);
+ //SSC32Serial.attach(&dmaSSC32SerialRx, MODSERIAL::RxIrq);
+ GPSSerial.baud(115200);
+ //GPSSerial.attach(&dmaGPSSerialRx, MODSERIAL::RxIrq);
+
+ PCTimeoutTimer.stop();
+ PCTimeoutTimer.reset();
+
+ //PCTimeoutTimer.attach(&PCTimeout, 0.1);
+ //LPC_TIM2->TCR = 0x00; //Stop timer
+ //LPC_TIM2->TCR = 0x02; //Reset timer
+
+ /*LPC_SC->PCONP |= 1 << 1; //Power up Timer 0
+ LPC_SC->PCLKSEL0 |= 1 << 2; // Clock for timer = CCLK
+ LPC_TIM0->MR0 = 1 << 23; // Give a value suitable for the LED blinking frequency based on the clock frequency
+ LPC_TIM0->MCR |= 1 << 0; // Interrupt on Match0 compare
+ LPC_TIM0->MCR |= 1 << 1; // Reset timer on Match 0.
+ LPC_TIM0->TCR |= 1 << 1; // Manually Reset Timer0 ( forced )
+ LPC_TIM0->TCR &= ~(1 << 1); // stop resetting the timer.
+ NVIC_EnableIRQ(TIMER0_IRQn); // Enable timer interrupt
+ LPC_TIM0->TCR |= 1 << 0; // Start timer
+ */
+ NVIC_SetPriority(DMA_IRQn, 1);
+ NVIC_SetPriority(USB_IRQn, 1);
+ //NVIC_SetPriority(dmaGPSSerial, 2);
+ //NVIC_SetPriority(dmaSSC32Serial, 3);
+ NVIC_SetPriority(RIT_IRQn, 1);
+ NVIC_SetPriority(TIMER0_IRQn, 0);
+ NVIC_SetPriority(TIMER1_IRQn, 0);
+ NVIC_SetPriority(TIMER2_IRQn, 0);
+ NVIC_SetPriority(TIMER3_IRQn, 0);
+ //NVIC_SetPriority(timer, 0);
+
+ armBase.channel = 0;
+ armShoulder.channel = 1;
+ armElbow.channel = 2;
+ armWrist.channel = 3;
+ armManipulator.channel = 4;
+ visionPitch.channel = 5;
+ visionYaw.channel = 6;
+
+ childIDLookup[0] = &armBase; // 1.01
+ childIDLookup[1] = &ultrasound1; // 1.02
+ childIDLookup[2] = &armShoulder; // 1.02
+ childIDLookup[3] = &armElbow; // 1.03
+ childIDLookup[4] = &armWrist; // 1.04
+ childIDLookup[5] = &armManipulator; // 1.05
+ childIDLookup[6] = &visionPitch; // 1.06
+ childIDLookup[7] = &visionYaw; // 1.07
+ childIDLookup[8] = &driveNearside; // 1.08
+ childIDLookup[9] = &driveOffside; // 1.09
+ childIDLookup[10] = 0; // 1.10 RESERVED
+ childIDLookup[11] = &accelerometer; // 1.11
+ childIDLookup[12] = &gyro; // 1.12
+ childIDLookup[13] = &magnetometer; // 1.13
+ childIDLookup[14] = &gps; // 1.14
+ childIDLookup[15] = &temperature; // 1.15
+ childIDLookup[16] = &humidity; // 1.16
+ childIDLookup[17] = &batteryVoltage; // 1.17
+ childIDLookup[18] = &batteryCurrent; // 1.18
+ childIDLookup[19] = &PIR; // 1.19
+ childIDLookup[20] = &ultrasound1; // 1.20
+ childIDLookup[21] = &ultrasound2; // 1.21
+ childIDLookup[22] = &ultrasound3; // 1.22
+ childIDLookup[23] = &ultrasound4; // 1.23
+ childIDLookup[24] = &ultrasound5; // 1.24
+ childIDLookup[25] = &ultrasound6; // 1.25
+ childIDLookup[26] = &ultrasound7; // 1.26
+ childIDLookup[27] = &ultrasound8; // 1.27
+ childIDLookup[28] = &ultrasound9; // 1.28
+ childIDLookup[29] = &ultrasound10; // 1.29
+
+ getServoData(&armBase);
+ getServoData(&armShoulder);
+ getServoData(&armElbow);
+ getServoData(&armWrist);
+ getServoData(&armManipulator);
+ getServoData(&visionPitch);
+ getServoData(&visionYaw);
+
}
-void usbTimeout(void) {
- memcpy(txBuffer, TIMEOUT_MESSAGE, USB_BUFFER_SIZE); //send eStop over usb serial
- serialTx();
- shutdown();
-__disable_irq();
- while(1) {;} //remain here until hardware reset
+void getServoData(Servo *servo) {
+ SSC32Serial.printf("QP %d\r", servo->channel);
+ int temp = 1;
+ //read reply
+ //convert char to int
+ servo->position = temp;
+}
+
+void readUltrasound(UltrasoundSensor *sensor) {
+ //Set Mux to sensor->muxChannel
+ //Pulse trigger pin
+ //Disable interrupts
+ //Read PWM on echo pin
+ //Enable interrupts
+ //sensor->distanceMillimeters = result;
}
-void shutdown(void) {
- //turn off each peripheral safely
+void validateRequest(void) {
+ /*
+
+ BYTE 0 = CID = 0 ONLY
+ BYTE 1 = COMMA ONLY
+ BYTE 2 = SID = 1 | 1.01 upto 1.30
+ BYTE 3 = COMMA ONLY
+ BYTE 4 = FC = 1 upto 30
+ BYTE 5 = COMMA ONLY
+
+
+ */
+}
+
+void parseRequestData(void) {
+ /*
+
+ 1) Check CID is 0
+ 2) Check for comma
+ 3) Read SID and decide where to go next
+ 4) Check for comma
+ 5) Check for line termination
+ 5) Check for window termination
+ If not present then repeat sequence
+ If present then update variables with new data and return
+
+ Servo:
+ 1) Figure out which servo has been requested
+ 2) Read and validate position data
+ 3) Read and validate speed data
+ 4) Read and validate time data
+
+ Drive:
+ 1) Figure out which motor has been requested
+ 2) Read and validate power data
+
+ Sensors:
+ 1) Read number of fields
+
+ */
+
+ //Servo set request SID = 1.01 to 1.07
+ // CID SID FC F1 F2 F3 Term
+ // 0 1.0X 3 Pos Spd Tim \n
+
+ //Drive motor set request SID = 1.08 & 1.09
+ // CID SID FC F1 Term
+ // 0 1.0X 1 Pwr \n
+
+ //Sensor info request SID = 1
+ // CID SID FC FN Term
+ // 0 1 N 1.XX \n
}
-
-/**************** ISRs ****************/
+void PCTimeout(void) {
+__disable_irq();
+ PCSerial.printf(PC_TIMEOUT_MESSAGE);
+ //GPSSerial.printf(PC_TIMEOUT_MESSAGE);
+ led1 = 1;
+ while(1);
+}
-void serialRx_ISR(void) {
- char buffPntr = 0;
- bool terminated = 0;
+void timerISR(void) {
+ // Priority 0
+ // Fires when timer gets to 110ms
+ //LPC_TIM2->TCR = 0x00; //Stop timer
+ flags.PCTimeout = 1; // Set timeout flag
+ // Leave
+}
+
+void dmaPCSerialRx(MODSERIAL_IRQ_INFO *q) {
+ /*
+
+ Priority 1
- usbRIT.disable();
- usbTimer.stop();
- usbTimer.reset();
- usbTimer.start();
- usbRIT.enable();
- while ((buffPntr != USB_BUFFER_SIZE-1)&&(!flags.usbTimeout)) {
- if (buffPntr >= 3) {
- if ((rxBuffer[buffPntr-3] == 0x0A)&&(rxBuffer[buffPntr-2] == 0x0A)&&(rxBuffer[buffPntr-1] == 0x0A)&&(rxBuffer[buffPntr] == 0x0A)) {
- terminated = 1;
+ Start timer
+ If timer is < 100ms
+ Copy dma buffer into local buffer
+ Look for termination in local buffer
+ If found then stop/reset/(start again once ive sent) timer and set new data flag
+ Else leave isr
+ Else report timeout and leave
+
+ LPC_TIM2->TCR = 0x01; //Start timer
+ LPC_TIM2->TCR = 0x00; //Stop timer
+ LPC_TIM2->TCR = 0x02; //Reset timer
+ */
+
+ MODSERIAL *serial = q->serial; //Define local serial class
+ if (flags.txSentData == 1) {
+ PCTimeoutTimer.stop();
+ PCTimeoutTimer.reset();
+ flags.txSentData = 0;
+ }
+ PCTimeoutTimer.start();
+
+
+ //LPC_TIM2->TCR = 0x01; //Start timer
+ if (PCTimeoutTimer.read_ms() < 100) {
+ incommingPCRxData[incommingPCRxDataCount] = serial->getc();
+ //GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]);
+ if (incommingPCRxDataCount > 2) {
+ if ((incommingPCRxData[incommingPCRxDataCount-3] == (char)PC_TERMINATION_BYTE0) &&
+ (incommingPCRxData[incommingPCRxDataCount-2] == (char)PC_TERMINATION_BYTE1) &&
+ (incommingPCRxData[incommingPCRxDataCount-1] == (char)PC_TERMINATION_BYTE2) &&
+ (incommingPCRxData[incommingPCRxDataCount] == (char)PC_TERMINATION_BYTE3)) {
+ //LPC_TIM2->TCR = 0x00; //Stop timer
+ //LPC_TIM2->TCR = 0x02; //Reset timer
+ memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
+ memcpy(PCRxData, incommingPCRxData, PC_RX_BUFFER_SIZE); //PCRxData = incommingPCRxData;
+ memset(incommingPCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
+ incommingPCRxDataCount = 0;
+ flags.rxNewData = 1;
+ PCTimeoutTimer.stop();
+ PCTimeoutTimer.reset();
+ //GPSSerial.printf("\nReceived a termination\n");
}
- }
- if (!terminated) {
- while((!serial.readable())&&(!flags.usbTimeout)) {;}
- if (serial.readable()) {
- usbRIT.disable();
- usbTimer.stop();
- usbTimer.reset();
- usbTimer.start();
- usbRIT.enable();
- rxBuffer[buffPntr] = serial.getc();//whenDataReady, insert into rx buffer
+ else {
+ incommingPCRxDataCount++;
}
}
else {
- rxBuffer[buffPntr] = 0;
+ incommingPCRxDataCount++;
}
- buffPntr++;
- }
- if (!terminated) {
- flags.usbTimeout = 1; //no valid termination seen
}
else {
- flags.rxNewData = 1;
+ flags.PCTimeout = 1;
+ PCTimeoutTimer.stop();
+ PCTimeoutTimer.reset();
+ }
+
+ /* //code for a \n termination
+
+ if (PCTimeoutTimer.read_ms() < 100) {
+ incommingPCRxData[incommingPCRxDataCount] = serial->getc();
+ if (incommingPCRxData[incommingPCRxDataCount] == 0x0A) {
+ PCRxTerminationCount++;
+ if (PCRxTerminationCount == 4) {
+ //LPC_TIM2->TCR = 0x00; //Stop timer
+ //LPC_TIM2->TCR = 0x02; //Reset timer
+ memset(PCRxData, 0, PC_RX_BUFFER_SIZE); //reset rx buffer
+ memcpy(PCRxData, incommingPCRxData, incommingPCRxDataCount); //PCRxData = incommingPCRxData;
+ incommingPCRxDataCount = 0;
+ PCRxTerminationCount = 0;
+ flags.rxNewData = 1;
+ PCTimeoutTimer.stop();
+ PCTimeoutTimer.reset();
+ }
+ }
+ incommingPCRxDataCount++;
}
- flags.rxNewData = 1;
+ else {
+ flags.PCTimeout = 1;
+ PCTimeoutTimer.stop();
+ PCTimeoutTimer.reset();
+ } */
}
-void usbTimer_ISR(void) {
- usbRIT.disable();
- usbTimer.stop();
- if ((flags.rxNewData) && (flags.txSentData)) {
- usbTimer.reset();
- usbTimer.start();
- usbRIT.enable();
- flags.rxNewData = 0;
- flags.txSentData = 0;
- }
- else {
- flags.usbTimeout = 1;
- }
+void sendSerialData(void) {
+ PCSerial.printf(PCRxData);
+ PCTimeoutTimer.stop(); // precautionary - done after receiving
+ PCTimeoutTimer.reset(); // precautionary - done after receiving
+ //GPSSerial.printf("Echoed back...\n");
+ //GPSSerial.printf(PCRxData);
+ //GPSSerial.printf("...Echo end\n");
+ flags.txSentData = 1;
+ PCTimeoutTimer.start();
+ //LPC_TIM2->TCR = 0x01; //Start timer
}
+
+void dmaSSC32SerialRx(MODSERIAL_IRQ_INFO *q) {
+
+}
+
+void dmaGPSSerialRx(MODSERIAL_IRQ_INFO *q) {
+
+}