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@31:e3339036c98b, 2015-09-27 (annotated)

Committer:
taoqiuyang
Date:
Sun Sep 27 00:58:18 2015 +0000
Revision:
31:e3339036c98b
Parent:
30:faf6e0f81a0c
Child:
32:263d39ea6d3e
Added simple bang-bang controller for speed

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 20:a820531c78bc 7
taoqiuyang 2:afb333543af5 8 Serial pc(USBTX, USBRX);
taoqiuyang 2:afb333543af5 9 Serial IMU(p28, p27); // tx, rx
taoqiuyang 5:451b8203ef99 10 Serial GPS(p13, p14); // tx, rx
dem123456789 21:99be83550601 11 Servo rudderServo(p25);
dem123456789 21:99be83550601 12 Servo wingServo(p26);
dem123456789 21:99be83550601 13 SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs
taoqiuyang 26:353a80179346 14 Ticker ctrl_updt_timer; //timer to refresh controller
taoqiuyang 2:afb333543af5 15
taoqiuyang 2:afb333543af5 16 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 17 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 18 char GPS_message[256];
taoqiuyang 5:451b8203ef99 19 int GPS_message_counter=0;
dem123456789 14:92bacb5af01b 20 char PC_message[256];
dem123456789 14:92bacb5af01b 21 int PC_message_counter=0;
dem123456789 11:1caacb994236 22
dem123456789 11:1caacb994236 23 string IMU_Y="N/A";
dem123456789 11:1caacb994236 24 string IMU_P="N/A";
dem123456789 11:1caacb994236 25 string IMU_R="N/A";
dem123456789 14:92bacb5af01b 26 string GPS_Quality="N/A";
dem123456789 11:1caacb994236 27 string GPS_UTC="N/A";
dem123456789 11:1caacb994236 28 string GPS_Latitude="N/A";
dem123456789 27:1be1f25be449 29 string GPS_Longitude="N/A";
dem123456789 11:1caacb994236 30 string GPS_Altitude="N/A";
dem123456789 22:faba67585854 31 string GPS_Altitude_Unit="N/A";
dem123456789 11:1caacb994236 32 string GPS_Num_Satellite="N/A";
dem123456789 11:1caacb994236 33 string GPS_HDOP="N/A";
dem123456789 12:8644abfa86da 34 string GPS_VDOP="N/A";
dem123456789 12:8644abfa86da 35 string GPS_PDOP="N/A";
dem123456789 14:92bacb5af01b 36 string GPS_Date="N/A";
dem123456789 14:92bacb5af01b 37 string GPS_VelocityKnot="N/A";
dem123456789 22:faba67585854 38 string GPS_VelocityKnot_Unit="N/A";
dem123456789 14:92bacb5af01b 39 string GPS_VelocityKph="N/A";
dem123456789 22:faba67585854 40 string GPS_VelocityKph_Unit="N/A";
dem123456789 23:cc06a8226f72 41 string temp = "";
dem123456789 22:faba67585854 42
dem123456789 22:faba67585854 43 double D_IMU_Y=0;
dem123456789 22:faba67585854 44 double D_IMU_P=0;
dem123456789 22:faba67585854 45 double D_IMU_R=0;
dem123456789 22:faba67585854 46 double D_GPS_Quality=0;
dem123456789 22:faba67585854 47 double D_GPS_UTC=0;
dem123456789 22:faba67585854 48 double D_GPS_Latitude=0;
dem123456789 22:faba67585854 49 double D_GPS_Latitude_Direction=0;
dem123456789 27:1be1f25be449 50 double D_GPS_Longitude=0;
dem123456789 27:1be1f25be449 51 double D_GPS_Longitude_Direction=0;
dem123456789 22:faba67585854 52 double D_GPS_Altitude=0;
dem123456789 22:faba67585854 53 double D_GPS_Num_Satellite=0;
dem123456789 22:faba67585854 54 double D_GPS_HDOP=0;
dem123456789 22:faba67585854 55 double D_GPS_VDOP=0;
dem123456789 22:faba67585854 56 double D_GPS_PDOP=0;
dem123456789 22:faba67585854 57 double D_GPS_Date=0;
dem123456789 22:faba67585854 58 double D_GPS_VelocityKnot=0;
dem123456789 22:faba67585854 59 double D_GPS_VelocityKph=0;
dem123456789 27:1be1f25be449 60 double D_temp=0;
dem123456789 12:8644abfa86da 61 int asterisk_idx;
dem123456789 9:bf5939466e86 62
dem123456789 27:1be1f25be449 63 double Longitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 64 double Latitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 65
taoqiuyang 26:353a80179346 66 double angle_to_path_point,distance_to_path_point,desired_speed;
taoqiuyang 26:353a80179346 67 double rudder_ctrl_parameters[3];
taoqiuyang 26:353a80179346 68 double rudder_variables[5];//,,,prev,out
taoqiuyang 30:faf6e0f81a0c 69 double distance_to_route;
taoqiuyang 26:353a80179346 70 double T=0.2; //controller update period=0.2sec, 5Hz
taoqiuyang 26:353a80179346 71
taoqiuyang 26:353a80179346 72
dem123456789 9:bf5939466e86 73 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 74 stringstream ss(s);
dem123456789 9:bf5939466e86 75 string item;
dem123456789 9:bf5939466e86 76 vector<string> tokens;
dem123456789 9:bf5939466e86 77 while (getline(ss, item, delim)) {
dem123456789 15:dbf20c1209ae 78 if (item.empty()) {
dem123456789 20:a820531c78bc 79 item = "N/A";
dem123456789 15:dbf20c1209ae 80 }
dem123456789 9:bf5939466e86 81 tokens.push_back(item);
dem123456789 9:bf5939466e86 82 }
dem123456789 9:bf5939466e86 83 return tokens;
dem123456789 9:bf5939466e86 84 }
dem123456789 9:bf5939466e86 85
dem123456789 27:1be1f25be449 86 double GPSdecimal(double coordinate) {
dem123456789 27:1be1f25be449 87 //Convert a NMEA decimal-decimal value into decimal degree value
dem123456789 27:1be1f25be449 88 //5144.3855 (ddmm.mmmm) = 51 44.3855 = 51 + 44.3855/60 = 51.7397583 degrees
dem123456789 27:1be1f25be449 89 coordinate = coordinate/100;
dem123456789 27:1be1f25be449 90 return ((int) coordinate) + ((coordinate-((int) coordinate))/60);
dem123456789 27:1be1f25be449 91 }
dem123456789 27:1be1f25be449 92
dem123456789 29:95b0320bf779 93 void initialize_Path() {
dem123456789 29:95b0320bf779 94 fill(Longitude_Path, Longitude_Path+MAX_TASK_SIZE, 181);
dem123456789 29:95b0320bf779 95 fill(Latitude_Path, Latitude_Path+MAX_TASK_SIZE, 181);
dem123456789 29:95b0320bf779 96 }
dem123456789 29:95b0320bf779 97
dem123456789 9:bf5939466e86 98 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 99 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 100 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 101 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 102 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 103 IMU_Y = result.at(0);
dem123456789 23:cc06a8226f72 104 D_IMU_Y = strtod(IMU_Y.c_str(), NULL);
dem123456789 9:bf5939466e86 105 IMU_P = result.at(1);
dem123456789 23:cc06a8226f72 106 D_IMU_P = strtod(IMU_P.c_str(), NULL);
dem123456789 15:dbf20c1209ae 107 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 23:cc06a8226f72 108 D_IMU_R = strtod(IMU_R.c_str(), NULL);
dem123456789 9:bf5939466e86 109 }
dem123456789 9:bf5939466e86 110 }
dem123456789 9:bf5939466e86 111
dem123456789 10:12ba6ed2d6f0 112 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 113 string GPS_data_string(GPS_data);
dem123456789 11:1caacb994236 114 if (GPS_data_string.substr(0,6) == "$GPGGA") {
dem123456789 11:1caacb994236 115 GPS_data_string = GPS_data_string.substr(7);
dem123456789 10:12ba6ed2d6f0 116 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 117 GPS_Quality = result.at(5);
dem123456789 22:faba67585854 118 D_GPS_Quality = strtod(result.at(5).c_str(), NULL);
dem123456789 14:92bacb5af01b 119 if(GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 11:1caacb994236 120 GPS_UTC = result.at(0);
dem123456789 23:cc06a8226f72 121 D_GPS_UTC = strtod(GPS_UTC.c_str(), NULL);
dem123456789 23:cc06a8226f72 122 GPS_Latitude = result.at(1) + result.at(2);
dem123456789 23:cc06a8226f72 123 if (result.at(2) == "N") { // 0 means North
dem123456789 22:faba67585854 124 D_GPS_Latitude_Direction = 0;
dem123456789 23:cc06a8226f72 125 } else if (result.at(2) == "S") { // 1 means South
dem123456789 27:1be1f25be449 126 D_GPS_Latitude_Direction = 1;
dem123456789 27:1be1f25be449 127 temp = "-" + result.at(1);
dem123456789 22:faba67585854 128 }
dem123456789 27:1be1f25be449 129 D_temp = strtod(temp.c_str(), NULL);
dem123456789 27:1be1f25be449 130 D_GPS_Latitude = GPSdecimal(D_temp);
dem123456789 27:1be1f25be449 131 GPS_Longitude = result.at(3) + result.at(4);
dem123456789 23:cc06a8226f72 132 if (result.at(4) == "E") { // 0 means East
dem123456789 27:1be1f25be449 133 D_GPS_Longitude_Direction = 0;
dem123456789 23:cc06a8226f72 134 } else if (result.at(4) == "W") { // 1 means West
dem123456789 27:1be1f25be449 135 D_GPS_Longitude_Direction = 1;
dem123456789 27:1be1f25be449 136 temp = "-" + result.at(3);
dem123456789 22:faba67585854 137 }
dem123456789 27:1be1f25be449 138 D_temp = strtod(temp.c_str(), NULL);
dem123456789 27:1be1f25be449 139 D_GPS_Longitude = GPSdecimal(D_temp);
dem123456789 11:1caacb994236 140 GPS_Num_Satellite = result.at(6);
dem123456789 22:faba67585854 141 D_GPS_Num_Satellite = strtod(result.at(6).c_str(), NULL);
dem123456789 11:1caacb994236 142 GPS_HDOP = result.at(7);
dem123456789 22:faba67585854 143 D_GPS_HDOP = strtod(result.at(7).c_str(), NULL);
dem123456789 11:1caacb994236 144 GPS_Altitude = result.at(8) + result.at(9);
dem123456789 22:faba67585854 145 D_GPS_Altitude = strtod(result.at(8).c_str(), NULL);
dem123456789 22:faba67585854 146 GPS_Altitude_Unit = result.at(9);
dem123456789 11:1caacb994236 147 }
dem123456789 14:92bacb5af01b 148 } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 149 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 150 vector<string> result = split(GPS_data_string, ',');
dem123456789 12:8644abfa86da 151 GPS_PDOP = result.at(14);
dem123456789 22:faba67585854 152 D_GPS_PDOP = strtod(result.at(14).c_str(), NULL);
dem123456789 12:8644abfa86da 153 asterisk_idx = result.at(16).find('*');
dem123456789 12:8644abfa86da 154 GPS_VDOP = result.at(16).substr(0, asterisk_idx);
dem123456789 22:faba67585854 155 D_GPS_VDOP = strtod(result.at(16).substr(0, asterisk_idx).c_str(), NULL);
dem123456789 14:92bacb5af01b 156 } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 157 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 158 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 159 GPS_Date = result.at(8);
dem123456789 22:faba67585854 160 D_GPS_Date = strtod(result.at(8).c_str(), NULL);
dem123456789 14:92bacb5af01b 161 } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 162 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 163 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 164 GPS_VelocityKnot = result.at(4) + result.at(5);
dem123456789 22:faba67585854 165 D_GPS_VelocityKnot = strtod(result.at(4).c_str(), NULL);
dem123456789 22:faba67585854 166 GPS_VelocityKnot_Unit = result.at(5);
dem123456789 12:8644abfa86da 167 asterisk_idx = result.at(7).find('*');
dem123456789 14:92bacb5af01b 168 GPS_VelocityKph = result.at(6) + result.at(7).substr(0, asterisk_idx);
dem123456789 22:faba67585854 169 D_GPS_VelocityKph = strtod(result.at(6).c_str(), NULL);
dem123456789 22:faba67585854 170 GPS_VelocityKph_Unit = result.at(7).substr(0, asterisk_idx);
dem123456789 10:12ba6ed2d6f0 171 }
dem123456789 12:8644abfa86da 172
dem123456789 10:12ba6ed2d6f0 173 }
dem123456789 16:0ea992d9a390 174 /* Get data from SailBoat
dem123456789 16:0ea992d9a390 175 @GET=parameter
dem123456789 16:0ea992d9a390 176 Ex: @GET=GPS_Quality
dem123456789 27:1be1f25be449 177 Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longitude, GPS_Altitude,
dem123456789 16:0ea992d9a390 178 GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph
dem123456789 16:0ea992d9a390 179 Set path to SailBoat
dem123456789 27:1be1f25be449 180 @SET=PATH, Latitude, Longitude, Task id
dem123456789 20:a820531c78bc 181 @SET=PATH, 33.776318, -84.407590, 3
dem123456789 16:0ea992d9a390 182 */
dem123456789 15:dbf20c1209ae 183 void decodePC(string PC_data) {
dem123456789 15:dbf20c1209ae 184 string PC_data_string(PC_data);
dem123456789 15:dbf20c1209ae 185 if (PC_data_string.substr(0,4) == "@GET") {
dem123456789 15:dbf20c1209ae 186 pc.printf("%s", PC_data_string.c_str());
dem123456789 15:dbf20c1209ae 187 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 188 pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str());
dem123456789 16:0ea992d9a390 189 } else if (PC_data_string.substr(0,4) == "@SET") {
dem123456789 20:a820531c78bc 190 pc.printf("%s", PC_data_string.c_str());
dem123456789 20:a820531c78bc 191 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 192 string claim = decodeCommandSET(PC_data_string);
dem123456789 20:a820531c78bc 193 if (claim == "DONE") {
dem123456789 27:1be1f25be449 194 pc.printf("Current Path: Longitude, Latitude\n");
dem123456789 20:a820531c78bc 195 for (int i=0;i<MAX_TASK_SIZE;++i) {
dem123456789 27:1be1f25be449 196 pc.printf(" %f, %f\n", Longitude_Path[i], Latitude_Path[i]);
dem123456789 20:a820531c78bc 197 }
dem123456789 20:a820531c78bc 198 }
dem123456789 20:a820531c78bc 199 pc.printf("%s\n", claim.c_str());
dem123456789 16:0ea992d9a390 200 }
dem123456789 15:dbf20c1209ae 201 }
dem123456789 15:dbf20c1209ae 202
dem123456789 15:dbf20c1209ae 203
taoqiuyang 24:8e38cc14150c 204
dem123456789 11:1caacb994236 205 void printStateIMU() {
dem123456789 23:cc06a8226f72 206 //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 207 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 208 }
dem123456789 11:1caacb994236 209
dem123456789 12:8644abfa86da 210 void printStateGPS() {
dem123456789 23:cc06a8226f72 211 /*
dem123456789 27:1be1f25be449 212 pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longitude: %s, GPS_Altitude: %s, "
dem123456789 14:92bacb5af01b 213 "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 214 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 215 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 216 */
dem123456789 23:cc06a8226f72 217
dem123456789 27:1be1f25be449 218 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 219 "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 220 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 221 D_GPS_HDOP, D_GPS_VDOP, D_GPS_PDOP, D_GPS_Date, D_GPS_VelocityKnot, D_GPS_VelocityKph);
dem123456789 23:cc06a8226f72 222
dem123456789 12:8644abfa86da 223 }
dem123456789 14:92bacb5af01b 224
dem123456789 11:1caacb994236 225 //#YPR=-183.74,-134.27,-114.39
taoqiuyang 4:37d118f2348c 226 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 227 char buf;
taoqiuyang 3:ab9f94d112c0 228
taoqiuyang 3:ab9f94d112c0 229 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 230 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 231
dem123456789 14:92bacb5af01b 232 //pc.putc(buf);
taoqiuyang 3:ab9f94d112c0 233 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 234 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 235
taoqiuyang 3:ab9f94d112c0 236 if (buf=='\n'){
dem123456789 9:bf5939466e86 237 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 11:1caacb994236 238 //pc.printf("%s", IMU_copy.c_str());
dem123456789 9:bf5939466e86 239 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 240 IMU_message_counter=0;
dem123456789 9:bf5939466e86 241 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 242 }
dem123456789 9:bf5939466e86 243
taoqiuyang 3:ab9f94d112c0 244 }
taoqiuyang 2:afb333543af5 245 led2 = !led2;
taoqiuyang 2:afb333543af5 246 }
dem123456789 9:bf5939466e86 247
dem123456789 9:bf5939466e86 248
dem123456789 8:1f5a44bade4d 249
taoqiuyang 5:451b8203ef99 250 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 251 char buf;
taoqiuyang 5:451b8203ef99 252
taoqiuyang 5:451b8203ef99 253 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 254 buf = GPS.getc();
dem123456789 14:92bacb5af01b 255
dem123456789 14:92bacb5af01b 256 //pc.putc(buf);
dem123456789 10:12ba6ed2d6f0 257 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 258 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 259
dem123456789 10:12ba6ed2d6f0 260 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 261 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 11:1caacb994236 262 //pc.printf("%s", GPS_copy.c_str());
dem123456789 10:12ba6ed2d6f0 263 updateGPS(GPS_copy);
dem123456789 10:12ba6ed2d6f0 264 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 265 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 266 }
taoqiuyang 5:451b8203ef99 267 }
taoqiuyang 5:451b8203ef99 268
taoqiuyang 5:451b8203ef99 269 led3 = !led3;
taoqiuyang 5:451b8203ef99 270 }
taoqiuyang 3:ab9f94d112c0 271
dem123456789 8:1f5a44bade4d 272 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 273 char buf;
dem123456789 8:1f5a44bade4d 274
dem123456789 8:1f5a44bade4d 275 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 276 buf = pc.getc();
dem123456789 14:92bacb5af01b 277
dem123456789 14:92bacb5af01b 278 //pc.putc(buf);
dem123456789 14:92bacb5af01b 279 PC_message[PC_message_counter]=buf;
dem123456789 14:92bacb5af01b 280 PC_message_counter+=1;
dem123456789 14:92bacb5af01b 281
dem123456789 14:92bacb5af01b 282 if (buf=='\n'){
dem123456789 14:92bacb5af01b 283 string PC_copy(PC_message, PC_message_counter);
dem123456789 14:92bacb5af01b 284 //pc.printf("%s", PC_copy.c_str());
dem123456789 14:92bacb5af01b 285 decodePC(PC_copy);
dem123456789 14:92bacb5af01b 286 PC_message_counter=0;
dem123456789 14:92bacb5af01b 287 PC_copy[0] = '\0';
dem123456789 14:92bacb5af01b 288 }
dem123456789 8:1f5a44bade4d 289 }
dem123456789 8:1f5a44bade4d 290
dem123456789 8:1f5a44bade4d 291 led4= !led4;
dem123456789 8:1f5a44bade4d 292 }
dem123456789 21:99be83550601 293
taoqiuyang 24:8e38cc14150c 294
dem123456789 21:99be83550601 295 void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
dem123456789 21:99be83550601 296 /*angleDeg: desired angle
dem123456789 21:99be83550601 297 minDeg: minimum angle in degrees
dem123456789 21:99be83550601 298 minNorVal: normalized value[0,1] when servo is at its minimum angle
dem123456789 21:99be83550601 299 maxDeg: maximum angle in degrees
dem123456789 21:99be83550601 300 maxNorVal: normalized value[0,1] when servo is at its maximum angle
dem123456789 21:99be83550601 301 */
dem123456789 21:99be83550601 302
dem123456789 21:99be83550601 303 float pos; //normalized angle, [0,1]
dem123456789 21:99be83550601 304 float scale; //scale factor for servo calibration
dem123456789 21:99be83550601 305
dem123456789 21:99be83550601 306 //extreme conditions
dem123456789 21:99be83550601 307 if (angleDeg<minDeg){
dem123456789 21:99be83550601 308 pos=minNorVal;
dem123456789 21:99be83550601 309 }
dem123456789 21:99be83550601 310 if (angleDeg>maxDeg){
dem123456789 21:99be83550601 311 pos=maxNorVal;
dem123456789 21:99be83550601 312 }
dem123456789 21:99be83550601 313
dem123456789 21:99be83550601 314 //Calculate scale factor for servo calibration
dem123456789 21:99be83550601 315 scale = (angleDeg-minDeg)/(maxDeg-minDeg);
dem123456789 21:99be83550601 316 //Calculate servo normalized value
dem123456789 21:99be83550601 317 pos = minNorVal + scale*(maxNorVal-minNorVal);
dem123456789 21:99be83550601 318
dem123456789 21:99be83550601 319 //send signal to servo
dem123456789 21:99be83550601 320 targetServo=pos;
dem123456789 21:99be83550601 321 }
dem123456789 21:99be83550601 322
taoqiuyang 26:353a80179346 323
taoqiuyang 26:353a80179346 324 void initialize_controller(){
taoqiuyang 26:353a80179346 325 rudder_variables[0]=0;
taoqiuyang 26:353a80179346 326 rudder_variables[1]=0;
taoqiuyang 26:353a80179346 327 rudder_variables[2]=0;
taoqiuyang 26:353a80179346 328 rudder_variables[3]=0;
taoqiuyang 26:353a80179346 329 rudder_variables[4]=0;
taoqiuyang 26:353a80179346 330 rudder_ctrl_parameters[0]=1;
taoqiuyang 26:353a80179346 331 rudder_ctrl_parameters[1]=0;
taoqiuyang 26:353a80179346 332 rudder_ctrl_parameters[2]=0;
taoqiuyang 26:353a80179346 333 }
taoqiuyang 26:353a80179346 334
taoqiuyang 26:353a80179346 335 void update_controller_tmr_ISR() {
taoqiuyang 26:353a80179346 336 /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角
taoqiuyang 30:faf6e0f81a0c 337 angle to path point
taoqiuyang 30:faf6e0f81a0c 338 distance_to_route(meter)
taoqiuyang 30:faf6e0f81a0c 339
taoqiuyang 30:faf6e0f81a0c 340 Global Variables: angle_to_path_point,distance_to_path_point,distance_to_route;
taoqiuyang 26:353a80179346 341
taoqiuyang 26:353a80179346 342 Function: drive two servos to navigate the sailboat to the desired path point
taoqiuyang 26:353a80179346 343 */
taoqiuyang 31:e3339036c98b 344
taoqiuyang 31:e3339036c98b 345 //CTE based controller for rudder
taoqiuyang 30:faf6e0f81a0c 346 if (angle_to_path_point<0){distance_to_route=-1*distance_to_route;}
taoqiuyang 30:faf6e0f81a0c 347
taoqiuyang 30:faf6e0f81a0c 348 rudder_variables[0]=rudder_ctrl_parameters[0]*distance_to_route;
taoqiuyang 30:faf6e0f81a0c 349 rudder_variables[1]=rudder_variables[1]+rudder_ctrl_parameters[1]*distance_to_route*T;
taoqiuyang 30:faf6e0f81a0c 350 rudder_variables[2]=(rudder_variables[3]-distance_to_route)/T;
taoqiuyang 30:faf6e0f81a0c 351 rudder_variables[3]=distance_to_route;
taoqiuyang 26:353a80179346 352 rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2];
taoqiuyang 26:353a80179346 353
taoqiuyang 31:e3339036c98b 354 //bang-bang controller for vehicle velocity
taoqiuyang 31:e3339036c98b 355 //Our sailboat is a slow moving vehicle and GPS cannot provide
taoqiuyang 31:e3339036c98b 356 //very accurate speed reading in our application
taoqiuyang 31:e3339036c98b 357 if (distance_to_path_point>30){
taoqiuyang 31:e3339036c98b 358 set_servo_position(wingServo,45,-45,0,45,1);
taoqiuyang 31:e3339036c98b 359 }else{
taoqiuyang 31:e3339036c98b 360 set_servo_position(wingServo,0,-45,0,45,1);
taoqiuyang 31:e3339036c98b 361 }
taoqiuyang 31:e3339036c98b 362
taoqiuyang 31:e3339036c98b 363 //actuator saturation
taoqiuyang 31:e3339036c98b 364 if (rudder_variables[4]> 60){rudder_variables[4]= 60;}
taoqiuyang 31:e3339036c98b 365 if (rudder_variables[4]<-60){rudder_variables[4]=-60;}
taoqiuyang 26:353a80179346 366
taoqiuyang 26:353a80179346 367 //Drive servos
taoqiuyang 26:353a80179346 368 set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);
taoqiuyang 26:353a80179346 369 }
taoqiuyang 26:353a80179346 370
taoqiuyang 26:353a80179346 371
taoqiuyang 26:353a80179346 372
dem123456789 21:99be83550601 373 int log_data_SD(){
dem123456789 21:99be83550601 374 FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
dem123456789 21:99be83550601 375 if(fp == NULL) {
dem123456789 21:99be83550601 376 return 0;
dem123456789 21:99be83550601 377 }else{
dem123456789 21:99be83550601 378 //Write all the useful data to the SD card
dem123456789 21:99be83550601 379 fprintf(fp, "Nya Pass~");
dem123456789 21:99be83550601 380 fclose(fp);
dem123456789 21:99be83550601 381 return 1;
dem123456789 21:99be83550601 382 }
dem123456789 21:99be83550601 383 }
dem123456789 21:99be83550601 384
dem123456789 21:99be83550601 385
taoqiuyang 0:f4d390c06705 386 int main() {
taoqiuyang 2:afb333543af5 387 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 388 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 389 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 390 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 391 pc.baud(115200);
dem123456789 8:1f5a44bade4d 392 pc.attach(&PC_serial_ISR);
taoqiuyang 26:353a80179346 393 ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz
taoqiuyang 26:353a80179346 394
taoqiuyang 26:353a80179346 395 initialize_controller();
dem123456789 29:95b0320bf779 396 initialize_Path();
dem123456789 23:cc06a8226f72 397 //float angle=20;
taoqiuyang 2:afb333543af5 398 while (1) {
dem123456789 23:cc06a8226f72 399 // if (angle>160){angle=20;}
dem123456789 23:cc06a8226f72 400 // set_servo_position(rudderServo, angle, 0, 0, 180, 1);
dem123456789 23:cc06a8226f72 401 // set_servo_position(wingServo, angle, 0, 0, 180, 1);
dem123456789 23:cc06a8226f72 402 // angle=angle+10;
taoqiuyang 26:353a80179346 403
taoqiuyang 26:353a80179346 404
dem123456789 15:dbf20c1209ae 405 wait(0.4);
dem123456789 23:cc06a8226f72 406 printStateIMU();
dem123456789 23:cc06a8226f72 407 printStateGPS();
dem123456789 21:99be83550601 408 led1 = !led1;
taoqiuyang 0:f4d390c06705 409 }
taoqiuyang 0:f4d390c06705 410 }