Diff: main.cpp

Revision:

4:1017848d2fe1

Parent:

3:074db974b47a

Child:

5:d0b2b4d8b9ba

--- 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) {
+    
+}