Zack Motter / Mbed 2 deprecated Circuit

mbed Code for Multi-Waypoint Tests

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }