test
Dependencies: MODDMA MODSERIAL mbed
Fork of IRIS_MBED by
Diff: main.cpp
- Revision:
- 5:d0b2b4d8b9ba
- Parent:
- 4:1017848d2fe1
- Child:
- 6:8ffc6d3a7c1d
diff -r 1017848d2fe1 -r d0b2b4d8b9ba main.cpp --- a/main.cpp Thu Mar 26 12:02:10 2015 +0000 +++ b/main.cpp Thu Mar 26 16:26:35 2015 +0000 @@ -1,14 +1,15 @@ #include "mbed.h" #include "main.h" -#include "MODDMA.h" -#include "MODSERIAL.h" -#include "botStateHandler.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 @@ -16,10 +17,10 @@ #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 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 @@ -120,7 +121,7 @@ //LPC_TIM2->TCR = 0x02; if (PCTimeoutTimer.read_ms() > 100) { flags.PCTimeout = 1; - led2 = 1; + GPSSerial.printf("Idle timeout\n"); } if (flags.PCTimeout == 1) { PCTimeout(); @@ -218,6 +219,8 @@ getServoData(&visionPitch); getServoData(&visionYaw); + GPSSerial.printf("hi"); + } void getServoData(Servo *servo) { @@ -251,51 +254,62 @@ */ } -void parseRequestData(void) { +void parseRequestData(char *request) { /* - 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 + 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 - 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 + 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; */ - - //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); + GPSSerial.printf(PC_TIMEOUT_MESSAGE); led1 = 1; + led2 = flags.rxNewData; + led3 = flags.txSentData; while(1); } @@ -337,7 +351,7 @@ //LPC_TIM2->TCR = 0x01; //Start timer if (PCTimeoutTimer.read_ms() < 100) { incommingPCRxData[incommingPCRxDataCount] = serial->getc(); - //GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]); + GPSSerial.putc(incommingPCRxData[incommingPCRxDataCount]); if (incommingPCRxDataCount > 2) { if ((incommingPCRxData[incommingPCRxDataCount-3] == (char)PC_TERMINATION_BYTE0) && (incommingPCRxData[incommingPCRxDataCount-2] == (char)PC_TERMINATION_BYTE1) && @@ -352,7 +366,7 @@ flags.rxNewData = 1; PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); - //GPSSerial.printf("\nReceived a termination\n"); + GPSSerial.printf("\nReceived a termination\n"); } else { incommingPCRxDataCount++; @@ -364,6 +378,7 @@ } else { flags.PCTimeout = 1; + GPSSerial.printf("Rx timeout\n"); PCTimeoutTimer.stop(); PCTimeoutTimer.reset(); } @@ -396,12 +411,37 @@ } void sendSerialData(void) { - PCSerial.printf(PCRxData); + 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("Echoed back...\n"); //GPSSerial.printf(PCRxData); - //GPSSerial.printf("...Echo end\n"); flags.txSentData = 1; PCTimeoutTimer.start(); //LPC_TIM2->TCR = 0x01; //Start timer