Diff: main.cpp

Revision:

37:ac60a8a0ae8a

Parent:

36:8e84b5ade03e

Child:

38:b5352d6f8166

diff -r 8e84b5ade03e -r ac60a8a0ae8a main.cpp
--- a/main.cpp	Sat Feb 21 17:51:04 2015 +0000
+++ b/main.cpp	Sun Feb 22 21:55:45 2015 +0000
@@ -37,8 +37,8 @@
 PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 DigitalOut enable_motor(p7);
 
-PwmOut pwm1(p22);
-PwmOut pwm2(p23);
+PwmOut pwm1(p21);
+PwmOut pwm2(p22);
 
 //equipment switches
 PinDetect  boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
@@ -384,28 +384,6 @@
     return dms;
 }
 
-
-/*string To_DMS_lon(double dec_deg)
-{
-    dms = "";
-    dec_deg = abs(dec_deg);
-    int d = (int)(dec_deg);
-    sprintf(dms2,"%i",d);
-    dms = string(dms2);
-    double m = (double)(((double)dec_deg - (double)d) * 60.0);
-    sprintf(dms2,"%0.9f",m);
-    if ( m < 10 ) {
-        dms += ("00" + string(dms2));
-    } else if ( m < 100 ) {
-        dms += ("0" + string(dms2));
-    } else {
-        dms += string(dms2);
-    }
-    //sprintf(dms,"%03d%09.7f\0",d,m);
-    return dms;
-}*/
-
-
 char* To_DMS_lon(double dec_deg)
 {
     dec_deg = abs(dec_deg);
@@ -446,30 +424,36 @@
         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;
-            enable_motor = 1;
-        } else if ( motorspeed < 127.0 ) {
-            pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
-            pwm1_speed = 0.0;
-            enable_motor = 1;
+        if ( motor_enable == 0 ) {
+
         } else {
-            pwm1_speed = 0;
-            pwm2_speed = 0;
-            enable_motor = 0;
+            if ( motorspeed > 127.0 ) {
+                pwm2_speed = 0.0;
+                pwm1_speed = ((double)motorspeed - (double)127.0) / 128.0;
+
+            } else if ( motorspeed < 127.0 ) {
+                pwm1_speed = 0.0;
+                pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
+
+            } else {
+                pwm1_speed = 0;
+                pwm2_speed = 0;
+                enable_motor = 0;
+            }
+            //  if(Authenticated)
+            // {
+            pwm1 = pwm1_speed;
+            pwm2 = pwm2_speed;
+            enable_motor = 1;
+            pc.printf("SPEED,%f,%f\r\n",pwm1_speed,pwm2_speed);
+            //  }
+            //else
+            //{
+            //   sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);
+            //   pc.puts(output);
+            //   bluetooth.puts(output);
+            // }
         }
-        //  if(Authenticated)
-        // {
-        pwm1 = pwm1_speed;
-        pwm2 = pwm2_speed;
-        //  }
-        //else
-        //{
-        //   sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);
-        //   pc.puts(output);
-        //   bluetooth.puts(output);
-        // }
     }
 }
 
