Revision 0:a955f7fabdf5, committed 2014-04-29

Comitter:

landnavvest

Date:

Tue Apr 29 03:02:11 2014 +0000

Commit message:

Zachary Motter

Changed in this revision

--- /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");
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 29 03:02:11 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6473597d706e
\ No newline at end of file