Diff: main.cpp

Revision:

42:854d8cc26bbb

Parent:

41:a26acd346c2f

Child:

43:e3f064f5eecd

diff -r a26acd346c2f -r 854d8cc26bbb main.cpp
--- a/main.cpp	Fri Mar 06 21:19:23 2015 +0000
+++ b/main.cpp	Mon Mar 09 15:58:28 2015 +0000
@@ -352,7 +352,7 @@
     }
     if ( height ) {
         antennaheight = atof(height);
-        Config_Save();
+        // Config_Save();
     }
 }
 
@@ -434,7 +434,7 @@
     }
 }
 
-double lastval = 0;
+/*double lastval = 0;
 //gets the motor value between -255 and 255 (- means left, positive means right)
 //distance in meters, always positive
 //angle in degrees
@@ -485,7 +485,7 @@
 
     return (int)error;
 }
-
+*/
 char *strsep(char **stringp, char *delim)
 {
     char *s;
@@ -524,21 +524,21 @@
 #define PI 3.14159265359
 #endif
 
-double m_Tcenter[10];
-double mphaseadv[10];
-double morder[10];
+double m_Tcenter[5] = {0,0,0,0,0};
+double mphaseadv[5]= {0,0,0,0,0};
+double morder[5]= {0,0,0,0,0};
 int order;
-double B0[10];
-double B1[10];
-double B2[10];
-double B3[10];
-double A_1[10];
-double A_2[10];
-double A_3[10];
+double B0[5]= {0,0,0,0,0};
+double B1[5]= {0,0,0,0,0};
+double B2[5]= {0,0,0,0,0};
+double B3[5]= {0,0,0,0,0};
+double A_1[5]= {0,0,0,0,0};
+double A_2[5]= {0,0,0,0,0};
+double A_3[5]= {0,0,0,0,0};
 
-double mx[10];
-double my[10];
-double mz[10];
+double mx[5]= {0,0,0,0,0};
+double my[5]= {0,0,0,0,0};
+double mz[5]= {0,0,0,0,0};
 int Err_aPort = 0;
 
 double OutputValue = 0;
@@ -572,7 +572,7 @@
     double _L = (1 - 2 * _T1 / _T);
     double _M = (1 + 2 * _T2 / _T);
     double _N = (1 - 2 * _T2 / _T);
-order = 2;
+//   order = 2;
     //version 1,
     switch (order) {
         case 3:
@@ -614,22 +614,22 @@
 {
     string sRet = "";
     //f ((Err_aPort == 0)) {
-        //  if (ActiveTime > 300) ActiveTime = 300;
-        if (value < 0) value = 0;
-        if ((value > 255)) value = 255;
-        OutputValue = value;
-        OutputTime = ActiveTime;
+    //  if (ActiveTime > 300) ActiveTime = 300;
+    if (value < 0) value = 0;
+    if ((value > 255)) value = 255;
+    OutputValue = value;
+    OutputTime = ActiveTime;
 
-        // d =  //= DateTime.Now - autosteer.LastCommunication;
+    // d =  //= DateTime.Now - autosteer.LastCommunication;
 
-        //no need to send repeated 127=do nothing
-        //if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
-            sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
-            LastOutputValue = OutputValue;
-            //  autosteer.Timer1counter = 0;
-            //  autosteer.LastCommunication = DateTime.Now;
-        //}
- //  == }
+    //no need to send repeated 127=do nothing
+    //if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
+    sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
+    LastOutputValue = OutputValue;
+    //  autosteer.Timer1counter = 0;
+    //  autosteer.LastCommunication = DateTime.Now;
+    //}
+//  == }
     return (sRet);
 }
 
@@ -639,69 +639,70 @@
 
     int N = 3;
     double y = 0;
-  //  if (B0[0] == 9999.0) {
-   //     SetDigitalFilter(47, 4.2 / 2 / PI, 0);
-   // }
-  //  if (distAUTOL == 5000) distAUTOL = 0; //not set
- //   if (speed < 1) steer=false;
+    //  if (B0[0] == 9999.0) {
+    //     SetDigitalFilter(47, 4.2 / 2 / PI, 0);
+    // }
+    //  if (distAUTOL == 5000) distAUTOL = 0; //not set
+//   if (speed < 1) steer=false;
 
     mx[N - 3] = mx[N - 2];
     mx[N - 2] = mx[N - 1];
     mx[N - 1] = mx[N];
-    mx[N] = distAUTOL * FilterGain;
+    if ( FilterGain > 0 ) {
+        if ( abs(distAUTOL) > 0 ) {
+            mx[N] = distAUTOL * FilterGain;
 
-    my[N - 3] = my[N - 2];
-    my[N - 2] = my[N - 1];
-    my[N - 1] = my[N];
+
+            my[N] = -A_1[0] * (double)my[N - 1] - A_2[0] * (double)my[N - 2] - A_3[0] * (double)my[N - 3] + B0[0] * (double)mx[N] + B1[0] * (double)mx[N - 1] + B2[0] * (double)mx[N - 2] + B3[0] * (double)mx[N - 3];
+            mz[N] = my[N];
 
-    mz[N - 3] = mz[N - 2];
-    mz[N - 2] = mz[N - 1];
-    mz[N - 1] = mz[N];
+            my[N - 3] = my[N - 2];
+            my[N - 2] = my[N - 1];
+            my[N - 1] = my[N];
 
-    my[N] = -A_1[0] * (double)my[N - 1] - A_2[0] * (double)my[N - 2] - A_3[0] * (double)my[N - 3] + B0[0] * (double)mx[N] + B1[0] * (double)mx[N - 1] + B2[0] * (double)mx[N - 2] + B3[0] * (double)mx[N - 3];
-    mz[N] = my[N];
-
+            mz[N - 3] = mz[N - 2];
+            mz[N - 2] = mz[N - 1];
+            mz[N - 1] = mz[N];
+        }
+    }
     double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
-    pc.printf("%f\r\n",y1);
+    //pc.printf("%f\r\n",y1);
 
     //modify scale depending on distance!
     double mscale = SCALE;
-    if (abs(distAUTOL) > 0.5) mscale = scale * 1.5;
-    if (abs(distAUTOL) > 1.5) mscale = scale * 1.5;
+    //if (abs(distAUTOL) > 0.5) mscale = SCALE * 1.5;
+    //if (abs(distAUTOL) > 1.5) mscale = SCALE * 1.5;
+    mscale = 0.85 * abs(distAUTOL) + SCALE;
 
     y = (double)(y1 * mscale);      // scale it here   if neccesary
-    pc.printf("%f\r\n",y);
+    // pc.printf("%f\r\n",y);
 
     // y = (double) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
     y = (double)(y * pow((10.0 / speed), SpeedN)); //SpeedN varies the gain factor with speed  n=0 to 1 or more. n=0 should give  y=
 
-   y = y * SCALE;
-/*
-            if ( y < -max )
-            {
-                y = -max;
-            }
-            else if ( y > max)
-            {
-                y = max;
-            }
+    y = y * SCALE;
 
-            y = 127 + y;
+    if ( y < -max ) {
+        y = -max;
+    } else if ( y > max) {
+        y = max;
+    }
+    y = 127 + y;
+
+    if (y <= 127) y = y - (min / 2.0);
+    if (y >= 127) y = y + (min / 2.0);
 
-            if (y <= 127) y = y - (min / 2.0);
-            if (y >= 127) y = y + (min / 2.0);
-*/
+    //  y = y + 127;
+    if (y >= 255) y = 255;
+    if (y <= 0) y = 0;
+
+    int value = (int)y;
 
-y = y + 127;
-            if (y >= 255) y = 255;
-            if (y <= 0) y = 0;
-
-            int value = (int)y;
-    pc.printf("%f\r\n",y);
-
-    //if (steer) {
+    if (speed > 1.0 ) {
         sRet= Steer( ActiveTime, value);
-    //}
+    } else {
+        sRet = Steer( ActiveTime, 127 );
+    }
 
     return (sRet);
 }
@@ -803,7 +804,7 @@
         cm_per_deg_lat = 11054000;
         cm_per_deg_lon = 11132000 * cos(decimal_latitude);
 
-//        pitch_and_roll((track-90)*-1);// as to be real bearing
+        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);
@@ -816,12 +817,11 @@
         looked_ahead.SetY(look_ahead_lon);
         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;/////////////////////////////////////////////////
