Diff: main.cpp

Revision:

0:d33d2745b11b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 24 23:58:30 2016 +0000
@@ -0,0 +1,90 @@
+#include "mbed.h"
+#include <string>
+#include <vector>
+// Need to create sensor library to handle all the sensor readings
+ 
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+ 
+Serial pc(USBTX, USBRX);
+
+typedef struct { 
+    string direction; //Forward or Downward facing gives context for the values of x and y
+    float x;
+    float y;
+    float yaw;
+    float depth;
+} message;
+
+typedef struct {
+    float w_yaw;
+    float w_pitch;
+    float w_roll;
+    float yaw;
+    float pitch;
+    float roll;
+} orientation;
+
+typedef struct {
+    float depth; //From pressure sensor
+    float velsurge; //from flow meter
+    float accelsurge; //forward
+    float accelsway; //side to side
+    float accelheave; //downwards
+} translation;
+
+
+orientation vehicle_att;
+translation vehicle_trans;
+message vehicle_cmd;
+
+#define BUFFER_SIZE 255
+
+char buffer[BUFFER_SIZE] = {' '};
+int buffer_iter = 0;
+ 
+void callback() {
+    char c = 0;
+    string packet;
+    string command;
+    float x, y, yaw, depth;
+    
+    // Note: you need to actually read from the serial to clear the RX interrupt
+    if (pc.readable()) {
+       do {
+            c = pc.getc();
+            buffer[buffer_iter] = c;
+            buffer_iter++;
+        }
+        while (buffer_iter < BUFFER_SIZE && c != '\n');
+        
+        command = strtok (buffer," ,\n");
+        x = atof(strtok (NULL, " ,\n"));
+        y = atof(strtok (NULL, " ,\n"));
+        yaw = atof(strtok (NULL, " ,\n"));
+        depth = atof(strtok (NULL, " ,\n"));
+        pc.printf("Received Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", command, x, y, yaw);
+        
+        memset(buffer, ' ', sizeof(buffer));
+        buffer_iter = 0;
+        fflush(stdout);
+        
+        vehicle_cmd.direction = command;
+        vehicle_cmd.x = x;
+        vehicle_cmd.y = y;
+        vehicle_cmd.yaw = yaw;
+        vehicle_cmd.depth = depth;
+    }
+}
+
+
+int main() {
+    pc.attach(&callback);
+    
+    while (1) {
+        led1 = !led1;
+        pc.printf("Current Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", vehicle_cmd.direction, vehicle_cmd.x, vehicle_cmd.y, 
+            vehicle_cmd.yaw, vehicle_cmd.depth);
+            wait_ms(500);
+    }
+}
\ No newline at end of file