Diff: main.cpp

Revision:

39:6767d4c840f9

Parent:

38:b5352d6f8166

Child:

40:a68cc1a1a1e7

--- a/main.cpp	Wed Mar 04 13:11:33 2015 +0000
+++ b/main.cpp	Thu Mar 05 16:26:52 2015 +0000
@@ -11,8 +11,6 @@
 #include "checksum.h"
 #include <string.h>
 
-//buttun filter
-
 #define dot(u,v)   ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
 #define norm(v)     sqrt(dot(v,v))     // norm = length of  vector
 #define d(u,v)      norm(point_sub(u,v))          // distance = norm of difference
@@ -34,8 +32,6 @@
 Ticker filterTicker;
 Ticker  angle_print;
 
-//Ticker PID_TICK;
-
 //Motor
 PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 DigitalOut enable_motor(p7);
@@ -62,7 +58,6 @@
 Point yaw_compensated_position;
 
 extern int gyro_pos;
-
 double distance_to_line;
 
 //FreePilot variables
@@ -141,11 +136,6 @@
 double w_z;
 
 int readings[3];
-
-double Freepilot_lat;
-double Freepilot_lon;
-double Freepilot_lat1;
-double Freepilot_lon1;
 double Freepilot_bearing;
 
 int time_till_stop = 200;
@@ -167,14 +157,10 @@
     return yaw_angle;
 }
 
-void update_motor()
-{
-
-}
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //PID
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-double outMin_ = 0.0;
+/*double outMin_ = 0.0;
 double outMax_ = 510.0;
 double inMin_ = 0.0;
 double inMax_ = 10.0;
@@ -290,7 +276,7 @@
 
     //Scale the output from percent span back out to a real world number.
     return ((controllerOutput_ * outSpan_) + outMin_);
-}
+}*/
 
 double get_roll()
 {
@@ -353,7 +339,6 @@
     return val;
 }
 
-int dir = 1;
 // isLeft(): test if a point is Left|On|Right of an infinite 2D line.
 //    Input:  three points P0, P1, and P2
 //    Return: >0 for P2 left of the line through P0 to P1
@@ -367,14 +352,6 @@
     } else {
         isleft = -1;
     }
-    
-    if ( dir == 1 )
-    {
-        isleft = isleft;
-        } else
-        {
-            isleft *= -1;
-            }
     return (int)isleft;
 }
 
@@ -554,7 +531,6 @@
         enable_time = atof(asteer_time);
         autosteer_timeout.reset();
         time_till_stop = atoi(asteer_time);
-        //autosteer_timeout.attach_us(autosteer_done,(double)enable_time *  (double)1000.0);
         if ( motor_enable == 0 ) {
         } else {
             if ( motorspeed > 127.0 ) {
@@ -574,9 +550,7 @@
             pwm1 = pwm1_speed;
             pwm2 = pwm2_speed;
             enable_motor = 1;
-            //  pc.puts("SPEED,%f,%f\r\n",pwm1_speed,pwm2_speed);
             //}
-
         }
     }
 }
@@ -590,7 +564,6 @@
 {
     dist_line = abs(dist_line);
     double error = 1;
-   // 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);
@@ -600,23 +573,18 @@
     } else if ( forward_sign == 1 ) {
         error = error;
     }
