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@24:8e38cc14150c, 2015-09-10 (annotated)

Committer:
taoqiuyang
Date:
Thu Sep 10 00:09:31 2015 +0000
Revision:
24:8e38cc14150c
Parent:
23:cc06a8226f72
Child:
26:353a80179346
shell code added for navigation

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 2:afb333543af5 14
taoqiuyang 2:afb333543af5 15 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 16 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 17 char GPS_message[256];
taoqiuyang 5:451b8203ef99 18 int GPS_message_counter=0;
dem123456789 14:92bacb5af01b 19 char PC_message[256];
dem123456789 14:92bacb5af01b 20 int PC_message_counter=0;
dem123456789 11:1caacb994236 21
dem123456789 11:1caacb994236 22 string IMU_Y="N/A";
dem123456789 11:1caacb994236 23 string IMU_P="N/A";
dem123456789 11:1caacb994236 24 string IMU_R="N/A";
dem123456789 14:92bacb5af01b 25 string GPS_Quality="N/A";
dem123456789 11:1caacb994236 26 string GPS_UTC="N/A";
dem123456789 11:1caacb994236 27 string GPS_Latitude="N/A";
dem123456789 11:1caacb994236 28 string GPS_Longtitude="N/A";
dem123456789 11:1caacb994236 29 string GPS_Altitude="N/A";
dem123456789 22:faba67585854 30 string GPS_Altitude_Unit="N/A";
dem123456789 11:1caacb994236 31 string GPS_Num_Satellite="N/A";
dem123456789 11:1caacb994236 32 string GPS_HDOP="N/A";
dem123456789 12:8644abfa86da 33 string GPS_VDOP="N/A";
dem123456789 12:8644abfa86da 34 string GPS_PDOP="N/A";
dem123456789 14:92bacb5af01b 35 string GPS_Date="N/A";
dem123456789 14:92bacb5af01b 36 string GPS_VelocityKnot="N/A";
dem123456789 22:faba67585854 37 string GPS_VelocityKnot_Unit="N/A";
dem123456789 14:92bacb5af01b 38 string GPS_VelocityKph="N/A";
dem123456789 22:faba67585854 39 string GPS_VelocityKph_Unit="N/A";
dem123456789 23:cc06a8226f72 40 string temp = "";
dem123456789 22:faba67585854 41
dem123456789 22:faba67585854 42 double D_IMU_Y=0;
dem123456789 22:faba67585854 43 double D_IMU_P=0;
dem123456789 22:faba67585854 44 double D_IMU_R=0;
dem123456789 22:faba67585854 45 double D_GPS_Quality=0;
dem123456789 22:faba67585854 46 double D_GPS_UTC=0;
dem123456789 22:faba67585854 47 double D_GPS_Latitude=0;
dem123456789 22:faba67585854 48 double D_GPS_Latitude_Direction=0;
dem123456789 22:faba67585854 49 double D_GPS_Longtitude=0;
dem123456789 22:faba67585854 50 double D_GPS_Longtitude_Direction=0;
dem123456789 22:faba67585854 51 double D_GPS_Altitude=0;
dem123456789 22:faba67585854 52 double D_GPS_Num_Satellite=0;
dem123456789 22:faba67585854 53 double D_GPS_HDOP=0;
dem123456789 22:faba67585854 54 double D_GPS_VDOP=0;
dem123456789 22:faba67585854 55 double D_GPS_PDOP=0;
dem123456789 22:faba67585854 56 double D_GPS_Date=0;
dem123456789 22:faba67585854 57 double D_GPS_VelocityKnot=0;
dem123456789 22:faba67585854 58 double D_GPS_VelocityKph=0;
dem123456789 14:92bacb5af01b 59
dem123456789 12:8644abfa86da 60 int asterisk_idx;
dem123456789 9:bf5939466e86 61
dem123456789 20:a820531c78bc 62 double Longtitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 63 double Latitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 64
dem123456789 9:bf5939466e86 65 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 66 stringstream ss(s);
dem123456789 9:bf5939466e86 67 string item;
dem123456789 9:bf5939466e86 68 vector<string> tokens;
dem123456789 9:bf5939466e86 69 while (getline(ss, item, delim)) {
dem123456789 15:dbf20c1209ae 70 if (item.empty()) {
dem123456789 20:a820531c78bc 71 item = "N/A";
dem123456789 15:dbf20c1209ae 72 }
dem123456789 9:bf5939466e86 73 tokens.push_back(item);
dem123456789 9:bf5939466e86 74 }
dem123456789 9:bf5939466e86 75 return tokens;
dem123456789 9:bf5939466e86 76 }
dem123456789 9:bf5939466e86 77
dem123456789 9:bf5939466e86 78 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 79 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 80 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 81 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 82 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 83 IMU_Y = result.at(0);
dem123456789 23:cc06a8226f72 84 D_IMU_Y = strtod(IMU_Y.c_str(), NULL);
dem123456789 9:bf5939466e86 85 IMU_P = result.at(1);
dem123456789 23:cc06a8226f72 86 D_IMU_P = strtod(IMU_P.c_str(), NULL);
dem123456789 15:dbf20c1209ae 87 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 23:cc06a8226f72 88 D_IMU_R = strtod(IMU_R.c_str(), NULL);
dem123456789 9:bf5939466e86 89 }
dem123456789 9:bf5939466e86 90 }
dem123456789 9:bf5939466e86 91
dem123456789 10:12ba6ed2d6f0 92 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 93 string GPS_data_string(GPS_data);
dem123456789 11:1caacb994236 94 if (GPS_data_string.substr(0,6) == "$GPGGA") {
dem123456789 11:1caacb994236 95 GPS_data_string = GPS_data_string.substr(7);
dem123456789 10:12ba6ed2d6f0 96 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 97 GPS_Quality = result.at(5);
dem123456789 22:faba67585854 98 D_GPS_Quality = strtod(result.at(5).c_str(), NULL);
dem123456789 14:92bacb5af01b 99 if(GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 11:1caacb994236 100 GPS_UTC = result.at(0);
dem123456789 23:cc06a8226f72 101 D_GPS_UTC = strtod(GPS_UTC.c_str(), NULL);
dem123456789 23:cc06a8226f72 102 GPS_Latitude = result.at(1) + result.at(2);
dem123456789 23:cc06a8226f72 103 temp = result.at(1);
dem123456789 23:cc06a8226f72 104 if (temp[0] == '0') {
dem123456789 23:cc06a8226f72 105 temp[0] = '-';
dem123456789 23:cc06a8226f72 106 }
dem123456789 23:cc06a8226f72 107 D_GPS_Latitude = strtod(temp.c_str(), NULL);
dem123456789 23:cc06a8226f72 108 if (result.at(2) == "N") { // 0 means North
dem123456789 22:faba67585854 109 D_GPS_Latitude_Direction = 0;
dem123456789 23:cc06a8226f72 110 } else if (result.at(2) == "S") { // 1 means South
dem123456789 22:faba67585854 111 D_GPS_Latitude_Direction = 1;
dem123456789 22:faba67585854 112 }
dem123456789 23:cc06a8226f72 113 GPS_Longtitude = result.at(3) + result.at(4);
dem123456789 23:cc06a8226f72 114 temp = result.at(3);
dem123456789 23:cc06a8226f72 115 if (temp[0] == '0') {
dem123456789 23:cc06a8226f72 116 temp[0] = '-';
dem123456789 23:cc06a8226f72 117 }
dem123456789 23:cc06a8226f72 118 D_GPS_Longtitude = strtod(temp.c_str(), NULL);
dem123456789 23:cc06a8226f72 119 if (result.at(4) == "E") { // 0 means East
dem123456789 23:cc06a8226f72 120 D_GPS_Longtitude_Direction = 0;
dem123456789 23:cc06a8226f72 121 } else if (result.at(4) == "W") { // 1 means West
dem123456789 23:cc06a8226f72 122 D_GPS_Longtitude_Direction = 1;
dem123456789 22:faba67585854 123 }
dem123456789 11:1caacb994236 124 GPS_Num_Satellite = result.at(6);
dem123456789 22:faba67585854 125 D_GPS_Num_Satellite = strtod(result.at(6).c_str(), NULL);
dem123456789 11:1caacb994236 126 GPS_HDOP = result.at(7);
dem123456789 22:faba67585854 127 D_GPS_HDOP = strtod(result.at(7).c_str(), NULL);
dem123456789 11:1caacb994236 128 GPS_Altitude = result.at(8) + result.at(9);
dem123456789 22:faba67585854 129 D_GPS_Altitude = strtod(result.at(8).c_str(), NULL);
dem123456789 22:faba67585854 130 GPS_Altitude_Unit = result.at(9);
dem123456789 11:1caacb994236 131 }
dem123456789 14:92bacb5af01b 132 } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 133 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 134 vector<string> result = split(GPS_data_string, ',');
dem123456789 12:8644abfa86da 135 GPS_PDOP = result.at(14);
dem123456789 22:faba67585854 136 D_GPS_PDOP = strtod(result.at(14).c_str(), NULL);
dem123456789 12:8644abfa86da 137 asterisk_idx = result.at(16).find('*');
dem123456789 12:8644abfa86da 138 GPS_VDOP = result.at(16).substr(0, asterisk_idx);
dem123456789 22:faba67585854 139 D_GPS_VDOP = strtod(result.at(16).substr(0, asterisk_idx).c_str(), NULL);
dem123456789 14:92bacb5af01b 140 } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 141 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 142 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 143 GPS_Date = result.at(8);
dem123456789 22:faba67585854 144 D_GPS_Date = strtod(result.at(8).c_str(), NULL);
dem123456789 14:92bacb5af01b 145 } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 146 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 147 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 148 GPS_VelocityKnot = result.at(4) + result.at(5);
dem123456789 22:faba67585854 149 D_GPS_VelocityKnot = strtod(result.at(4).c_str(), NULL);
dem123456789 22:faba67585854 150 GPS_VelocityKnot_Unit = result.at(5);
dem123456789 12:8644abfa86da 151 asterisk_idx = result.at(7).find('*');
dem123456789 14:92bacb5af01b 152 GPS_VelocityKph = result.at(6) + result.at(7).substr(0, asterisk_idx);
dem123456789 22:faba67585854 153 D_GPS_VelocityKph = strtod(result.at(6).c_str(), NULL);
dem123456789 22:faba67585854 154 GPS_VelocityKph_Unit = result.at(7).substr(0, asterisk_idx);
dem123456789 10:12ba6ed2d6f0 155 }
dem123456789 12:8644abfa86da 156
dem123456789 10:12ba6ed2d6f0 157 }
dem123456789 16:0ea992d9a390 158 /* Get data from SailBoat
dem123456789 16:0ea992d9a390 159 @GET=parameter
dem123456789 16:0ea992d9a390 160 Ex: @GET=GPS_Quality
dem123456789 16:0ea992d9a390 161 Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longtitude, GPS_Altitude,
dem123456789 16:0ea992d9a390 162 GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph
dem123456789 16:0ea992d9a390 163 Set path to SailBoat
dem123456789 20:a820531c78bc 164 @SET=PATH, Latitude, Longtitude, Task id
dem123456789 20:a820531c78bc 165 @SET=PATH, 33.776318, -84.407590, 3
dem123456789 16:0ea992d9a390 166 */
dem123456789 15:dbf20c1209ae 167 void decodePC(string PC_data) {
dem123456789 15:dbf20c1209ae 168 string PC_data_string(PC_data);
dem123456789 15:dbf20c1209ae 169 if (PC_data_string.substr(0,4) == "@GET") {
dem123456789 15:dbf20c1209ae 170 pc.printf("%s", PC_data_string.c_str());
dem123456789 15:dbf20c1209ae 171 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 172 pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str());
dem123456789 16:0ea992d9a390 173 } else if (PC_data_string.substr(0,4) == "@SET") {
dem123456789 20:a820531c78bc 174 pc.printf("%s", PC_data_string.c_str());
dem123456789 20:a820531c78bc 175 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 176 string claim = decodeCommandSET(PC_data_string);
dem123456789 20:a820531c78bc 177 if (claim == "DONE") {
dem123456789 20:a820531c78bc 178 pc.printf("Current Path: Longtitude, Latitude\n");
dem123456789 20:a820531c78bc 179 for (int i=0;i<MAX_TASK_SIZE;++i) {
dem123456789 20:a820531c78bc 180 pc.printf(" %f, %f\n", Longtitude_Path[i], Latitude_Path[i]);
dem123456789 20:a820531c78bc 181 }
dem123456789 20:a820531c78bc 182 }
dem123456789 20:a820531c78bc 183 pc.printf("%s\n", claim.c_str());
dem123456789 16:0ea992d9a390 184 }
dem123456789 15:dbf20c1209ae 185 }
dem123456789 15:dbf20c1209ae 186
dem123456789 15:dbf20c1209ae 187
taoqiuyang 24:8e38cc14150c 188
dem123456789 11:1caacb994236 189 void printStateIMU() {
dem123456789 23:cc06a8226f72 190 //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 191 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 192 }
dem123456789 11:1caacb994236 193
dem123456789 12:8644abfa86da 194 void printStateGPS() {
dem123456789 23:cc06a8226f72 195 /*
dem123456789 14:92bacb5af01b 196 pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longtitude: %s, GPS_Altitude: %s, "
dem123456789 14:92bacb5af01b 197 "GPS_Num_Satellite: %s, GPS_HDOP: %s, GPS_VDOP: %s, GPS_PDOP: %s, GPS_Date: %s, GPS_VelocityKnot: %s, GPS_VelocityKph: %s\n",
dem123456789 14:92bacb5af01b 198 GPS_Quality.c_str(), GPS_UTC.c_str(), GPS_Latitude.c_str(), GPS_Longtitude.c_str(), GPS_Altitude.c_str(), GPS_Num_Satellite.c_str(),
dem123456789 23:cc06a8226f72 199 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 200 */
dem123456789 23:cc06a8226f72 201
dem123456789 23:cc06a8226f72 202 pc.printf("D_GPS_Quality: %f, D_GPS_UTC: %f, D_GPS_Latitude: %f, D_GPS_Latitude_Direction: %f, D_GPS_Longtitude: %f, D_GPS_Longtitude_Direction: %f, D_GPS_Altitude: %f,\n"
dem123456789 23:cc06a8226f72 203 "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 23:cc06a8226f72 204 D_GPS_Quality, D_GPS_UTC, D_GPS_Latitude, D_GPS_Latitude_Direction, D_GPS_Longtitude, D_GPS_Longtitude_Direction, D_GPS_Altitude, D_GPS_Num_Satellite,
dem123456789 23:cc06a8226f72 205 D_GPS_HDOP, D_GPS_VDOP, D_GPS_PDOP, D_GPS_Date, D_GPS_VelocityKnot, D_GPS_VelocityKph);
dem123456789 23:cc06a8226f72 206
dem123456789 12:8644abfa86da 207 }
dem123456789 14:92bacb5af01b 208
dem123456789 11:1caacb994236 209 //#YPR=-183.74,-134.27,-114.39
taoqiuyang 4:37d118f2348c 210 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 211 char buf;
taoqiuyang 3:ab9f94d112c0 212
taoqiuyang 3:ab9f94d112c0 213 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 214 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 215
dem123456789 14:92bacb5af01b 216 //pc.putc(buf);
taoqiuyang 3:ab9f94d112c0 217 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 218 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 219
taoqiuyang 3:ab9f94d112c0 220 if (buf=='\n'){
dem123456789 9:bf5939466e86 221 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 11:1caacb994236 222 //pc.printf("%s", IMU_copy.c_str());
dem123456789 9:bf5939466e86 223 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 224 IMU_message_counter=0;
dem123456789 9:bf5939466e86 225 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 226 }
dem123456789 9:bf5939466e86 227
taoqiuyang 3:ab9f94d112c0 228 }
taoqiuyang 2:afb333543af5 229 led2 = !led2;
taoqiuyang 2:afb333543af5 230 }
dem123456789 9:bf5939466e86 231
dem123456789 9:bf5939466e86 232
dem123456789 8:1f5a44bade4d 233
taoqiuyang 5:451b8203ef99 234 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 235 char buf;
taoqiuyang 5:451b8203ef99 236
taoqiuyang 5:451b8203ef99 237 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 238 buf = GPS.getc();
dem123456789 14:92bacb5af01b 239
dem123456789 14:92bacb5af01b 240 //pc.putc(buf);
dem123456789 10:12ba6ed2d6f0 241 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 242 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 243
dem123456789 10:12ba6ed2d6f0 244 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 245 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 11:1caacb994236 246 //pc.printf("%s", GPS_copy.c_str());
dem123456789 10:12ba6ed2d6f0 247 updateGPS(GPS_copy);
dem123456789 10:12ba6ed2d6f0 248 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 249 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 250 }
taoqiuyang 5:451b8203ef99 251 }
taoqiuyang 5:451b8203ef99 252
taoqiuyang 5:451b8203ef99 253 led3 = !led3;
taoqiuyang 5:451b8203ef99 254 }
taoqiuyang 3:ab9f94d112c0 255
dem123456789 8:1f5a44bade4d 256 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 257 char buf;
dem123456789 8:1f5a44bade4d 258
dem123456789 8:1f5a44bade4d 259 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 260 buf = pc.getc();
dem123456789 14:92bacb5af01b 261
dem123456789 14:92bacb5af01b 262 //pc.putc(buf);
dem123456789 14:92bacb5af01b 263 PC_message[PC_message_counter]=buf;
dem123456789 14:92bacb5af01b 264 PC_message_counter+=1;
dem123456789 14:92bacb5af01b 265
dem123456789 14:92bacb5af01b 266 if (buf=='\n'){
dem123456789 14:92bacb5af01b 267 string PC_copy(PC_message, PC_message_counter);
dem123456789 14:92bacb5af01b 268 //pc.printf("%s", PC_copy.c_str());
dem123456789 14:92bacb5af01b 269 decodePC(PC_copy);
dem123456789 14:92bacb5af01b 270 PC_message_counter=0;
dem123456789 14:92bacb5af01b 271 PC_copy[0] = '\0';
dem123456789 14:92bacb5af01b 272 }
dem123456789 8:1f5a44bade4d 273 }
dem123456789 8:1f5a44bade4d 274
dem123456789 8:1f5a44bade4d 275 led4= !led4;
dem123456789 8:1f5a44bade4d 276 }
dem123456789 21:99be83550601 277
taoqiuyang 24:8e38cc14150c 278
taoqiuyang 24:8e38cc14150c 279
taoqiuyang 24:8e38cc14150c 280 void update_controller() {
taoqiuyang 24:8e38cc14150c 281 /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角
taoqiuyang 24:8e38cc14150c 282 distance(meter) to the next path point
taoqiuyang 24:8e38cc14150c 283
taoqiuyang 24:8e38cc14150c 284 Function: drive two servos to navigate the sailboat to the desired path point
taoqiuyang 24:8e38cc14150c 285 */
taoqiuyang 24:8e38cc14150c 286
taoqiuyang 24:8e38cc14150c 287 }
taoqiuyang 24:8e38cc14150c 288
dem123456789 21:99be83550601 289 void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
dem123456789 21:99be83550601 290 /*angleDeg: desired angle
dem123456789 21:99be83550601 291 minDeg: minimum angle in degrees
dem123456789 21:99be83550601 292 minNorVal: normalized value[0,1] when servo is at its minimum angle
dem123456789 21:99be83550601 293 maxDeg: maximum angle in degrees
dem123456789 21:99be83550601 294 maxNorVal: normalized value[0,1] when servo is at its maximum angle
dem123456789 21:99be83550601 295 */
dem123456789 21:99be83550601 296
dem123456789 21:99be83550601 297 float pos; //normalized angle, [0,1]
dem123456789 21:99be83550601 298 float scale; //scale factor for servo calibration
dem123456789 21:99be83550601 299
dem123456789 21:99be83550601 300 //extreme conditions
dem123456789 21:99be83550601 301 if (angleDeg<minDeg){
dem123456789 21:99be83550601 302 pos=minNorVal;
dem123456789 21:99be83550601 303 }
dem123456789 21:99be83550601 304 if (angleDeg>maxDeg){
dem123456789 21:99be83550601 305 pos=maxNorVal;
dem123456789 21:99be83550601 306 }
dem123456789 21:99be83550601 307
dem123456789 21:99be83550601 308 //Calculate scale factor for servo calibration
dem123456789 21:99be83550601 309 scale = (angleDeg-minDeg)/(maxDeg-minDeg);
dem123456789 21:99be83550601 310 //Calculate servo normalized value
dem123456789 21:99be83550601 311 pos = minNorVal + scale*(maxNorVal-minNorVal);
dem123456789 21:99be83550601 312
dem123456789 21:99be83550601 313 //send signal to servo
dem123456789 21:99be83550601 314 targetServo=pos;
dem123456789 21:99be83550601 315 }
dem123456789 21:99be83550601 316
dem123456789 21:99be83550601 317 int log_data_SD(){
dem123456789 21:99be83550601 318 FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
dem123456789 21:99be83550601 319 if(fp == NULL) {
dem123456789 21:99be83550601 320 return 0;
dem123456789 21:99be83550601 321 }else{
dem123456789 21:99be83550601 322 //Write all the useful data to the SD card
dem123456789 21:99be83550601 323 fprintf(fp, "Nya Pass~");
dem123456789 21:99be83550601 324 fclose(fp);
dem123456789 21:99be83550601 325 return 1;
dem123456789 21:99be83550601 326 }
dem123456789 21:99be83550601 327 }
dem123456789 21:99be83550601 328
dem123456789 21:99be83550601 329
taoqiuyang 0:f4d390c06705 330 int main() {
taoqiuyang 2:afb333543af5 331 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 332 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 333 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 334 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 335 pc.baud(115200);
dem123456789 8:1f5a44bade4d 336 pc.attach(&PC_serial_ISR);
taoqiuyang 1:e7245ffb4820 337
dem123456789 23:cc06a8226f72 338 //float angle=20;
taoqiuyang 2:afb333543af5 339 while (1) {
dem123456789 23:cc06a8226f72 340 // if (angle>160){angle=20;}
dem123456789 23:cc06a8226f72 341 // set_servo_position(rudderServo, angle, 0, 0, 180, 1);
dem123456789 23:cc06a8226f72 342 // set_servo_position(wingServo, angle, 0, 0, 180, 1);
dem123456789 23:cc06a8226f72 343 // angle=angle+10;
dem123456789 15:dbf20c1209ae 344 wait(0.4);
dem123456789 23:cc06a8226f72 345 printStateIMU();
dem123456789 23:cc06a8226f72 346 printStateGPS();
dem123456789 21:99be83550601 347 led1 = !led1;
taoqiuyang 0:f4d390c06705 348 }
taoqiuyang 0:f4d390c06705 349 }