main.cpp

00001 #include "mbed.h"
00002 #include <string>
00003 #include <vector>
00004 // Need to create sensor library to handle all the sensor readings
00005  
00006 DigitalOut led1(LED1);
00007 DigitalOut led2(LED2);
00008  
00009 Serial pc(USBTX, USBRX);
00010 
00011 typedef struct { 
00012     string direction; //Forward or Downward facing gives context for the values of x and y
00013     float x;
00014     float y;
00015     float yaw;
00016     float depth;
00017 } message;
00018 
00019 typedef struct {
00020     float w_yaw;
00021     float w_pitch;
00022     float w_roll;
00023     float yaw;
00024     float pitch;
00025     float roll;
00026 } orientation;
00027 
00028 typedef struct {
00029     float depth; //From pressure sensor
00030     float velsurge; //from flow meter
00031     float accelsurge; //forward
00032     float accelsway; //side to side
00033     float accelheave; //downwards
00034 } translation;
00035 
00036 
00037 orientation vehicle_att;
00038 translation vehicle_trans;
00039 message vehicle_cmd;
00040 
00041 #define BUFFER_SIZE 255
00042 
00043 char buffer[BUFFER_SIZE] = {' '};
00044 int buffer_iter = 0;
00045  
00046 void callback() {
00047     char c = 0;
00048     string packet;
00049     string command;
00050     float x, y, yaw, depth;
00051     
00052     // Note: you need to actually read from the serial to clear the RX interrupt
00053     if (pc.readable()) {
00054        do {
00055             c = pc.getc();
00056             buffer[buffer_iter] = c;
00057             buffer_iter++;
00058         }
00059         while (buffer_iter < BUFFER_SIZE && c != '\n');
00060         
00061         command = strtok (buffer," ,\n");
00062         x = atof(strtok (NULL, " ,\n"));
00063         y = atof(strtok (NULL, " ,\n"));
00064         yaw = atof(strtok (NULL, " ,\n"));
00065         depth = atof(strtok (NULL, " ,\n"));
00066         pc.printf("Received Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", command, x, y, yaw);
00067         
00068         memset(buffer, ' ', sizeof(buffer));
00069         buffer_iter = 0;
00070         fflush(stdout);
00071         
00072         vehicle_cmd.direction = command;
00073         vehicle_cmd.x = x;
00074         vehicle_cmd.y = y;
00075         vehicle_cmd.yaw = yaw;
00076         vehicle_cmd.depth = depth;
00077     }
00078 }
00079 
00080 
00081 int main() {
00082     pc.attach(&callback);
00083     
00084     while (1) {
00085         led1 = !led1;
00086         pc.printf("Current Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", vehicle_cmd.direction, vehicle_cmd.x, vehicle_cmd.y, 
00087             vehicle_cmd.yaw, vehicle_cmd.depth);
00088             wait_ms(500);
00089     }
00090 }