![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mbed Code for Multi-Waypoint Tests
main.cpp@0:a955f7fabdf5, 2014-04-29 (annotated)
- Committer:
- landnavvest
- Date:
- Tue Apr 29 03:02:11 2014 +0000
- Revision:
- 0:a955f7fabdf5
Zachary Motter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
landnavvest | 0:a955f7fabdf5 | 1 | #include "mbed.h" |
landnavvest | 0:a955f7fabdf5 | 2 | |
landnavvest | 0:a955f7fabdf5 | 3 | |
landnavvest | 0:a955f7fabdf5 | 4 | Serial pc(USBTX, USBRX); // tx, rx for PC |
landnavvest | 0:a955f7fabdf5 | 5 | Serial mag(p28, p27); // tx,rx for Magnetometer |
landnavvest | 0:a955f7fabdf5 | 6 | Serial gps(p9, p10); //tx, rx for GPS |
landnavvest | 0:a955f7fabdf5 | 7 | // GPS is 9,10; MAG is 28,27 |
landnavvest | 0:a955f7fabdf5 | 8 | |
landnavvest | 0:a955f7fabdf5 | 9 | DigitalOut on(LED1); // running lights |
landnavvest | 0:a955f7fabdf5 | 10 | DigitalOut test_led(LED2); // extra for testing |
landnavvest | 0:a955f7fabdf5 | 11 | DigitalOut front(p21); /// Connect LED cathode to pin 21 |
landnavvest | 0:a955f7fabdf5 | 12 | DigitalOut frontR(p22); |
landnavvest | 0:a955f7fabdf5 | 13 | DigitalOut R(p23); |
landnavvest | 0:a955f7fabdf5 | 14 | DigitalOut back(p24); |
landnavvest | 0:a955f7fabdf5 | 15 | DigitalOut L(p25); |
landnavvest | 0:a955f7fabdf5 | 16 | DigitalOut frontL(p26); |
landnavvest | 0:a955f7fabdf5 | 17 | |
landnavvest | 0:a955f7fabdf5 | 18 | float dir_m,dir_t,rel,decl,latitude,longitude,lock,degrees,minutes,uttime,old_lat,old_long; |
landnavvest | 0:a955f7fabdf5 | 19 | float san_coop_lat,san_coop_long; |
landnavvest | 0:a955f7fabdf5 | 20 | float lat_1,long_1,lat_2,long_2,lat_3,long_3; |
landnavvest | 0:a955f7fabdf5 | 21 | int waypoint; |
landnavvest | 0:a955f7fabdf5 | 22 | int wait_front,wait_front_cycle; |
landnavvest | 0:a955f7fabdf5 | 23 | char ns,ew; |
landnavvest | 0:a955f7fabdf5 | 24 | bool there,gps_lock; |
landnavvest | 0:a955f7fabdf5 | 25 | float PI = 3.1415926535; |
landnavvest | 0:a955f7fabdf5 | 26 | |
landnavvest | 0:a955f7fabdf5 | 27 | float course_to,gps_t,gps_m; |
landnavvest | 0:a955f7fabdf5 | 28 | double meters_to; |
landnavvest | 0:a955f7fabdf5 | 29 | |
landnavvest | 0:a955f7fabdf5 | 30 | |
landnavvest | 0:a955f7fabdf5 | 31 | /* Instructions: |
landnavvest | 0:a955f7fabdf5 | 32 | 1) f10 to download to mbed, save it. |
landnavvest | 0:a955f7fabdf5 | 33 | 2) TeraTerm, serial, port 6 (MBED) |
landnavvest | 0:a955f7fabdf5 | 34 | 3) Tera Term, setup>serialport>baudrate (921600), setup>terminal>newline (LF) */ |
landnavvest | 0:a955f7fabdf5 | 35 | |
landnavvest | 0:a955f7fabdf5 | 36 | int main() { |
landnavvest | 0:a955f7fabdf5 | 37 | //Declarations |
landnavvest | 0:a955f7fabdf5 | 38 | gps.baud(4800); |
landnavvest | 0:a955f7fabdf5 | 39 | pc.baud(9600); |
landnavvest | 0:a955f7fabdf5 | 40 | mag.baud(19200); |
landnavvest | 0:a955f7fabdf5 | 41 | |
landnavvest | 0:a955f7fabdf5 | 42 | pc.printf("Starting...\n"); |
landnavvest | 0:a955f7fabdf5 | 43 | |
landnavvest | 0:a955f7fabdf5 | 44 | lat_1=38.983148; |
landnavvest | 0:a955f7fabdf5 | 45 | long_1= -76.484680; |
landnavvest | 0:a955f7fabdf5 | 46 | lat_2=38.984525; |
landnavvest | 0:a955f7fabdf5 | 47 | long_2=-76.483457; |
landnavvest | 0:a955f7fabdf5 | 48 | lat_3=38.984045; |
landnavvest | 0:a955f7fabdf5 | 49 | long_3=-76.484192; |
landnavvest | 0:a955f7fabdf5 | 50 | waypoint=1; |
landnavvest | 0:a955f7fabdf5 | 51 | |
landnavvest | 0:a955f7fabdf5 | 52 | there = 0; |
landnavvest | 0:a955f7fabdf5 | 53 | gps_lock = 0; |
landnavvest | 0:a955f7fabdf5 | 54 | wait_front_cycle=3; |
landnavvest | 0:a955f7fabdf5 | 55 | wait_front=wait_front_cycle; |
landnavvest | 0:a955f7fabdf5 | 56 | |
landnavvest | 0:a955f7fabdf5 | 57 | decl = -11.11; //11.11 WEST |
landnavvest | 0:a955f7fabdf5 | 58 | |
landnavvest | 0:a955f7fabdf5 | 59 | while(1) { |
landnavvest | 0:a955f7fabdf5 | 60 | // Read Serial |
landnavvest | 0:a955f7fabdf5 | 61 | //pc.printf("^1"); |
landnavvest | 0:a955f7fabdf5 | 62 | if(mag.readable()) { |
landnavvest | 0:a955f7fabdf5 | 63 | while (mag.getc() != '$'); { |
landnavvest | 0:a955f7fabdf5 | 64 | //mag.scanf("HCHDT,%f,T*%c%c",&dir,&cs1,&cs2); |
landnavvest | 0:a955f7fabdf5 | 65 | //pc.printf("MAG dir = %f, cs = %c%c\n", dir, cs1,cs2); |
landnavvest | 0:a955f7fabdf5 | 66 | mag.scanf("HCHDT,%f,",&dir_m); |
landnavvest | 0:a955f7fabdf5 | 67 | //pc.printf("_read_mag"); |
landnavvest | 0:a955f7fabdf5 | 68 | dir_t=dir_m+decl; |
landnavvest | 0:a955f7fabdf5 | 69 | } |
landnavvest | 0:a955f7fabdf5 | 70 | } |
landnavvest | 0:a955f7fabdf5 | 71 | //pc.printf("^2"); |
landnavvest | 0:a955f7fabdf5 | 72 | |
landnavvest | 0:a955f7fabdf5 | 73 | if(gps.readable()){ |
landnavvest | 0:a955f7fabdf5 | 74 | gps.getc();} |
landnavvest | 0:a955f7fabdf5 | 75 | |
landnavvest | 0:a955f7fabdf5 | 76 | //pc.printf("_buffer"); |
landnavvest | 0:a955f7fabdf5 | 77 | |
landnavvest | 0:a955f7fabdf5 | 78 | if(gps.readable()){ |
landnavvest | 0:a955f7fabdf5 | 79 | while (gps.getc()!='$');{ |
landnavvest | 0:a955f7fabdf5 | 80 | if(gps.scanf("GPGGA,%f,%f,%c,%f,%c",&uttime, &latitude, &ns, &longitude, &ew)) |
landnavvest | 0:a955f7fabdf5 | 81 | { |
landnavvest | 0:a955f7fabdf5 | 82 | //pc.printf("_read_gps"); |
landnavvest | 0:a955f7fabdf5 | 83 | gps_lock=1; |
landnavvest | 0:a955f7fabdf5 | 84 | } |
landnavvest | 0:a955f7fabdf5 | 85 | } |
landnavvest | 0:a955f7fabdf5 | 86 | } |
landnavvest | 0:a955f7fabdf5 | 87 | |
landnavvest | 0:a955f7fabdf5 | 88 | |
landnavvest | 0:a955f7fabdf5 | 89 | //pc.printf("_%c",ew); |
landnavvest | 0:a955f7fabdf5 | 90 | |
landnavvest | 0:a955f7fabdf5 | 91 | degrees = floor(latitude / 100.0); |
landnavvest | 0:a955f7fabdf5 | 92 | minutes = latitude - (degrees * 100.0); |
landnavvest | 0:a955f7fabdf5 | 93 | latitude = degrees + minutes / 60.0; |
landnavvest | 0:a955f7fabdf5 | 94 | degrees = floor(longitude / 100.0); |
landnavvest | 0:a955f7fabdf5 | 95 | minutes = longitude - (degrees * 100.0); |
landnavvest | 0:a955f7fabdf5 | 96 | longitude = degrees + minutes / 60.0; |
landnavvest | 0:a955f7fabdf5 | 97 | |
landnavvest | 0:a955f7fabdf5 | 98 | //pc.printf("_b_%f",longitude); |
landnavvest | 0:a955f7fabdf5 | 99 | if(ns == 'S') { latitude *= -1.0;} |
landnavvest | 0:a955f7fabdf5 | 100 | if(ew == 'W') { longitude *= -1.0;} |
landnavvest | 0:a955f7fabdf5 | 101 | //pc.printf("_a_%f",longitude); |
landnavvest | 0:a955f7fabdf5 | 102 | |
landnavvest | 0:a955f7fabdf5 | 103 | if(gps_lock==0) |
landnavvest | 0:a955f7fabdf5 | 104 | { |
landnavvest | 0:a955f7fabdf5 | 105 | longitude=old_long; |
landnavvest | 0:a955f7fabdf5 | 106 | latitude=old_lat; |
landnavvest | 0:a955f7fabdf5 | 107 | } |
landnavvest | 0:a955f7fabdf5 | 108 | |
landnavvest | 0:a955f7fabdf5 | 109 | old_lat=latitude; |
landnavvest | 0:a955f7fabdf5 | 110 | old_long=longitude; |
landnavvest | 0:a955f7fabdf5 | 111 | gps_lock=0; |
landnavvest | 0:a955f7fabdf5 | 112 | |
landnavvest | 0:a955f7fabdf5 | 113 | //pc.printf("^3"); |
landnavvest | 0:a955f7fabdf5 | 114 | |
landnavvest | 0:a955f7fabdf5 | 115 | if(waypoint==1){ |
landnavvest | 0:a955f7fabdf5 | 116 | san_coop_lat=lat_1; |
landnavvest | 0:a955f7fabdf5 | 117 | san_coop_long=long_1; |
landnavvest | 0:a955f7fabdf5 | 118 | } else if (waypoint ==2){ |
landnavvest | 0:a955f7fabdf5 | 119 | san_coop_lat=lat_2; |
landnavvest | 0:a955f7fabdf5 | 120 | san_coop_long=long_2; |
landnavvest | 0:a955f7fabdf5 | 121 | } else if (waypoint ==3){ |
landnavvest | 0:a955f7fabdf5 | 122 | san_coop_lat=lat_3; |
landnavvest | 0:a955f7fabdf5 | 123 | san_coop_long=long_3; |
landnavvest | 0:a955f7fabdf5 | 124 | } |
landnavvest | 0:a955f7fabdf5 | 125 | |
landnavvest | 0:a955f7fabdf5 | 126 | const double d2r = PI / 180.0; |
landnavvest | 0:a955f7fabdf5 | 127 | const double r2d = 180.0 / PI; |
landnavvest | 0:a955f7fabdf5 | 128 | |
landnavvest | 0:a955f7fabdf5 | 129 | |
landnavvest | 0:a955f7fabdf5 | 130 | double dlat = abs(san_coop_lat - latitude) * d2r; |
landnavvest | 0:a955f7fabdf5 | 131 | double dlong = abs(san_coop_long - longitude) * d2r; |
landnavvest | 0:a955f7fabdf5 | 132 | double x_sign = san_coop_long-longitude; |
landnavvest | 0:a955f7fabdf5 | 133 | double y = sin(dlong) * cos(san_coop_lat * d2r); |
landnavvest | 0:a955f7fabdf5 | 134 | double x = cos(latitude*d2r)*sin(san_coop_lat*d2r) - sin(latitude*d2r)*cos(san_coop_lat*d2r)*cos(dlong); |
landnavvest | 0:a955f7fabdf5 | 135 | |
landnavvest | 0:a955f7fabdf5 | 136 | course_to = atan2(y,x)*r2d; |
landnavvest | 0:a955f7fabdf5 | 137 | |
landnavvest | 0:a955f7fabdf5 | 138 | if (x_sign<0){ |
landnavvest | 0:a955f7fabdf5 | 139 | course_to=-course_to+360;} |
landnavvest | 0:a955f7fabdf5 | 140 | |
landnavvest | 0:a955f7fabdf5 | 141 | // calculate range/bearing |
landnavvest | 0:a955f7fabdf5 | 142 | /*float deltalat = abs(san_coop_lat - latitude); |
landnavvest | 0:a955f7fabdf5 | 143 | float deltalon = abs(san_coop_long - longitude); |
landnavvest | 0:a955f7fabdf5 | 144 | |
landnavvest | 0:a955f7fabdf5 | 145 | float deltalatr = deltalat *(PI/180); |
landnavvest | 0:a955f7fabdf5 | 146 | float deltalonr = deltalon *(PI/180); |
landnavvest | 0:a955f7fabdf5 | 147 | |
landnavvest | 0:a955f7fabdf5 | 148 | //float d = sin(deltalatr/2.0); |
landnavvest | 0:a955f7fabdf5 | 149 | |
landnavvest | 0:a955f7fabdf5 | 150 | //float x = pow(d,2.0); |
landnavvest | 0:a955f7fabdf5 | 151 | //float y = cos(latitude*(PI/180))*cos(san_coop_lat*(PI/180))*(sin(deltalonr/2.0f)^2.0f); |
landnavvest | 0:a955f7fabdf5 | 152 | */ |
landnavvest | 0:a955f7fabdf5 | 153 | float a = pow(sin(dlat/2.0),2.0) + cos(latitude*(PI/180)) * cos(san_coop_lat*(PI/180)) * pow(sin(dlong/2.0),2.0); |
landnavvest | 0:a955f7fabdf5 | 154 | float c = 2*atan2(sqrt(a),sqrt(1-a)); |
landnavvest | 0:a955f7fabdf5 | 155 | |
landnavvest | 0:a955f7fabdf5 | 156 | float distft = (3956*c)*5280; |
landnavvest | 0:a955f7fabdf5 | 157 | //pc.printf("^4"); |
landnavvest | 0:a955f7fabdf5 | 158 | //course_to = (atan2(sin(-1*deltalonr)*cos(san_coop_lat*(PI/180)),cos(latitude*(PI/180))*sin(san_coop_lat*(PI/180))-sin(latitude*(PI/180))*cos(san_coop_lat*(PI/180))*cos(-1*deltalonr)))*(180/PI); |
landnavvest | 0:a955f7fabdf5 | 159 | |
landnavvest | 0:a955f7fabdf5 | 160 | |
landnavvest | 0:a955f7fabdf5 | 161 | //course_to = gps.calc_course_to(san_coop_lat,san_coop_long); |
landnavvest | 0:a955f7fabdf5 | 162 | //meters_to = gps.calc_dist_to_m(san_coop_lat,san_coop_long); |
landnavvest | 0:a955f7fabdf5 | 163 | rel = course_to-dir_t; |
landnavvest | 0:a955f7fabdf5 | 164 | |
landnavvest | 0:a955f7fabdf5 | 165 | if (rel<0) { |
landnavvest | 0:a955f7fabdf5 | 166 | rel=rel+360;} |
landnavvest | 0:a955f7fabdf5 | 167 | |
landnavvest | 0:a955f7fabdf5 | 168 | if (distft<30){ |
landnavvest | 0:a955f7fabdf5 | 169 | there=1;} |
landnavvest | 0:a955f7fabdf5 | 170 | //pc.printf("^5"); |
landnavvest | 0:a955f7fabdf5 | 171 | pc.printf("mag_%f,lat_%f,long_%f,c2_%f,d2_%f,rel_%f,urhere_%d\n",dir_t,latitude,longitude,course_to,distft,rel,there); |
landnavvest | 0:a955f7fabdf5 | 172 | //pc.printf("^6"); |
landnavvest | 0:a955f7fabdf5 | 173 | // LED or Motor |
landnavvest | 0:a955f7fabdf5 | 174 | if (rel<10||rel>350) { |
landnavvest | 0:a955f7fabdf5 | 175 | if (wait_front>=wait_front_cycle){ |
landnavvest | 0:a955f7fabdf5 | 176 | front = 1; |
landnavvest | 0:a955f7fabdf5 | 177 | wait_front=0;} |
landnavvest | 0:a955f7fabdf5 | 178 | wait_front++; |
landnavvest | 0:a955f7fabdf5 | 179 | } |
landnavvest | 0:a955f7fabdf5 | 180 | if (rel>10&&rel<35){ |
landnavvest | 0:a955f7fabdf5 | 181 | front = 1; |
landnavvest | 0:a955f7fabdf5 | 182 | frontR =1;} |
landnavvest | 0:a955f7fabdf5 | 183 | if (rel>35&&rel<55) { |
landnavvest | 0:a955f7fabdf5 | 184 | frontR=1;} |
landnavvest | 0:a955f7fabdf5 | 185 | if (rel>55&&rel<80) { |
landnavvest | 0:a955f7fabdf5 | 186 | frontR=1; |
landnavvest | 0:a955f7fabdf5 | 187 | R=1;} |
landnavvest | 0:a955f7fabdf5 | 188 | if (rel>80&&rel<100){ |
landnavvest | 0:a955f7fabdf5 | 189 | R=1;} |
landnavvest | 0:a955f7fabdf5 | 190 | if (rel>100&&rel<150){ |
landnavvest | 0:a955f7fabdf5 | 191 | R=1; |
landnavvest | 0:a955f7fabdf5 | 192 | back=1;} |
landnavvest | 0:a955f7fabdf5 | 193 | if (rel>150&&rel<210){ |
landnavvest | 0:a955f7fabdf5 | 194 | back=1;} |
landnavvest | 0:a955f7fabdf5 | 195 | if (rel>210&&rel<260){ |
landnavvest | 0:a955f7fabdf5 | 196 | back=1; |
landnavvest | 0:a955f7fabdf5 | 197 | L=1;} |
landnavvest | 0:a955f7fabdf5 | 198 | if(rel>260&&rel<280){ |
landnavvest | 0:a955f7fabdf5 | 199 | L=1;} |
landnavvest | 0:a955f7fabdf5 | 200 | if(rel>280&&rel<305){ |
landnavvest | 0:a955f7fabdf5 | 201 | L=1; |
landnavvest | 0:a955f7fabdf5 | 202 | frontL=1;} |
landnavvest | 0:a955f7fabdf5 | 203 | if(rel>305&&rel<325){ |
landnavvest | 0:a955f7fabdf5 | 204 | frontL=1;} |
landnavvest | 0:a955f7fabdf5 | 205 | if(rel>325&&rel<350){ |
landnavvest | 0:a955f7fabdf5 | 206 | frontL=1; |
landnavvest | 0:a955f7fabdf5 | 207 | front=1;} |
landnavvest | 0:a955f7fabdf5 | 208 | |
landnavvest | 0:a955f7fabdf5 | 209 | if(there){ |
landnavvest | 0:a955f7fabdf5 | 210 | front = 1; |
landnavvest | 0:a955f7fabdf5 | 211 | frontR = 1; |
landnavvest | 0:a955f7fabdf5 | 212 | R=1; |
landnavvest | 0:a955f7fabdf5 | 213 | back=1; |
landnavvest | 0:a955f7fabdf5 | 214 | L=1; |
landnavvest | 0:a955f7fabdf5 | 215 | frontL=1; |
landnavvest | 0:a955f7fabdf5 | 216 | wait(0.5); |
landnavvest | 0:a955f7fabdf5 | 217 | |
landnavvest | 0:a955f7fabdf5 | 218 | front = 0; |
landnavvest | 0:a955f7fabdf5 | 219 | frontR = 0; |
landnavvest | 0:a955f7fabdf5 | 220 | R=0; |
landnavvest | 0:a955f7fabdf5 | 221 | back=0; |
landnavvest | 0:a955f7fabdf5 | 222 | L=0; |
landnavvest | 0:a955f7fabdf5 | 223 | frontL=0; |
landnavvest | 0:a955f7fabdf5 | 224 | wait(0.5); |
landnavvest | 0:a955f7fabdf5 | 225 | |
landnavvest | 0:a955f7fabdf5 | 226 | front = 1; |
landnavvest | 0:a955f7fabdf5 | 227 | frontR = 1; |
landnavvest | 0:a955f7fabdf5 | 228 | R=1; |
landnavvest | 0:a955f7fabdf5 | 229 | back=1; |
landnavvest | 0:a955f7fabdf5 | 230 | L=1; |
landnavvest | 0:a955f7fabdf5 | 231 | frontL=1; |
landnavvest | 0:a955f7fabdf5 | 232 | wait(0.5); |
landnavvest | 0:a955f7fabdf5 | 233 | |
landnavvest | 0:a955f7fabdf5 | 234 | front = 0; |
landnavvest | 0:a955f7fabdf5 | 235 | frontR = 0; |
landnavvest | 0:a955f7fabdf5 | 236 | R=0; |
landnavvest | 0:a955f7fabdf5 | 237 | back=0; |
landnavvest | 0:a955f7fabdf5 | 238 | L=0; |
landnavvest | 0:a955f7fabdf5 | 239 | frontL=0; |
landnavvest | 0:a955f7fabdf5 | 240 | wait(0.5); |
landnavvest | 0:a955f7fabdf5 | 241 | |
landnavvest | 0:a955f7fabdf5 | 242 | front = 1; |
landnavvest | 0:a955f7fabdf5 | 243 | frontR = 1; |
landnavvest | 0:a955f7fabdf5 | 244 | R=1; |
landnavvest | 0:a955f7fabdf5 | 245 | back=1; |
landnavvest | 0:a955f7fabdf5 | 246 | L=1; |
landnavvest | 0:a955f7fabdf5 | 247 | frontL=1; |
landnavvest | 0:a955f7fabdf5 | 248 | wait(0.5); |
landnavvest | 0:a955f7fabdf5 | 249 | |
landnavvest | 0:a955f7fabdf5 | 250 | there = 0; |
landnavvest | 0:a955f7fabdf5 | 251 | waypoint=waypoint+1; |
landnavvest | 0:a955f7fabdf5 | 252 | } |
landnavvest | 0:a955f7fabdf5 | 253 | |
landnavvest | 0:a955f7fabdf5 | 254 | //pc.printf("^7"); |
landnavvest | 0:a955f7fabdf5 | 255 | wait(.5); |
landnavvest | 0:a955f7fabdf5 | 256 | front = 0; |
landnavvest | 0:a955f7fabdf5 | 257 | frontR = 0; |
landnavvest | 0:a955f7fabdf5 | 258 | R=0; |
landnavvest | 0:a955f7fabdf5 | 259 | back=0; |
landnavvest | 0:a955f7fabdf5 | 260 | L=0; |
landnavvest | 0:a955f7fabdf5 | 261 | frontL=0; |
landnavvest | 0:a955f7fabdf5 | 262 | //pc.printf("^8"); |
landnavvest | 0:a955f7fabdf5 | 263 | } |
landnavvest | 0:a955f7fabdf5 | 264 | } |