File content as of revision 4:1017848d2fe1:

#include "mbed.h"
#include "main.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 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
 
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;

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) {
    
    //((Servo*)childIDLookup[0])->channel = 0;
    
    initialise();
    
    while(1) {
        /*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.PCTimeout == 1) {
            PCTimeout();
        }
        if (flags.rxNewData == 1) {
            sendSerialData();
            flags.rxNewData = 0;
        }
    }
}

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 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 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
}

void PCTimeout(void) {
__disable_irq();
    PCSerial.printf(PC_TIMEOUT_MESSAGE);
    //GPSSerial.printf(PC_TIMEOUT_MESSAGE);
    led1 = 1;
    while(1);
}

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
    
    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");
            }
            else {
                incommingPCRxDataCount++;
            }
        }
        else {
            incommingPCRxDataCount++;
        }
    }
    else {
        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++;
    }
    else {
        flags.PCTimeout = 1;
        PCTimeoutTimer.stop();
        PCTimeoutTimer.reset();
    }   */
}

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