Zack Motter
mbed Code for Multi-Waypoint Tests
Diff: main.cpp
- Revision:
- 0:a955f7fabdf5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 29 03:02:11 2014 +0000
@@ -0,0 +1,264 @@
+#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");
+ }
+}