-             SetDigitalFilter(phaseadv,tcenter, 0 );
+        SetDigitalFilter(phaseadv,tcenter, 0 );
 
         //  int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
-        string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 100.0, scale);
-//pc.printf("%f\r\n",distance_to_line);
-
+        string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 75.0, scale);
+        //pc.printf("%f\r\n",distance_to_line);
 
         char command[128];
         sprintf(command,"%s\r\n\0",steering.c_str());    //(int)((((val/2)-127)/scale)+127),500);
@@ -1013,7 +1013,7 @@
         //stop sending when already exists
     } else if (!strncmp(pc_string, "$BANY",5)) {
         _ID = Config_GetID();
-        Config_Save();
+        // Config_Save();
     } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
         string mystring = pc_string;
         string baud = pc_string;
@@ -1021,14 +1021,14 @@
             string baud = mystring.substr(0,mystring.find('*'));
         }
         process_GPSBAUD((char*)baud.c_str());
-        Config_Save();
+        // Config_Save();
         sprintf(output,"%s %d\r\n",pc_string,gps_baud);
         //   pc.puts(output);
     } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
         process_FGPSAUTO(pc_string);
         sprintf(output,"%s\r\n",pc_string);
         bluetooth.puts(output);
-        Config_Save();
+        // Config_Save();
     } else if (!strncmp(pc_string, "$FGPS,",6)) {
         int i=5;
         char c=pc_string[i];
@@ -1043,13 +1043,13 @@
         process_GPSHEIGHT(pc_string);
         sprintf(output,"%s\r\n",pc_string);
         bluetooth.puts(output);
-        Config_Save();
+        // Config_Save();
     } else if (!strncmp(pc_string, "$FGPSAB",7)) {
         process_FGPSAB(pc_string);
     } else if (!strncmp(pc_string, "$CALIBRATEACCEL",15)) {
         calibrateGyroscope();
         calibrateAccelerometer();
-        Config_Save();
+        //  Config_Save();
     } else if (!strncmp(pc_string, "$POSITION",9)) {
         char* pointer;
         char* Data[5];
@@ -1064,7 +1064,7 @@
             index++;
         }
         gyro_pos = atoi(Data[1]);
-        Config_Save();
+        //    Config_Save();
     } else {
     }
 }