Adam Wilcock
test
Dependencies: MODDMA MODSERIAL mbed
Diff: main.cpp
- Revision:
- 5:d0b2b4d8b9ba
- Parent:
- 4:1017848d2fe1
- Child:
- 6:8ffc6d3a7c1d
--- 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