Senior_Design_Sailboat / Mbed 2 deprecated ECE4012_Sailboat

GPS and IMU reading works

Dependencies:   mbed Servo SDFileSystem

/media/uploads/taoqiuyang/img_2352.jpg

Set.cpp@47:4b490931e75f, 2015-11-25 (annotated)

Committer:
ypeng42
Date:
Wed Nov 25 20:13:57 2015 +0000
Revision:
47:4b490931e75f
Parent:
44:4d90645d15ec 
update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dem123456789 20:a820531c78bc 1 #include "Config.h"
dem123456789 20:a820531c78bc 2
dem123456789 20:a820531c78bc 3
dem123456789 20:a820531c78bc 4
dem123456789 20:a820531c78bc 5 string decodeCommandSET(string cmd) {
dem123456789 20:a820531c78bc 6 vector<string> result = split(cmd, ',');
dem123456789 25:30966ed7f7e8 7 string head = result.at(0);
dem123456789 20:a820531c78bc 8 if (head == "PATH") {
dem123456789 35:009cc4509a90 9 double latitude = strtod(result.at(1).c_str(), NULL);
dem123456789 27:1be1f25be449 10 //pc.printf("%f\n",longitude);
dem123456789 35:009cc4509a90 11 double longitude = strtod(result.at(2).c_str(), NULL);
dem123456789 20:a820531c78bc 12 //pc.printf("%f\n",latitude);
dem123456789 25:30966ed7f7e8 13 int task_id = atoi(result.at(3).c_str());
dem123456789 20:a820531c78bc 14 //pc.printf("%d\n",task_id);
dem123456789 20:a820531c78bc 15 if (task_id < 1 or task_id > MAX_TASK_SIZE) {
dem123456789 20:a820531c78bc 16 return "Task id Out of Range";
dem123456789 20:a820531c78bc 17 } else {
dem123456789 27:1be1f25be449 18 Longitude_Path[task_id-1] = longitude;
dem123456789 20:a820531c78bc 19 Latitude_Path[task_id-1] = latitude;
dem123456789 20:a820531c78bc 20 return "DONE";
dem123456789 20:a820531c78bc 21 }
dem123456789 38:528e410f2f7d 22 } else if(head == "SERVO") {
dem123456789 38:528e410f2f7d 23 string servo = result.at(1);
dem123456789 38:528e410f2f7d 24 double angle = strtod(result.at(2).c_str(), NULL);
dem123456789 44:4d90645d15ec 25 if(servo == "RUDDER" or servo == " RUDDER" or servo == "WING" or servo == " WING"){
ypeng42 47:4b490931e75f 26 if(angle<=180 and angle >= 0) {
dem123456789 44:4d90645d15ec 27 if(servo == "RUDDER" or servo == " RUDDER"){
dem123456789 39:ef1a8055d744 28 set_servo_position(rudderServo, angle, 0, 0, 180, 1);
dem123456789 38:528e410f2f7d 29 } else {
dem123456789 39:ef1a8055d744 30 set_servo_position(wingServo, angle, 0, 0, 180, 1);
dem123456789 38:528e410f2f7d 31 }
dem123456789 39:ef1a8055d744 32 return("angle set");
dem123456789 38:528e410f2f7d 33 } else {
dem123456789 44:4d90645d15ec 34 return("angle should be within range 0-180");
dem123456789 38:528e410f2f7d 35 }
dem123456789 38:528e410f2f7d 36 } else {
dem123456789 38:528e410f2f7d 37 return("SERVO type should be RUDDER or WING");
dem123456789 38:528e410f2f7d 38 }
dem123456789 20:a820531c78bc 39 } else {
dem123456789 38:528e410f2f7d 40 return("Not valid command, example: @SET=PATH, 33.776318, -84.407590, 3 or @SET=SERVO, RUDDER, 30");
dem123456789 20:a820531c78bc 41 }
dem123456789 20:a820531c78bc 42 }