-
-    if ( forward_sign == position_sign ) {
-        // double temp = dist_line;
-
-        /*  if ( lastval > abs(dist_line)) {
-              dist_line = dist_line / 2.0;
-          }
-          lastval = temp;
-        */
-        if ( position_dist > (dist_line + (0.2 * position_dist))) { // && abs(position_dist) <
-            //added * position_dist
-            error = error * (dist_line * filter_g - position_dist * phase_adv);
-        } else {
-            error = error * dist_line * filter_g;
+    if ( position_dist < 0.50 ) {
+        if ( forward_sign == position_sign ) {
+            if ( position_dist > (dist_line + (0.5 * position_dist))) { // && abs(position_dist) <
+                error = error * (dist_line * filter_g - position_dist * phase_adv);
+            } else {
+                error = error * dist_line * filter_g;
+            }
+        } else { //
+            error = error * ((dist_line * filter_g)*0.5 - position_dist * phase_adv) * 0.5;//.8 in order to come back less on line than we came on it
         }
-    } else {
-        error = error * (dist_line * filter_g - position_dist * phase_adv) * 0.8;//.8 in order to come back less on line than we came on it
+    } else { //target coming back at 15-20 degrees on the line
+        error = error * (20 - heading_err) * 5;
     }
 
     error = error * scale;
@@ -667,9 +635,6 @@
 //char rmc_cpy[256];
 void nmea_rmc(char *s)
 {
-    // pc.puts(s);
-    // strcpy(rmc_cpy, s);
-    // sprintf(s,"%s\0",s);
     char *token;
     int  token_counter = 0;
     char *time   = (char *)NULL;
@@ -695,8 +660,7 @@
                 stat   = token;
                 break;
             case 3:
-                if ( token ) 
-                {
+                if ( token ) {
                     latit  = token;
                     latitude  = token;
                 }
@@ -745,10 +709,22 @@
         magvar     = atof(magv);
         // magvar_dir = magd[0];
     }
+
+    double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
+    double diff_angle = Freepilot_bearing - angle;
+    diff_angle = ((int)diff_angle + 180) % 360 - 180;
+
+    // pc.printf("%f  %f  %f\r\n",diff_angle, Freepilot_bearing, (track - 90) * -1);
+    if ( abs(diff_angle) > 90 ) {
+        if ( (abs(360 - diff_angle)) > 90 ) {
+            Point temp = line_end;
+            line_end = line_start;
+            line_start = temp;
+            Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
+        }
+    }
     if ( longit != '\0' && latit != '\0' ) {
         old_position = position;
-
-
         position.SetX(lat_to_deg(latitude,  lat_dir[0]));
         position.SetY(lon_to_deg(longitude, lon_dir[0]));
         cm_per_deg_lat = 11054000;
@@ -768,10 +744,9 @@
         double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
         distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;
 
-        int val = freepilot(position, looked_ahead, 0.0, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
+        int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
 
         char command[128];
-
         sprintf(command,"$ASTEER,%i,%i\r\n",val,250);    //(int)((((val/2)-127)/scale)+127),500);
         pc.puts(command);
         process_ASTEER(command);
@@ -828,28 +803,10 @@
     char test[256];
     sprintf(test,"%s*\0",rmc.c_str());
     sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
-    
+
     bluetooth.puts(output);
 }
 
-void line_dir(Point current_pos, Point old_pos, Point start, Point end, double line_heading, Point &start2, Point &end2)
-{
-    double ahead_lat = 0.0;
-    double ahead_lon = 0.0;
-//void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
-    get_latlon_byangle(start.GetX(), start.GetY(), (double)2500.0, ((line_heading - 90) * -1), ahead_lon, ahead_lat);
-    
-    if ( sqrt((ahead_lon - current_pos.GetY())*(ahead_lon - current_pos.GetY()) + (ahead_lat - current_pos.GetX())*(ahead_lat - current_pos.GetY()))
-            > sqrt((ahead_lon - old_pos.GetY())*(ahead_lon - old_pos.GetY()) + (ahead_lat - old_pos.GetX())*(ahead_lat - old_pos.GetX()))) {
-Point temp = line_start;
-line_start = line_end;
-line_end = temp;
-            
-        }
-    
-    }
-
-
 void process_FGPSAB(char* ab)
 {
     char *token;
@@ -888,31 +845,46 @@
         token = strtok((char *)NULL, ",");
         token_counter++;
     }
-    Freepilot_lon  = atof(line_lon);
-    Freepilot_lat = atof(line_lat);
-    Freepilot_lon1  = atof(line_lon1);
-    Freepilot_lat1 = atof(line_lat1);
-    Freepilot_bearing = atof(bearing.c_str());
-
+    double Freepilot_lon  = atof(line_lon);
+    double Freepilot_lat = atof(line_lat);
+    double Freepilot_lon1  = atof(line_lon1);
+    double Freepilot_lat1 = atof(line_lat1);
+    Freepilot_bearing = atof(bearing.c_str()) + 360;
+    Freepilot_bearing = ((int)Freepilot_bearing+ 180) % 360 - 180;
     line_start.SetX(Freepilot_lat);
     line_start.SetY(Freepilot_lon);
     line_end.SetX(Freepilot_lat1);
     line_end.SetY(Freepilot_lon1);
+    /*
+        double diff_angle = Freepilot_bearing - track;
+        diff_angle = ((int)diff_angle + 180) % 360 - 180;
 
+        if ( abs(diff_angle) > 90 ) {
+            line_end.SetX(Freepilot_lat);
+            line_end.SetY(Freepilot_lon);
+            line_start.SetX(Freepilot_lat1);
+            line_start.SetY(Freepilot_lon1);
+            Freepilot_bearing = Freepilot_bearing + 180;
+            Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
+        } else {
+            line_start.SetX(Freepilot_lat);
+            line_start.SetY(Freepilot_lon);
+            line_end.SetX(Freepilot_lat1);
+            line_end.SetY(Freepilot_lon1);
+            Freepilot_bearing = Freepilot_bearing;
+        }
+    */
     pc.puts(bearing.c_str());
     pc.puts("\r\n");
-    
+
     active_AB = 1;
 
-   // line_dir(position ,old_position, line_start, line_end, (atof(bearing.c_str())- 90) * -1, line_start,line_end);
-
     sprintf(output, "$ABLINE:%f , %f, %f, %f,  %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY(),Freepilot_bearing);
     pc.puts(output);
 }
 
 void process_FGPSAUTO(char* FGPSAUTO)
 {
-    //pc.puts(FGPSAUTO);
     char *token;
     int  token_counter = 0;
     char *ahead  = (char *)NULL;
@@ -924,7 +896,7 @@
     char *_ki  = (char *)NULL;
     char *fg  = (char *)NULL;
     char *_kd = (char *)NULL;
-    
+
     token = strtok(FGPSAUTO, ",");
     while (token) {
         switch (token_counter) {
@@ -963,8 +935,6 @@
         kp = atof(_kp);
         ki = atof(_ki);
         kd = atof(_kd);
-
-        // setTunings(kp , ki ,kd);
     }
     if ( phase && center && scl && avg && ahead ) {
         lookaheadtime = atof(ahead);
@@ -974,16 +944,15 @@
         tcenter = atof(center);
         filterg = atof(fg);
 
-            scale = 5 - scale;
-
+        scale = scale * -1;
     }
 }
 
 void pc_analyse(char* pc_string)
 {
-    // pc.puts(pc_string);
     if (!strncmp(pc_string, "$ASTEER", 7)) {
-        //  process_ASTEER(pc_string);
+        //stop sending when already exists
+
     } else if (!strncmp(pc_string, "$BANY",5)) {
         _ID = Config_GetID();
         Config_Save();
@@ -1032,7 +1001,6 @@
             index++;
         }
         gyro_pos = atoi(Data[1]);
-        //      pc.puts("POSITION=%i\r\n",gyro_pos);//("POSITION=");
         Config_Save();
     } else {
     }
@@ -1040,10 +1008,7 @@
 
 void gps_analyse(char* gps_string)
 {
-    //pc.puts(gps_string);
-    // bluetooth.puts(gps_string);
     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);
@@ -1265,7 +1230,7 @@
     activate_antenna();
     autosteer_timeout.start();
 
-    setTunings(0.25, 5, 1);
+    //setTunings(0.25, 5, 1);
 
     while(1) {
         //JH send version information every 10 seconds to keep Bluetooth alive
@@ -1318,7 +1283,6 @@
         }
         if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0){
             lastpitch = 0.8*lastpitch + 0.2 * (toDegrees(get_pitch()*3.5));
-
             lastroll = 0.8 * lastroll + 0.2 * toDegrees(get_roll()*3.5);
 
             double dps = - last_yaw;
@@ -1326,10 +1290,8 @@
             dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5
 
             sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
-            //      pc.puts(output);
             bluetooth.puts(output);
             angle_send = 0;
-
             counter++;
             if ( bear == false  && counter > 3 ) { //reinitialise the gyro after  600ms for the reference to be changed.
                 imuFilter.reset();