File content as of revision 5:d0b2b4d8b9ba:

#include "mbed.h"
#include "main.h"
#include "MODDMA/MODDMA.h"
#include "MODSERIAL/MODSERIAL.h"
#include "botHandler/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);
DigitalOut led3(LED3);
// ****************************

#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 0xef //0xcd //Packet termination sequence
#define PC_TERMINATION_BYTE1 0xff //0xcc
#define PC_TERMINATION_BYTE2 0xff //0x8c
#define PC_TERMINATION_BYTE3 0xff //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;
            GPSSerial.printf("Idle timeout\n");
        }
        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);
    
    GPSSerial.printf("hi");
    
}

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(char *request) {
    /*
    
    requestedSensorIDs[512] = {0};
    
    char posArr[4];
    char spdArr[4];
    char timArr[4];
    
    CID = request[0];               // Byte 0       Read CID 
    SID = request[1];               // Byte 1       Read SID
    fieldCount = request[2]; // Byte 2      Read Field Count
    
    If CID != 0
        return; // Leave as I'm only listening to the PC
    
    If SID == 0
        return; // Leave as that message is for the PC
    
    If SID == 1 {
        Get ready to pump out sensor data
        for (i=0; i++; i<fieldCount) {
            requestedSensorIDs[i] = request[3+i];
        }
    }
    
    If (SID > 1) && (SID < 0x09) { //We're writing to a servo
        for (i=0; i<3; i++) {
            posArr[i] = request[3+i]; //byte 3, 4, 5 & 6
            spdArr[i] = request[7+i]; //byte 7, 8, 9 & 10
            timArr[i] = request[11+i]; //byte 11, 12, 13 & 14
        }
        position = (float)posArr;
        speed = (float)spdArr;
        time =(float)timArr;
        if ((position > 600)&&(position < 2300)&&(speed > -1)&&(time > -1)) {
            SSC32Serial.printf("#%d P%d S%d T%d \r", SID-2, position, speed, time); //Set servo
            botData.setVal(SID, 0, &position); //update botData with new value
            botData.setVal(SID, 1, &speed);
            botData.setVal(SID, 2, &time);
        }
        else {
            led4 = 1; //indicate an invalid data warning
        }
    }
    return;
    */
}

void PCTimeout(void) {
__disable_irq();
    PCSerial.printf(PC_TIMEOUT_MESSAGE);
    GPSSerial.printf(PC_TIMEOUT_MESSAGE);
    led1 = 1;
    led2 = flags.rxNewData;
    led3 = flags.txSentData;
    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;
        GPSSerial.printf("Rx timeout\n");
        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) {
    int i = 0;
    bool terminated = 0;
    //PCSerial.printf(PCRxData);
    for (i=0; i<PC_RX_BUFFER_SIZE; i++) {
        if (    (PCRxData[i-4] == (char)PC_TERMINATION_BYTE0) &&
                    (PCRxData[i-3] == (char)PC_TERMINATION_BYTE1) &&
                    (PCRxData[i-2] == (char)PC_TERMINATION_BYTE2) &&
                    (PCRxData[i-1] == (char)PC_TERMINATION_BYTE3)) {
                        terminated = 1;
        }
        if (!terminated) {
            PCSerial.putc(PCRxData[i]);
        }
    }
    terminated = 0;
    GPSSerial.printf("Echoed back...\n");
    for (i=0; i<PC_RX_BUFFER_SIZE; i++) {
        if (    (PCRxData[i-4] == (char)PC_TERMINATION_BYTE0) &&
                    (PCRxData[i-3] == (char)PC_TERMINATION_BYTE1) &&
                    (PCRxData[i-2] == (char)PC_TERMINATION_BYTE2) &&
                    (PCRxData[i-1] == (char)PC_TERMINATION_BYTE3)) {
                        terminated = 1;
        }
        if (!terminated) {
            GPSSerial.putc(PCRxData[i]);
        }
    }
    GPSSerial.printf("...Echo end\n");
    PCTimeoutTimer.stop(); // precautionary - done after receiving
    PCTimeoutTimer.reset(); // precautionary - done after receiving
    //GPSSerial.printf(PCRxData);
    flags.txSentData = 1;
    PCTimeoutTimer.start();
    //LPC_TIM2->TCR = 0x01; //Start timer
}

void dmaSSC32SerialRx(MODSERIAL_IRQ_INFO *q) {
    
}

void dmaGPSSerialRx(MODSERIAL_IRQ_INFO *q) {
    
}