![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: main.cpp
- Revision:
- 20:a820531c78bc
- Parent:
- 19:eef0e3ec32a0
- Child:
- 21:99be83550601
diff -r eef0e3ec32a0 -r a820531c78bc main.cpp --- a/main.cpp Fri Aug 28 08:04:57 2015 +0000 +++ b/main.cpp Sat Aug 29 01:09:40 2015 +0000 @@ -1,27 +1,13 @@ -#include "mbed.h" -#include <string> -#include <sstream> -#include <vector> -#include "Get.h" -#include "Servo.h" -#include "SDFileSystem.h" -using namespace std; +#include "Config.h" -#define MAX_IMU_SIZE 29 - -//Indicator LEDs DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); - -//Hardware connections + Serial pc(USBTX, USBRX); Serial IMU(p28, p27); // tx, rx Serial GPS(p13, p14); // tx, rx -Servo rudderServo(p25); -Servo wingServo(p26); -SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs char IMU_message[256]; int IMU_message_counter=0; @@ -48,13 +34,16 @@ int asterisk_idx; +double Longtitude_Path[MAX_TASK_SIZE]; +double Latitude_Path[MAX_TASK_SIZE]; + vector<string> split(const string &s, char delim) { stringstream ss(s); string item; vector<string> tokens; while (getline(ss, item, delim)) { if (item.empty()) { - item == "N/A"; + item = "N/A"; } tokens.push_back(item); } @@ -111,16 +100,26 @@ Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longtitude, GPS_Altitude, GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph Set path to SailBoat - @SET=Latitude, Longtitude, Task index - @SET=33.776318, -84.407590, 3 + @SET=PATH, Latitude, Longtitude, Task id + @SET=PATH, 33.776318, -84.407590, 3 */ void decodePC(string PC_data) { string PC_data_string(PC_data); if (PC_data_string.substr(0,4) == "@GET") { pc.printf("%s", PC_data_string.c_str()); PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6); - pc.printf("%s\n", decodeCommand(PC_data_string).c_str()); + pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str()); } else if (PC_data_string.substr(0,4) == "@SET") { + pc.printf("%s", PC_data_string.c_str()); + PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6); + string claim = decodeCommandSET(PC_data_string); + if (claim == "DONE") { + pc.printf("Current Path: Longtitude, Latitude\n"); + for (int i=0;i<MAX_TASK_SIZE;++i) { + pc.printf(" %f, %f\n", Longtitude_Path[i], Latitude_Path[i]); + } + } + pc.printf("%s\n", claim.c_str()); } } @@ -204,46 +203,6 @@ led4= !led4; } - -void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){ - /*angleDeg: desired angle - minDeg: minimum angle in degrees - minNorVal: normalized value[0,1] when servo is at its minimum angle - maxDeg: maximum angle in degrees - maxNorVal: normalized value[0,1] when servo is at its maximum angle - */ - - float pos; //normalized angle, [0,1] - float scale; //scale factor for servo calibration - - //extreme conditions - if (angleDeg<minDeg){ - pos=minNorVal; - } - if (angleDeg>maxDeg){ - pos=maxNorVal; - } - - //Calculate scale factor for servo calibration - scale = (angleDeg-minDeg)/(maxDeg-minDeg); - //Calculate servo normalized value - pos = minNorVal + scale*(maxNorVal-minNorVal); - - //send signal to servo - targetServo=pos; -} - -int log_data_SD(){ - FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w"); - if(fp == NULL) { - return 0; - }else{ - //Write all the useful data to the SD card - fprintf(fp, "Nya Pass~"); - fclose(fp); - return 1; - } -} int main() { IMU.baud(57600); @@ -253,6 +212,7 @@ pc.baud(115200); pc.attach(&PC_serial_ISR); + while (1) { led1 = !led1; wait(0.4);