![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mbed Code for Multi-Waypoint Tests
main.cpp
- Committer:
- landnavvest
- Date:
- 2014-04-29
- Revision:
- 0:a955f7fabdf5
File content as of revision 0:a955f7fabdf5:
#include "mbed.h" Serial pc(USBTX, USBRX); // tx, rx for PC Serial mag(p28, p27); // tx,rx for Magnetometer Serial gps(p9, p10); //tx, rx for GPS // GPS is 9,10; MAG is 28,27 DigitalOut on(LED1); // running lights DigitalOut test_led(LED2); // extra for testing DigitalOut front(p21); /// Connect LED cathode to pin 21 DigitalOut frontR(p22); DigitalOut R(p23); DigitalOut back(p24); DigitalOut L(p25); DigitalOut frontL(p26); float dir_m,dir_t,rel,decl,latitude,longitude,lock,degrees,minutes,uttime,old_lat,old_long; float san_coop_lat,san_coop_long; float lat_1,long_1,lat_2,long_2,lat_3,long_3; int waypoint; int wait_front,wait_front_cycle; char ns,ew; bool there,gps_lock; float PI = 3.1415926535; float course_to,gps_t,gps_m; double meters_to; /* Instructions: 1) f10 to download to mbed, save it. 2) TeraTerm, serial, port 6 (MBED) 3) Tera Term, setup>serialport>baudrate (921600), setup>terminal>newline (LF) */ int main() { //Declarations gps.baud(4800); pc.baud(9600); mag.baud(19200); pc.printf("Starting...\n"); lat_1=38.983148; long_1= -76.484680; lat_2=38.984525; long_2=-76.483457; lat_3=38.984045; long_3=-76.484192; waypoint=1; there = 0; gps_lock = 0; wait_front_cycle=3; wait_front=wait_front_cycle; decl = -11.11; //11.11 WEST while(1) { // Read Serial //pc.printf("^1"); if(mag.readable()) { while (mag.getc() != '$'); { //mag.scanf("HCHDT,%f,T*%c%c",&dir,&cs1,&cs2); //pc.printf("MAG dir = %f, cs = %c%c\n", dir, cs1,cs2); mag.scanf("HCHDT,%f,",&dir_m); //pc.printf("_read_mag"); dir_t=dir_m+decl; } } //pc.printf("^2"); if(gps.readable()){ gps.getc();} //pc.printf("_buffer"); if(gps.readable()){ while (gps.getc()!='$');{ if(gps.scanf("GPGGA,%f,%f,%c,%f,%c",&uttime, &latitude, &ns, &longitude, &ew)) { //pc.printf("_read_gps"); gps_lock=1; } } } //pc.printf("_%c",ew); degrees = floor(latitude / 100.0); minutes = latitude - (degrees * 100.0); latitude = degrees + minutes / 60.0; degrees = floor(longitude / 100.0); minutes = longitude - (degrees * 100.0); longitude = degrees + minutes / 60.0; //pc.printf("_b_%f",longitude); if(ns == 'S') { latitude *= -1.0;} if(ew == 'W') { longitude *= -1.0;} //pc.printf("_a_%f",longitude); if(gps_lock==0) { longitude=old_long; latitude=old_lat; } old_lat=latitude; old_long=longitude; gps_lock=0; //pc.printf("^3"); if(waypoint==1){ san_coop_lat=lat_1; san_coop_long=long_1; } else if (waypoint ==2){ san_coop_lat=lat_2; san_coop_long=long_2; } else if (waypoint ==3){ san_coop_lat=lat_3; san_coop_long=long_3; } const double d2r = PI / 180.0; const double r2d = 180.0 / PI; double dlat = abs(san_coop_lat - latitude) * d2r; double dlong = abs(san_coop_long - longitude) * d2r; double x_sign = san_coop_long-longitude; double y = sin(dlong) * cos(san_coop_lat * d2r); double x = cos(latitude*d2r)*sin(san_coop_lat*d2r) - sin(latitude*d2r)*cos(san_coop_lat*d2r)*cos(dlong); course_to = atan2(y,x)*r2d; if (x_sign<0){ course_to=-course_to+360;} // calculate range/bearing /*float deltalat = abs(san_coop_lat - latitude); float deltalon = abs(san_coop_long - longitude); float deltalatr = deltalat *(PI/180); float deltalonr = deltalon *(PI/180); //float d = sin(deltalatr/2.0); //float x = pow(d,2.0); //float y = cos(latitude*(PI/180))*cos(san_coop_lat*(PI/180))*(sin(deltalonr/2.0f)^2.0f); */ 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); float c = 2*atan2(sqrt(a),sqrt(1-a)); float distft = (3956*c)*5280; //pc.printf("^4"); //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); //course_to = gps.calc_course_to(san_coop_lat,san_coop_long); //meters_to = gps.calc_dist_to_m(san_coop_lat,san_coop_long); rel = course_to-dir_t; if (rel<0) { rel=rel+360;} if (distft<30){ there=1;} //pc.printf("^5"); 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); //pc.printf("^6"); // LED or Motor if (rel<10||rel>350) { if (wait_front>=wait_front_cycle){ front = 1; wait_front=0;} wait_front++; } if (rel>10&&rel<35){ front = 1; frontR =1;} if (rel>35&&rel<55) { frontR=1;} if (rel>55&&rel<80) { frontR=1; R=1;} if (rel>80&&rel<100){ R=1;} if (rel>100&&rel<150){ R=1; back=1;} if (rel>150&&rel<210){ back=1;} if (rel>210&&rel<260){ back=1; L=1;} if(rel>260&&rel<280){ L=1;} if(rel>280&&rel<305){ L=1; frontL=1;} if(rel>305&&rel<325){ frontL=1;} if(rel>325&&rel<350){ frontL=1; front=1;} if(there){ front = 1; frontR = 1; R=1; back=1; L=1; frontL=1; wait(0.5); front = 0; frontR = 0; R=0; back=0; L=0; frontL=0; wait(0.5); front = 1; frontR = 1; R=1; back=1; L=1; frontL=1; wait(0.5); front = 0; frontR = 0; R=0; back=0; L=0; frontL=0; wait(0.5); front = 1; frontR = 1; R=1; back=1; L=1; frontL=1; wait(0.5); there = 0; waypoint=waypoint+1; } //pc.printf("^7"); wait(.5); front = 0; frontR = 0; R=0; back=0; L=0; frontL=0; //pc.printf("^8"); } }