@@ -477,36 +461,30 @@
 //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, double center)//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, double filtering)//dist in meters
 {
     double error = 0;
-    // int motor_pwm = 0;
-
+    scale = scale - 5;
     int position_sign = isLeft( line_start, line_end, current);
     int forward_sign = isLeft(line_start,line_end, target);
+    double position_dist = abs(dist_Point_to_Line( current,line_start,line_end) * filtering);
 
-    dist_line = dist_line * filter_g;
-    heading_err = heading_err * phase_adv;
-    if ( heading_err > 45 ) {
-        heading_err = 45;
-    }
+    error = 1;
 
-    if ( position_sign == forward_sign ) {
-        error = dist_line + heading_err;
-    } else {
-        error = dist_line - heading_err;
+    if ( forward_sign == -1 ) {
+        error = error * -1;
+    } else if ( forward_sign == 1 ) {
+        error = error;
     }
-
-    if (position_sign == forward_sign && forward_sign == -1 ) {
-        error = error * -1;
-    } else if (position_sign == forward_sign && forward_sign == 1 ) {
-        error = error;
-    } else if(position_sign != forward_sign && forward_sign == 1) {
-        error = (error / 2);
-    } else if ( position_sign != forward_sign  && forward_sign == -1) {
-        error = (error / 2) * -1;
+    if ( forward_sign == position_sign ) {
+        if ( position_dist > (dist_line + 0.2)) {
+            error = error * (dist_line * filter_g - position_dist * phase_adv);
+        } else {
+            error = error * dist_line * filter_g;
+        }
+    } else {
+        error = error * (dist_line * filter_g - position_dist * phase_adv) * 0.8;
     }
-
     error = error * scale;
 
     if ( error > 255 ) {
@@ -514,12 +492,9 @@
     } else if ( error < -255 ) {
         error = -255;
     }
-
     error = error + 255;
     error = (int)(error / 2);
 
-    error = (int)((double)error / 255 * (255- center) + center);
-
     return (int)error;
 }
 
@@ -599,7 +574,7 @@
     cm_per_deg_lat = 11054000;
     cm_per_deg_lon = 11132000 * cos(decimal_latitude);
 
-    pitch_and_roll((track-90)*-1);// as to be real ebaring
+    pitch_and_roll((track-90)*-1);// as to be real bearing
 
     compensation.SetY(compensation.GetY() / cm_per_deg_lon);
     compensation.SetX(compensation.GetX() / cm_per_deg_lat);
@@ -615,10 +590,10 @@
 
     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,tcenter);//dist in meters
+    int val = freepilot(position, looked_ahead, ErrAngle, abs(distance_to_line*filtering), scale,  filterg,phaseadv,tcenter,filtering);//dist in meters
 
     char command[128];
-    sprintf(command,"$ASTEER,%i,%i\r\n",val,200);
+    sprintf(command,"$ASTEER,%i,%i\r\n",val,500);
     pc.puts(command);
     process_ASTEER(command);
 
@@ -796,7 +771,7 @@
 {
     // pc.puts(pc_string);
     if (!strncmp(pc_string, "$ASTEER", 7)) {
-        process_ASTEER(pc_string);
+        //  process_ASTEER(pc_string);
     } else if (!strncmp(pc_string, "$BANY",5)) {
         _ID = Config_GetID();
         Config_Save();
@@ -825,9 +800,7 @@
         sprintf(output,"%s\r\n",pc_string);
         bluetooth.puts(output);
         Config_Save();
-    }
-
-    else if (!strncmp(pc_string, "$FGPSAB",7)) {
+    } else if (!strncmp(pc_string, "$FGPSAB",7)) {
         process_FGPSAB(pc_string);
     } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
         calibrateGyroscope();
@@ -950,7 +923,9 @@
 void keyPressedHeld( void )
 {
     motor_enable_state = "$ENABLE,0\r\n";
-    motor_enable = 1;
+    motor_enable = 0;
+    pwm1 = 0.0;
+    pwm2 = 0.0;
     ledGREEN=1; //show green for being ready to steer
 }
 
@@ -958,7 +933,9 @@
 {
     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
     motor_enable_state = "$ENABLE,1\r\n";
-    motor_enable = 0;
+    motor_enable = 1;
+    pwm1 = 0.0;
+    pwm2 = 0.0;
     ledGREEN=0;
 }
 
@@ -1008,10 +985,8 @@
     angle_send = 1;
 }
 
-
 int main()
 {
-    //int x=0;
     bluetooth.baud(115200);
     gps.baud(38400);
     pc.baud(38400);
@@ -1121,7 +1096,6 @@
         }
         if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0)
             sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(get_roll()),toDegrees(get_pitch()),toDegrees(get_yaw()));
-            // pc.puts(output);
             bluetooth.puts(output);
             angle_send = 0;
         }