Diff: main.cpp

Revision:

36:8e84b5ade03e

Parent:

35:f9caeb8ca31e

Child:

37:ac60a8a0ae8a

--- a/main.cpp	Sat Feb 21 13:47:37 2015 +0000
+++ b/main.cpp	Sat Feb 21 17:51:04 2015 +0000
@@ -27,7 +27,7 @@
 //all timing objects
 Timer gps_connecting;
 Timer autosteer_time;
-Timeout autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed.
+Timer autosteer_timeout; //timeout work near as timer, but they don't give timing. they just trigger an interrupt once the time we assigned it is passed.
 Ticker accelerometerTicker;
 Ticker gyroscopeTicker;
 Ticker filterTicker;
@@ -145,6 +145,7 @@
 double Freepilot_lon1;
 double Freepilot_bearing;
 
+int time_till_stop = 200;
 volatile bool newline_detected = false;
 
 Point point_add(Point a, Point b)
@@ -299,7 +300,6 @@
         token = strtok((char *)NULL, ",");
         token_counter++;
     }
-
     if (latitude && longitude && altitude && sats) {
         decimal_latitude = lat_to_deg(latitude,  lat_dir[0]);
         decimal_lon = lon_to_deg(longitude, lon_dir[0]);
@@ -312,7 +312,6 @@
 
 void autosteer_done()
 {
-    //kill the autosteer once the timeout reech
     enable_motor = 0;
 }
 
@@ -339,8 +338,6 @@
     yaw = get_yaw();
 }
 
-
-
 void pitch_and_roll(double real_bearing)
 {
     pitch = get_pitch();
@@ -378,6 +375,7 @@
     int d = (int)(dec_deg);
     sprintf(dms,"%0.2i\0",d);
     double m = (double)(((double)dec_deg - (double)d) * 60.0);
+    // m = m + d*100;
     if (m < 10 ) {
         sprintf(dms,"%s0%0.9f\0",dms,m);
     } else {
@@ -445,7 +443,9 @@
     if ( asteer_speed && asteer_time ) {
         motorspeed = atof(asteer_speed);
         enable_time = atof(asteer_time);
-        autosteer_timeout.attach_us(autosteer_done,(double)enable_time *  (double)1000.0);
+        autosteer_timeout.reset();
+        time_till_stop = atoi(asteer_time);
+        //autosteer_timeout.attach_us(autosteer_done,(double)enable_time *  (double)1000.0);
         if ( motorspeed > 127.0 ) {
             pwm2_speed = 0.0;
             pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
@@ -477,7 +477,7 @@
 //distance in meters, always positive
 //angle in degrees
 //Points in lat/lon format
-int freepilot(Point current, Point target, double heading_err, double dist_line, double scale, double filter_g, double phase_adv)//dist in meters
+int freepilot(Point current, Point target, double heading_err, double dist_line, double scale, double filter_g, double phase_adv, double center)//dist in meters
 {
     double error = 0;
     // int motor_pwm = 0;
@@ -518,6 +518,8 @@
     error = error + 255;
     error = (int)(error / 2);
 
+    error = (int)((double)error / 255 * (255- center) + center);
+
     return (int)error;
 }
 
@@ -613,12 +615,12 @@
 
     double ErrAngle = asin(abs(distance_to_line * filtering)/(sqrt(lookaheaddistance*lookaheaddistance + abs(distance_to_line * filtering*distance_to_line * filtering))))*57.295779513;
 
-int val = freepilot(position, looked_ahead, ErrAngle, abs(distance_to_line*filtering), scale,  filterg,phaseadv);//dist in meters
+    int val = freepilot(position, looked_ahead, ErrAngle, abs(distance_to_line*filtering), scale,  filterg,phaseadv,tcenter);//dist in meters
 
-char command[128];
-sprintf(command,"$ASTEER,%i,%i\r\n",val,200);
-pc.puts(command);
-process_ASTEER(command);
+    char command[128];
+    sprintf(command,"$ASTEER,%i,%i\r\n",val,200);
+    pc.puts(command);
+    process_ASTEER(command);
 
     string rmc = "";
     if(sizeof(time) > 0) {
@@ -671,7 +673,7 @@
 
     bluetooth.puts(output);
 //pc.puts(output);
-      sprintf(output,"$DIST_TO_LINE: % .12f\r\n",distance_to_line * filtering);//,motor_val,ErrAngle);
+    sprintf(output,"$DIST_TO_LINE: % .12f\r\n",distance_to_line * filtering);//,motor_val,ErrAngle);
     pc.puts(output);
 }
 
@@ -785,9 +787,6 @@
         avgpos = atof(avg);
         tcenter = atof(center);
         filterg = atof(fg);
-        //   / sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
-        // pc.puts(output);
-        //SetFilter(phaseadv, tcenter, lookaheadtime, scale,filterg);
     }
 }
 
@@ -821,9 +820,7 @@
             gps.putc(c);
             pc.putc(c);
         }
-    }
-
-    else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
+    } else if (!strncmp(pc_string, "$GPSHEIGHT",10)) {
         process_GPSHEIGHT(pc_string);
         sprintf(output,"%s\r\n",pc_string);
         bluetooth.puts(output);
@@ -865,6 +862,8 @@
     if (!strncmp(gps_string, "$GPRMC", 6)) {
         //   pc.puts(gps_string);
         nmea_rmc(gps_string); //analyse and decompose the rmc string
+    } else {
+        bluetooth.puts(gps_string);
     }
 }
 
@@ -950,7 +949,7 @@
 
 void keyPressedHeld( void )
 {
-    motor_enable_state = "$ENABLE,1\r\n";
+    motor_enable_state = "$ENABLE,0\r\n";
     motor_enable = 1;
     ledGREEN=1; //show green for being ready to steer
 }
@@ -1024,10 +1023,7 @@
     motTimer.reset();
     lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
     motor_enable_state = "$ENABLE,1\r\n";
-    initializeAccelerometer();
-    calibrateAccelerometer();
-    initializeGyroscope();
-    calibrateGyroscope();
+
     btTimer.start();
     btTimer.reset();
     lastgetBT=  btTimer.read_ms();
@@ -1065,12 +1061,17 @@
     motor_switch.attach_asserted_held( &keyPressedHeld );
     motor_switch.attach_deasserted_held( &keyReleasedHeld );
 
+    initializeAccelerometer();
+    calibrateAccelerometer();
+    initializeGyroscope();
+    calibrateGyroscope();
+
     accelerometerTicker.attach(&sampleAccelerometer, 0.005);
     gyroscopeTicker.attach(&sampleGyroscope, 0.005);
     filterTicker.attach(&filter, FILTER_RATE);
     angle_print.attach(&toprint,0.2);
     activate_antenna();
-
+    autosteer_timeout.start();
     while(1) {
         //JH send version information every 10 seconds to keep Bluetooth alive
         if ((vTimer.read_ms()-lastsend_version)>25000) {
@@ -1080,10 +1081,13 @@
             lastsend_version=vTimer.read_ms();
         }
 
+        if ( autosteer_timeout.read_ms() > 5000 || autosteer_timeout.read_ms() > time_till_stop ) {
+            autosteer_timeout.reset();
+            enable_motor = 0;
+        }
         if ( antenna_active == 1 && gps.readable()) {
             if (getline(false)) {
                 if ( validate_checksum(msg)) {
-                    }
                     gps_analyse(msg);
                 } else {
                     pc.puts("INVALID!!!!\r\n");