Senior_Design_Sailboat / Mbed 2 deprecated ECE4012_Sailboat

GPS and IMU reading works

Dependencies:   mbed Servo SDFileSystem

/media/uploads/taoqiuyang/img_2352.jpg

main.cpp@48:575ec42c5f58, 2015-11-27 (annotated)

Committer:
dem123456789
Date:
Fri Nov 27 02:24:42 2015 +0000
Revision:
48:575ec42c5f58
Parent:
47:4b490931e75f 
update with gui

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dem123456789 20:a820531c78bc 1 #include "Config.h"
dem123456789 11:1caacb994236 2
taoqiuyang 2:afb333543af5 3 DigitalOut led1(LED1);
taoqiuyang 2:afb333543af5 4 DigitalOut led2(LED2);
taoqiuyang 5:451b8203ef99 5 DigitalOut led3(LED3);
dem123456789 8:1f5a44bade4d 6 DigitalOut led4(LED4);
dem123456789 39:ef1a8055d744 7
taoqiuyang 42:e1a2a6daf70b 8 Serial pc(p9, p10);
taoqiuyang 42:e1a2a6daf70b 9 //Serial pc(USBTX, USBRX);
taoqiuyang 2:afb333543af5 10 Serial IMU(p28, p27); // tx, rx
taoqiuyang 5:451b8203ef99 11 Serial GPS(p13, p14); // tx, rx
dem123456789 21:99be83550601 12 Servo rudderServo(p25);
dem123456789 21:99be83550601 13 Servo wingServo(p26);
dem123456789 21:99be83550601 14 SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs
taoqiuyang 26:353a80179346 15 Ticker ctrl_updt_timer; //timer to refresh controller
taoqiuyang 2:afb333543af5 16
taoqiuyang 2:afb333543af5 17 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 18 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 19 char GPS_message[256];
taoqiuyang 5:451b8203ef99 20 int GPS_message_counter=0;
dem123456789 14:92bacb5af01b 21 char PC_message[256];
dem123456789 14:92bacb5af01b 22 int PC_message_counter=0;
dem123456789 11:1caacb994236 23
dem123456789 11:1caacb994236 24 string IMU_Y="N/A";
dem123456789 11:1caacb994236 25 string IMU_P="N/A";
dem123456789 11:1caacb994236 26 string IMU_R="N/A";
dem123456789 14:92bacb5af01b 27 string GPS_Quality="N/A";
dem123456789 11:1caacb994236 28 string GPS_UTC="N/A";
dem123456789 11:1caacb994236 29 string GPS_Latitude="N/A";
dem123456789 27:1be1f25be449 30 string GPS_Longitude="N/A";
dem123456789 11:1caacb994236 31 string GPS_Altitude="N/A";
dem123456789 22:faba67585854 32 string GPS_Altitude_Unit="N/A";
dem123456789 11:1caacb994236 33 string GPS_Num_Satellite="N/A";
dem123456789 11:1caacb994236 34 string GPS_HDOP="N/A";
dem123456789 12:8644abfa86da 35 string GPS_VDOP="N/A";
dem123456789 12:8644abfa86da 36 string GPS_PDOP="N/A";
dem123456789 14:92bacb5af01b 37 string GPS_Date="N/A";
dem123456789 14:92bacb5af01b 38 string GPS_VelocityKnot="N/A";
dem123456789 22:faba67585854 39 string GPS_VelocityKnot_Unit="N/A";
dem123456789 14:92bacb5af01b 40 string GPS_VelocityKph="N/A";
dem123456789 22:faba67585854 41 string GPS_VelocityKph_Unit="N/A";
dem123456789 23:cc06a8226f72 42 string temp = "";
dem123456789 22:faba67585854 43
dem123456789 22:faba67585854 44 double D_IMU_Y=0;
dem123456789 22:faba67585854 45 double D_IMU_P=0;
dem123456789 22:faba67585854 46 double D_IMU_R=0;
dem123456789 22:faba67585854 47 double D_GPS_Quality=0;
dem123456789 22:faba67585854 48 double D_GPS_UTC=0;
dem123456789 22:faba67585854 49 double D_GPS_Latitude=0;
dem123456789 22:faba67585854 50 double D_GPS_Latitude_Direction=0;
dem123456789 27:1be1f25be449 51 double D_GPS_Longitude=0;
dem123456789 27:1be1f25be449 52 double D_GPS_Longitude_Direction=0;
dem123456789 22:faba67585854 53 double D_GPS_Altitude=0;
dem123456789 22:faba67585854 54 double D_GPS_Num_Satellite=0;
dem123456789 22:faba67585854 55 double D_GPS_HDOP=0;
dem123456789 22:faba67585854 56 double D_GPS_VDOP=0;
dem123456789 22:faba67585854 57 double D_GPS_PDOP=0;
dem123456789 22:faba67585854 58 double D_GPS_Date=0;
dem123456789 22:faba67585854 59 double D_GPS_VelocityKnot=0;
dem123456789 22:faba67585854 60 double D_GPS_VelocityKph=0;
dem123456789 27:1be1f25be449 61 double D_temp=0;
dem123456789 12:8644abfa86da 62 int asterisk_idx;
dem123456789 46:305112d73c69 63 int current_task=0;
dem123456789 9:bf5939466e86 64
dem123456789 27:1be1f25be449 65 double Longitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 66 double Latitude_Path[MAX_TASK_SIZE];
dem123456789 46:305112d73c69 67 int IF_Path_Complete[MAX_TASK_SIZE];
dem123456789 33:37345814fad0 68 double dis[MAX_TASK_SIZE];
dem123456789 33:37345814fad0 69 double ang[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 70
taoqiuyang 40:9537722d185e 71 int autonomous_mode=0;
dem123456789 46:305112d73c69 72 int arrived_destination=0; // fot test purpose
dem123456789 46:305112d73c69 73
taoqiuyang 41:d207b407c8bc 74 double angle_to_path_point,distance_to_path_point,desired_speed,distance_to_route;
taoqiuyang 26:353a80179346 75 double rudder_ctrl_parameters[3];
taoqiuyang 26:353a80179346 76 double rudder_variables[5];//,,,prev,out
taoqiuyang 41:d207b407c8bc 77 double T=0.5; //controller update period=0.5sec, 2Hz
taoqiuyang 26:353a80179346 78
taoqiuyang 26:353a80179346 79
dem123456789 9:bf5939466e86 80 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 81 stringstream ss(s);
dem123456789 9:bf5939466e86 82 string item;
dem123456789 9:bf5939466e86 83 vector<string> tokens;
dem123456789 9:bf5939466e86 84 while (getline(ss, item, delim)) {
dem123456789 15:dbf20c1209ae 85 if (item.empty()) {
dem123456789 20:a820531c78bc 86 item = "N/A";
dem123456789 15:dbf20c1209ae 87 }
dem123456789 9:bf5939466e86 88 tokens.push_back(item);
dem123456789 9:bf5939466e86 89 }
dem123456789 9:bf5939466e86 90 return tokens;
dem123456789 9:bf5939466e86 91 }
dem123456789 9:bf5939466e86 92
dem123456789 27:1be1f25be449 93 double GPSdecimal(double coordinate) {
dem123456789 27:1be1f25be449 94 //Convert a NMEA decimal-decimal value into decimal degree value
dem123456789 35:009cc4509a90 95 //5144.3855 (ddmm.mmmm) = 51 44.3855 = 51 + 0.443855/0.60 = 51.7397583 degrees
dem123456789 35:009cc4509a90 96 if(coordinate > 1000 or coordinate < -1000) {
dem123456789 35:009cc4509a90 97 coordinate = coordinate/100;
dem123456789 35:009cc4509a90 98 }
dem123456789 35:009cc4509a90 99 return ((int) coordinate) + ((coordinate-((int) coordinate))/0.60);
dem123456789 27:1be1f25be449 100 }
dem123456789 27:1be1f25be449 101
dem123456789 33:37345814fad0 102 void initialize() {
dem123456789 29:95b0320bf779 103 fill(Longitude_Path, Longitude_Path+MAX_TASK_SIZE, 181);
dem123456789 29:95b0320bf779 104 fill(Latitude_Path, Latitude_Path+MAX_TASK_SIZE, 181);
dem123456789 46:305112d73c69 105 fill(IF_Path_Complete, IF_Path_Complete+MAX_TASK_SIZE, 0);
dem123456789 33:37345814fad0 106 fill(dis, dis+MAX_TASK_SIZE, -1);
dem123456789 33:37345814fad0 107 fill(ang, ang+MAX_TASK_SIZE, -361);
dem123456789 46:305112d73c69 108 current_task = 1;
dem123456789 29:95b0320bf779 109 }
dem123456789 29:95b0320bf779 110
dem123456789 9:bf5939466e86 111 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 112 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 113 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 114 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 115 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 116 IMU_Y = result.at(0);
dem123456789 23:cc06a8226f72 117 D_IMU_Y = strtod(IMU_Y.c_str(), NULL);
dem123456789 9:bf5939466e86 118 IMU_P = result.at(1);
dem123456789 23:cc06a8226f72 119 D_IMU_P = strtod(IMU_P.c_str(), NULL);
dem123456789 15:dbf20c1209ae 120 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 23:cc06a8226f72 121 D_IMU_R = strtod(IMU_R.c_str(), NULL);
dem123456789 9:bf5939466e86 122 }
dem123456789 9:bf5939466e86 123 }
dem123456789 9:bf5939466e86 124
dem123456789 37:7a136279daf3 125 string stringify(double x)
dem123456789 37:7a136279daf3 126 {
dem123456789 37:7a136279daf3 127 ostringstream o;
dem123456789 37:7a136279daf3 128 if (o << x) {
dem123456789 37:7a136279daf3 129 return o.str();
dem123456789 37:7a136279daf3 130 }
dem123456789 37:7a136279daf3 131 return NULL;
dem123456789 37:7a136279daf3 132 }
dem123456789 37:7a136279daf3 133
dem123456789 10:12ba6ed2d6f0 134 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 135 string GPS_data_string(GPS_data);
dem123456789 11:1caacb994236 136 if (GPS_data_string.substr(0,6) == "$GPGGA") {
dem123456789 11:1caacb994236 137 GPS_data_string = GPS_data_string.substr(7);
dem123456789 10:12ba6ed2d6f0 138 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 139 GPS_Quality = result.at(5);
dem123456789 22:faba67585854 140 D_GPS_Quality = strtod(result.at(5).c_str(), NULL);
dem123456789 14:92bacb5af01b 141 if(GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 11:1caacb994236 142 GPS_UTC = result.at(0);
dem123456789 23:cc06a8226f72 143 D_GPS_UTC = strtod(GPS_UTC.c_str(), NULL);
dem123456789 35:009cc4509a90 144 //
dem123456789 23:cc06a8226f72 145 if (result.at(2) == "N") { // 0 means North
dem123456789 22:faba67585854 146 D_GPS_Latitude_Direction = 0;
dem123456789 35:009cc4509a90 147 temp = result.at(1);
dem123456789 23:cc06a8226f72 148 } else if (result.at(2) == "S") { // 1 means South
dem123456789 27:1be1f25be449 149 D_GPS_Latitude_Direction = 1;
dem123456789 27:1be1f25be449 150 temp = "-" + result.at(1);
dem123456789 22:faba67585854 151 }
dem123456789 27:1be1f25be449 152 D_temp = strtod(temp.c_str(), NULL);
dem123456789 27:1be1f25be449 153 D_GPS_Latitude = GPSdecimal(D_temp);
dem123456789 37:7a136279daf3 154 GPS_Latitude=stringify(D_GPS_Latitude);
dem123456789 35:009cc4509a90 155 //
dem123456789 23:cc06a8226f72 156 if (result.at(4) == "E") { // 0 means East
dem123456789 27:1be1f25be449 157 D_GPS_Longitude_Direction = 0;
dem123456789 35:009cc4509a90 158 temp = result.at(3);
dem123456789 23:cc06a8226f72 159 } else if (result.at(4) == "W") { // 1 means West
dem123456789 27:1be1f25be449 160 D_GPS_Longitude_Direction = 1;
dem123456789 27:1be1f25be449 161 temp = "-" + result.at(3);
dem123456789 22:faba67585854 162 }
dem123456789 27:1be1f25be449 163 D_temp = strtod(temp.c_str(), NULL);
dem123456789 27:1be1f25be449 164 D_GPS_Longitude = GPSdecimal(D_temp);
dem123456789 37:7a136279daf3 165 GPS_Longitude=stringify(D_GPS_Longitude);
dem123456789 35:009cc4509a90 166 //
dem123456789 11:1caacb994236 167 GPS_Num_Satellite = result.at(6);
dem123456789 22:faba67585854 168 D_GPS_Num_Satellite = strtod(result.at(6).c_str(), NULL);
dem123456789 11:1caacb994236 169 GPS_HDOP = result.at(7);
dem123456789 22:faba67585854 170 D_GPS_HDOP = strtod(result.at(7).c_str(), NULL);
dem123456789 48:575ec42c5f58 171 GPS_Altitude = result.at(8);
dem123456789 22:faba67585854 172 D_GPS_Altitude = strtod(result.at(8).c_str(), NULL);
dem123456789 22:faba67585854 173 GPS_Altitude_Unit = result.at(9);
dem123456789 11:1caacb994236 174 }
dem123456789 14:92bacb5af01b 175 } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 176 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 177 vector<string> result = split(GPS_data_string, ',');
dem123456789 12:8644abfa86da 178 GPS_PDOP = result.at(14);
dem123456789 22:faba67585854 179 D_GPS_PDOP = strtod(result.at(14).c_str(), NULL);
dem123456789 12:8644abfa86da 180 asterisk_idx = result.at(16).find('*');
dem123456789 12:8644abfa86da 181 GPS_VDOP = result.at(16).substr(0, asterisk_idx);
dem123456789 22:faba67585854 182 D_GPS_VDOP = strtod(result.at(16).substr(0, asterisk_idx).c_str(), NULL);
dem123456789 14:92bacb5af01b 183 } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 184 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 185 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 186 GPS_Date = result.at(8);
dem123456789 22:faba67585854 187 D_GPS_Date = strtod(result.at(8).c_str(), NULL);
dem123456789 14:92bacb5af01b 188 } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 189 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 190 vector<string> result = split(GPS_data_string, ',');
dem123456789 48:575ec42c5f58 191 GPS_VelocityKnot = result.at(4);
dem123456789 22:faba67585854 192 D_GPS_VelocityKnot = strtod(result.at(4).c_str(), NULL);
dem123456789 22:faba67585854 193 GPS_VelocityKnot_Unit = result.at(5);
dem123456789 12:8644abfa86da 194 asterisk_idx = result.at(7).find('*');
dem123456789 48:575ec42c5f58 195 GPS_VelocityKph = result.at(6);
dem123456789 22:faba67585854 196 D_GPS_VelocityKph = strtod(result.at(6).c_str(), NULL);
dem123456789 22:faba67585854 197 GPS_VelocityKph_Unit = result.at(7).substr(0, asterisk_idx);
dem123456789 10:12ba6ed2d6f0 198 }
dem123456789 12:8644abfa86da 199
dem123456789 10:12ba6ed2d6f0 200 }
dem123456789 16:0ea992d9a390 201 /* Get data from SailBoat
dem123456789 16:0ea992d9a390 202 @GET=parameter
dem123456789 16:0ea992d9a390 203 Ex: @GET=GPS_Quality
dem123456789 27:1be1f25be449 204 Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longitude, GPS_Altitude,
dem123456789 16:0ea992d9a390 205 GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph
dem123456789 16:0ea992d9a390 206 Set path to SailBoat
dem123456789 27:1be1f25be449 207 @SET=PATH, Latitude, Longitude, Task id
dem123456789 20:a820531c78bc 208 @SET=PATH, 33.776318, -84.407590, 3
dem123456789 16:0ea992d9a390 209 */
dem123456789 15:dbf20c1209ae 210 void decodePC(string PC_data) {
dem123456789 15:dbf20c1209ae 211 string PC_data_string(PC_data);
dem123456789 15:dbf20c1209ae 212 if (PC_data_string.substr(0,4) == "@GET") {
ypeng42 47:4b490931e75f 213 //pc.printf("%s", PC_data_string.c_str());
dem123456789 15:dbf20c1209ae 214 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 215 pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str());
dem123456789 16:0ea992d9a390 216 } else if (PC_data_string.substr(0,4) == "@SET") {
ypeng42 47:4b490931e75f 217 //pc.printf("%s", PC_data_string.c_str());
dem123456789 20:a820531c78bc 218 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 219 string claim = decodeCommandSET(PC_data_string);
dem123456789 20:a820531c78bc 220 if (claim == "DONE") {
ypeng42 47:4b490931e75f 221 //pc.printf("Current Path: Longitude, Latitude\n");
dem123456789 20:a820531c78bc 222 for (int i=0;i<MAX_TASK_SIZE;++i) {
ypeng42 47:4b490931e75f 223 //pc.printf(" %f, %f\n", Longitude_Path[i], Latitude_Path[i]);
dem123456789 20:a820531c78bc 224 }
dem123456789 20:a820531c78bc 225 }
ypeng42 47:4b490931e75f 226 //pc.printf("%s\n", claim.c_str());
dem123456789 38:528e410f2f7d 227 } else if(PC_data_string.substr(0,6) == "@Hello") {
ypeng42 47:4b490931e75f 228 //pc.printf("Successfully connected to mbed\n");
dem123456789 37:7a136279daf3 229 } else {
ypeng42 47:4b490931e75f 230 //pc.printf("Not supported command\n");
dem123456789 16:0ea992d9a390 231 }
dem123456789 15:dbf20c1209ae 232 }
dem123456789 15:dbf20c1209ae 233
dem123456789 15:dbf20c1209ae 234
taoqiuyang 24:8e38cc14150c 235
dem123456789 11:1caacb994236 236 void printStateIMU() {
dem123456789 23:cc06a8226f72 237 //pc.printf("IMU_Y: %s, IMU_P: %s, IMU_R: %s\n",IMU_Y.c_str(), IMU_P.c_str(), IMU_R.c_str());
dem123456789 23:cc06a8226f72 238 pc.printf("D_IMU_Y: %f, D_IMU_P: %f, D_IMU_R: %f\n",D_IMU_Y, D_IMU_P, D_IMU_R);
dem123456789 11:1caacb994236 239 }
dem123456789 11:1caacb994236 240
dem123456789 12:8644abfa86da 241 void printStateGPS() {
dem123456789 23:cc06a8226f72 242 /*
dem123456789 27:1be1f25be449 243 pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longitude: %s, GPS_Altitude: %s, "
dem123456789 14:92bacb5af01b 244 "GPS_Num_Satellite: %s, GPS_HDOP: %s, GPS_VDOP: %s, GPS_PDOP: %s, GPS_Date: %s, GPS_VelocityKnot: %s, GPS_VelocityKph: %s\n",
dem123456789 27:1be1f25be449 245 GPS_Quality.c_str(), GPS_UTC.c_str(), GPS_Latitude.c_str(), GPS_Longitude.c_str(), GPS_Altitude.c_str(), GPS_Num_Satellite.c_str(),
dem123456789 23:cc06a8226f72 246 GPS_HDOP.c_str(), GPS_VDOP.c_str(), GPS_PDOP.c_str(), GPS_Date.c_str(), GPS_VelocityKnot.c_str(), GPS_VelocityKph.c_str());
dem123456789 23:cc06a8226f72 247 */
dem123456789 23:cc06a8226f72 248
dem123456789 27:1be1f25be449 249 pc.printf("D_GPS_Quality: %f, D_GPS_UTC: %f, D_GPS_Latitude: %f, D_GPS_Latitude_Direction: %f, D_GPS_Longitude: %f, D_GPS_Longitude_Direction: %f, D_GPS_Altitude: %f,\n"
dem123456789 23:cc06a8226f72 250 "D_GPS_Num_Satellite: %f, D_GPS_HDOP: %f, D_GPS_VDOP: %f, D_GPS_PDOP: %f, D_GPS_Date: %f, D_GPS_VelocityKnot: %f, D_GPS_VelocityKph: %f\n",
dem123456789 27:1be1f25be449 251 D_GPS_Quality, D_GPS_UTC, D_GPS_Latitude, D_GPS_Latitude_Direction, D_GPS_Longitude, D_GPS_Longitude_Direction, D_GPS_Altitude, D_GPS_Num_Satellite,
dem123456789 23:cc06a8226f72 252 D_GPS_HDOP, D_GPS_VDOP, D_GPS_PDOP, D_GPS_Date, D_GPS_VelocityKnot, D_GPS_VelocityKph);
dem123456789 23:cc06a8226f72 253
dem123456789 12:8644abfa86da 254 }
dem123456789 14:92bacb5af01b 255
dem123456789 32:263d39ea6d3e 256 void printPath() {
dem123456789 32:263d39ea6d3e 257 pc.printf("Current Path: Longitude, Latitude\n");
dem123456789 32:263d39ea6d3e 258 for (int i=0;i<MAX_TASK_SIZE;++i) {
dem123456789 32:263d39ea6d3e 259 pc.printf(" %f, %f\n", Longitude_Path[i], Latitude_Path[i]);
dem123456789 32:263d39ea6d3e 260 }
dem123456789 32:263d39ea6d3e 261 }
dem123456789 32:263d39ea6d3e 262
dem123456789 34:610d315c1bab 263 void printDistance() {
dem123456789 34:610d315c1bab 264 pc.printf("Current Distance: Task id, Distance\n");
dem123456789 34:610d315c1bab 265 for(int i=0;i<MAX_TASK_SIZE;++i) {
dem123456789 35:009cc4509a90 266 dis[i] = getDistance(i+1);
dem123456789 32:263d39ea6d3e 267 pc.printf("Distance Task %d: %f\n", i+1, dis[i]);
dem123456789 32:263d39ea6d3e 268 }
dem123456789 32:263d39ea6d3e 269 }
dem123456789 32:263d39ea6d3e 270
dem123456789 32:263d39ea6d3e 271 void printAngle() {
dem123456789 34:610d315c1bab 272 pc.printf("Current Angle: Task id, Angle\n");
dem123456789 32:263d39ea6d3e 273 for(int i=0;i<MAX_TASK_SIZE;++i) {
dem123456789 35:009cc4509a90 274 ang[i] = getAngle(i+1);
dem123456789 32:263d39ea6d3e 275 pc.printf("Angle Task %d: %f\n", i+1, ang[i]);
dem123456789 32:263d39ea6d3e 276 }
dem123456789 32:263d39ea6d3e 277 }
dem123456789 32:263d39ea6d3e 278
dem123456789 11:1caacb994236 279 //#YPR=-183.74,-134.27,-114.39
taoqiuyang 4:37d118f2348c 280 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 281 char buf;
taoqiuyang 3:ab9f94d112c0 282
taoqiuyang 3:ab9f94d112c0 283 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 284 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 285
dem123456789 14:92bacb5af01b 286 //pc.putc(buf);
taoqiuyang 3:ab9f94d112c0 287 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 288 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 289
taoqiuyang 3:ab9f94d112c0 290 if (buf=='\n'){
dem123456789 9:bf5939466e86 291 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 11:1caacb994236 292 //pc.printf("%s", IMU_copy.c_str());
dem123456789 9:bf5939466e86 293 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 294 IMU_message_counter=0;
dem123456789 9:bf5939466e86 295 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 296 }
dem123456789 9:bf5939466e86 297
taoqiuyang 3:ab9f94d112c0 298 }
dem123456789 39:ef1a8055d744 299 //led2 = !led2;
taoqiuyang 2:afb333543af5 300 }
dem123456789 9:bf5939466e86 301
dem123456789 9:bf5939466e86 302
dem123456789 8:1f5a44bade4d 303
taoqiuyang 5:451b8203ef99 304 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 305 char buf;
taoqiuyang 5:451b8203ef99 306
taoqiuyang 5:451b8203ef99 307 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 308 buf = GPS.getc();
dem123456789 14:92bacb5af01b 309
dem123456789 14:92bacb5af01b 310 //pc.putc(buf);
dem123456789 10:12ba6ed2d6f0 311 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 312 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 313
dem123456789 10:12ba6ed2d6f0 314 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 315 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 11:1caacb994236 316 //pc.printf("%s", GPS_copy.c_str());
dem123456789 10:12ba6ed2d6f0 317 updateGPS(GPS_copy);
dem123456789 10:12ba6ed2d6f0 318 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 319 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 320 }
taoqiuyang 5:451b8203ef99 321 }
taoqiuyang 5:451b8203ef99 322
dem123456789 39:ef1a8055d744 323 //led3 = !led3;
taoqiuyang 5:451b8203ef99 324 }
taoqiuyang 3:ab9f94d112c0 325
dem123456789 8:1f5a44bade4d 326 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 327 char buf;
dem123456789 8:1f5a44bade4d 328
dem123456789 8:1f5a44bade4d 329 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 330 buf = pc.getc();
dem123456789 39:ef1a8055d744 331
dem123456789 14:92bacb5af01b 332 //pc.putc(buf);
dem123456789 14:92bacb5af01b 333 PC_message[PC_message_counter]=buf;
dem123456789 14:92bacb5af01b 334 PC_message_counter+=1;
dem123456789 14:92bacb5af01b 335
dem123456789 39:ef1a8055d744 336 if (buf=='\n'or buf =='#'){
dem123456789 14:92bacb5af01b 337 string PC_copy(PC_message, PC_message_counter);
dem123456789 14:92bacb5af01b 338 //pc.printf("%s", PC_copy.c_str());
dem123456789 14:92bacb5af01b 339 decodePC(PC_copy);
dem123456789 14:92bacb5af01b 340 PC_message_counter=0;
dem123456789 14:92bacb5af01b 341 PC_copy[0] = '\0';
dem123456789 14:92bacb5af01b 342 }
dem123456789 8:1f5a44bade4d 343 }
dem123456789 8:1f5a44bade4d 344
dem123456789 39:ef1a8055d744 345 //led4= !led4;
dem123456789 8:1f5a44bade4d 346 }
dem123456789 21:99be83550601 347
taoqiuyang 24:8e38cc14150c 348
dem123456789 21:99be83550601 349 void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
dem123456789 21:99be83550601 350 /*angleDeg: desired angle
dem123456789 21:99be83550601 351 minDeg: minimum angle in degrees
dem123456789 21:99be83550601 352 minNorVal: normalized value[0,1] when servo is at its minimum angle
dem123456789 21:99be83550601 353 maxDeg: maximum angle in degrees
dem123456789 21:99be83550601 354 maxNorVal: normalized value[0,1] when servo is at its maximum angle
dem123456789 21:99be83550601 355 */
dem123456789 21:99be83550601 356
dem123456789 21:99be83550601 357 float pos; //normalized angle, [0,1]
dem123456789 21:99be83550601 358 float scale; //scale factor for servo calibration
dem123456789 21:99be83550601 359
dem123456789 21:99be83550601 360 //extreme conditions
dem123456789 21:99be83550601 361 if (angleDeg<minDeg){
dem123456789 21:99be83550601 362 pos=minNorVal;
dem123456789 21:99be83550601 363 }
dem123456789 21:99be83550601 364 if (angleDeg>maxDeg){
dem123456789 21:99be83550601 365 pos=maxNorVal;
dem123456789 21:99be83550601 366 }
dem123456789 21:99be83550601 367
dem123456789 21:99be83550601 368 //Calculate scale factor for servo calibration
dem123456789 21:99be83550601 369 scale = (angleDeg-minDeg)/(maxDeg-minDeg);
dem123456789 21:99be83550601 370 //Calculate servo normalized value
dem123456789 21:99be83550601 371 pos = minNorVal + scale*(maxNorVal-minNorVal);
dem123456789 21:99be83550601 372
dem123456789 21:99be83550601 373 //send signal to servo
dem123456789 21:99be83550601 374 targetServo=pos;
dem123456789 21:99be83550601 375 }
dem123456789 21:99be83550601 376
taoqiuyang 26:353a80179346 377
taoqiuyang 26:353a80179346 378 void initialize_controller(){
taoqiuyang 26:353a80179346 379 rudder_variables[0]=0;
taoqiuyang 26:353a80179346 380 rudder_variables[1]=0;
taoqiuyang 26:353a80179346 381 rudder_variables[2]=0;
taoqiuyang 26:353a80179346 382 rudder_variables[3]=0;
taoqiuyang 26:353a80179346 383 rudder_variables[4]=0;
taoqiuyang 26:353a80179346 384 rudder_ctrl_parameters[0]=1;
taoqiuyang 26:353a80179346 385 rudder_ctrl_parameters[1]=0;
taoqiuyang 26:353a80179346 386 rudder_ctrl_parameters[2]=0;
taoqiuyang 26:353a80179346 387 }
taoqiuyang 26:353a80179346 388
dem123456789 46:305112d73c69 389
taoqiuyang 26:353a80179346 390 void update_controller_tmr_ISR() {
taoqiuyang 26:353a80179346 391 /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角
taoqiuyang 30:faf6e0f81a0c 392 angle to path point
taoqiuyang 30:faf6e0f81a0c 393 distance_to_route(meter)
taoqiuyang 30:faf6e0f81a0c 394
taoqiuyang 30:faf6e0f81a0c 395 Global Variables: angle_to_path_point,distance_to_path_point,distance_to_route;
taoqiuyang 26:353a80179346 396
taoqiuyang 26:353a80179346 397 Function: drive two servos to navigate the sailboat to the desired path point
taoqiuyang 26:353a80179346 398 */
taoqiuyang 31:e3339036c98b 399
taoqiuyang 41:d207b407c8bc 400
taoqiuyang 41:d207b407c8bc 401
taoqiuyang 31:e3339036c98b 402 //CTE based controller for rudder
taoqiuyang 30:faf6e0f81a0c 403 if (angle_to_path_point<0){distance_to_route=-1*distance_to_route;}
taoqiuyang 30:faf6e0f81a0c 404
taoqiuyang 30:faf6e0f81a0c 405 rudder_variables[0]=rudder_ctrl_parameters[0]*distance_to_route;
taoqiuyang 30:faf6e0f81a0c 406 rudder_variables[1]=rudder_variables[1]+rudder_ctrl_parameters[1]*distance_to_route*T;
taoqiuyang 30:faf6e0f81a0c 407 rudder_variables[2]=(rudder_variables[3]-distance_to_route)/T;
taoqiuyang 30:faf6e0f81a0c 408 rudder_variables[3]=distance_to_route;
taoqiuyang 26:353a80179346 409 rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2];
taoqiuyang 26:353a80179346 410
taoqiuyang 31:e3339036c98b 411 //bang-bang controller for vehicle velocity
taoqiuyang 31:e3339036c98b 412 //Our sailboat is a slow moving vehicle and GPS cannot provide
taoqiuyang 31:e3339036c98b 413 //very accurate speed reading in our application
taoqiuyang 31:e3339036c98b 414 if (distance_to_path_point>30){
taoqiuyang 40:9537722d185e 415 if (autonomous_mode){set_servo_position(wingServo,45,-45,0,45,1);}
taoqiuyang 31:e3339036c98b 416 }else{
taoqiuyang 40:9537722d185e 417 if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);}
taoqiuyang 31:e3339036c98b 418 }
taoqiuyang 31:e3339036c98b 419
taoqiuyang 31:e3339036c98b 420 //actuator saturation
taoqiuyang 41:d207b407c8bc 421 if (rudder_variables[4]> 45){rudder_variables[4]= 45;}
taoqiuyang 41:d207b407c8bc 422 if (rudder_variables[4]<-45){rudder_variables[4]=-45;}
taoqiuyang 26:353a80179346 423
taoqiuyang 26:353a80179346 424 //Drive servos
taoqiuyang 40:9537722d185e 425 if (autonomous_mode){set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);}
taoqiuyang 41:d207b407c8bc 426
taoqiuyang 41:d207b407c8bc 427 pc.printf("%f\n",rudder_variables[4]);
taoqiuyang 26:353a80179346 428 }
taoqiuyang 26:353a80179346 429
taoqiuyang 26:353a80179346 430
taoqiuyang 26:353a80179346 431
taoqiuyang 42:e1a2a6daf70b 432
taoqiuyang 42:e1a2a6daf70b 433 void update_controller_tmr_ISR2() {
taoqiuyang 42:e1a2a6daf70b 434 /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角
taoqiuyang 42:e1a2a6daf70b 435 distance(meter) to the next path point
taoqiuyang 42:e1a2a6daf70b 436 Global Variables: angle_to_path_point,distance_to_path_point,distance_to_route;
taoqiuyang 42:e1a2a6daf70b 437
taoqiuyang 42:e1a2a6daf70b 438 Function: drive two servos to navigate the sailboat to the desired path point
taoqiuyang 42:e1a2a6daf70b 439 */
taoqiuyang 42:e1a2a6daf70b 440
taoqiuyang 42:e1a2a6daf70b 441 rudder_variables[0]=rudder_ctrl_parameters[0]*angle_to_path_point;
taoqiuyang 42:e1a2a6daf70b 442 rudder_variables[1]=rudder_variables[1]+rudder_ctrl_parameters[1]*angle_to_path_point*T;
taoqiuyang 42:e1a2a6daf70b 443 rudder_variables[2]=(rudder_variables[3]-angle_to_path_point)/T;
taoqiuyang 42:e1a2a6daf70b 444 rudder_variables[3]=angle_to_path_point;
taoqiuyang 42:e1a2a6daf70b 445 rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2];
taoqiuyang 42:e1a2a6daf70b 446
taoqiuyang 42:e1a2a6daf70b 447 /*if (distance_to_route>0){
taoqiuyang 42:e1a2a6daf70b 448 rudder_variables[4]=rudder_variables[4]+10;
taoqiuyang 42:e1a2a6daf70b 449 }else{
taoqiuyang 42:e1a2a6daf70b 450 rudder_variables[4]=rudder_variables[4]-10;
taoqiuyang 42:e1a2a6daf70b 451 }*/
taoqiuyang 42:e1a2a6daf70b 452
taoqiuyang 42:e1a2a6daf70b 453
taoqiuyang 42:e1a2a6daf70b 454 //bang-bang controller for vehicle velocity
taoqiuyang 42:e1a2a6daf70b 455 //Our sailboat is a slow moving vehicle and GPS cannot provide
taoqiuyang 42:e1a2a6daf70b 456 //very accurate speed reading in our application
taoqiuyang 42:e1a2a6daf70b 457 if (distance_to_path_point>30){
taoqiuyang 42:e1a2a6daf70b 458 if (autonomous_mode){set_servo_position(wingServo,45,-45,0,45,1);}
taoqiuyang 43:a430d5e4f33d 459 arrived_destination=0;
taoqiuyang 42:e1a2a6daf70b 460 }else{
taoqiuyang 42:e1a2a6daf70b 461 if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);}
taoqiuyang 43:a430d5e4f33d 462 arrived_destination=1;
dem123456789 46:305112d73c69 463 IF_Path_Complete[current_task]=1;
taoqiuyang 42:e1a2a6daf70b 464 }
taoqiuyang 42:e1a2a6daf70b 465
taoqiuyang 42:e1a2a6daf70b 466 //actuator saturation
taoqiuyang 42:e1a2a6daf70b 467 if (rudder_variables[4]> 45){rudder_variables[4]= 45;}
taoqiuyang 42:e1a2a6daf70b 468 if (rudder_variables[4]<-45){rudder_variables[4]=-45;}
taoqiuyang 42:e1a2a6daf70b 469
taoqiuyang 42:e1a2a6daf70b 470 //Drive servos
taoqiuyang 42:e1a2a6daf70b 471 if (autonomous_mode){set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);}
taoqiuyang 42:e1a2a6daf70b 472
ypeng42 47:4b490931e75f 473 //pc.printf("%f\n",rudder_variables[4]);
taoqiuyang 42:e1a2a6daf70b 474 }
taoqiuyang 42:e1a2a6daf70b 475
taoqiuyang 42:e1a2a6daf70b 476
taoqiuyang 42:e1a2a6daf70b 477
taoqiuyang 42:e1a2a6daf70b 478
taoqiuyang 42:e1a2a6daf70b 479
dem123456789 21:99be83550601 480 int log_data_SD(){
dem123456789 21:99be83550601 481 FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
dem123456789 21:99be83550601 482 if(fp == NULL) {
dem123456789 21:99be83550601 483 return 0;
dem123456789 21:99be83550601 484 }else{
dem123456789 21:99be83550601 485 //Write all the useful data to the SD card
dem123456789 21:99be83550601 486 fprintf(fp, "Nya Pass~");
dem123456789 21:99be83550601 487 fclose(fp);
dem123456789 21:99be83550601 488 return 1;
dem123456789 21:99be83550601 489 }
dem123456789 21:99be83550601 490 }
dem123456789 21:99be83550601 491
dem123456789 21:99be83550601 492
taoqiuyang 0:f4d390c06705 493 int main() {
taoqiuyang 2:afb333543af5 494 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 495 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 496 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 497 GPS.attach(&GPS_serial_ISR);
dem123456789 36:f04f4ed6aabb 498 pc.baud(9600);
dem123456789 8:1f5a44bade4d 499 pc.attach(&PC_serial_ISR);
taoqiuyang 42:e1a2a6daf70b 500 //ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz
taoqiuyang 42:e1a2a6daf70b 501 ctrl_updt_timer.attach(&update_controller_tmr_ISR2, T); // Update controller at 1/T Hz
taoqiuyang 26:353a80179346 502 initialize_controller();
dem123456789 33:37345814fad0 503 initialize();
dem123456789 23:cc06a8226f72 504 //float angle=20;
taoqiuyang 2:afb333543af5 505 while (1) {
dem123456789 46:305112d73c69 506 current_task = get_current_task();
dem123456789 46:305112d73c69 507 angle_to_path_point=getAngle(current_task); //positive if pathpoint is on the right of the boat
dem123456789 46:305112d73c69 508 distance_to_path_point=getCTE(current_task); //positive if trace is on the right of the boat
dem123456789 46:305112d73c69 509 distance_to_route=getDistance(current_task); //no sign
taoqiuyang 40:9537722d185e 510
taoqiuyang 43:a430d5e4f33d 511 arrived_destination=0; //will be 1 if arrived destination
taoqiuyang 43:a430d5e4f33d 512 autonomous_mode=0; //set 1 if in autonomous mode, set 0 bypass the controller
taoqiuyang 40:9537722d185e 513
taoqiuyang 40:9537722d185e 514
taoqiuyang 40:9537722d185e 515
dem123456789 23:cc06a8226f72 516 // if (angle>160){angle=20;}
dem123456789 23:cc06a8226f72 517 // set_servo_position(rudderServo, angle, 0, 0, 180, 1);
dem123456789 23:cc06a8226f72 518 // set_servo_position(wingServo, angle, 0, 0, 180, 1);
dem123456789 23:cc06a8226f72 519 // angle=angle+10;
dem123456789 39:ef1a8055d744 520
taoqiuyang 43:a430d5e4f33d 521 wait(2);
dem123456789 37:7a136279daf3 522 //printStateIMU();
dem123456789 32:263d39ea6d3e 523 //printStateGPS();
dem123456789 32:263d39ea6d3e 524 //printPath();
dem123456789 35:009cc4509a90 525 //printDistance();
dem123456789 36:f04f4ed6aabb 526 //printAngle();
dem123456789 39:ef1a8055d744 527 //led1 = !led1;
dem123456789 39:ef1a8055d744 528
taoqiuyang 0:f4d390c06705 529 }
taoqiuyang 0:f4d390c06705 530 }