Diff: main.cpp

Revision:

28:5905886c76ee

Parent:

27:9ac59b261d87

Child:

29:23ccb2a50b6f

--- a/main.cpp	Mon Jan 19 01:35:34 2015 +0000
+++ b/main.cpp	Wed Jan 21 02:54:26 2015 +0000
@@ -1,5 +1,5 @@
 #include "mbed.h"
-#include "MODSERIAL.h"
+//#include "MODSERIAL.h"
 #include "PinDetect.h"
 #include "IMUfilter.h"
 #include "ADXL345_I2C.h"
@@ -8,15 +8,13 @@
 #include <vector>
 #include "Line.h"
 #include "stringUtils.h"
+#include "base.h"
+#include "Config.h"
 
-char *version="FreePilot V2.1 Jan 16, 2015\r\n";
+char *version="FreePilot V2.11 Jan 20, 2015\r\n";
 long lastsend_version=0;
 Timer vTimer; //this timer is int based! Max is 30 minutes
 
-// Connect the TX of the GPS module to p10 RX input
-MODSERIAL b(p9, p10); //GPS
-MODSERIAL pc(USBTX, USBRX);
-MODSERIAL bluetooth(p13, p14);
 int checksumm;
 
 int dont = 0;
@@ -46,10 +44,10 @@
 //Motor
 PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 DigitalOut enable_motor(p7);
-DigitalOut led1(p11);
-DigitalOut led2(p12);
-PwmOut pwm1(p21);
-PwmOut pwm2(p22);
+
+PwmOut pwm1(p22);
+PwmOut pwm2(p21);
+
 
 //equipment switches
 PinDetect  boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
@@ -77,34 +75,27 @@
 Point yaw_compensated_position;
 
 double distance_to_line;
-//FreePilot parameters
-double look_ahead_time = 2;
-double look_ahead_distance = 5;
-double scale = 1;
-double phaseadv = 50;
-double _Tcenter = 5;
-double filter_gain = 125;
-double avg_pos = -4;
 
 //FreePilot variables
 int timer_enabled;
-int motorspeed;
+double motorspeed;
 int enable_time;
 char* motor_enable_state = 0;
-int motor_enable_tosend = 0;
+int motor_enable = 0;
+int lastmotor_enable = 1;
 double pwm1_speed;
 double pwm2_speed;
+long lastsend_motorstate=0;
+Timer motTimer; //this timer is int based! Max is 30 minutes
+Timer btTimer; //measure time for Bluetooth communication
+long lastgetBT=0;
 
-// in prevision of PID addition to FreePilot
-double kp = 0;
-double ki = 0;
-double kd = 0;
 
 int msg2_changed = 1;
 char* buffer;
 double meter_lat = 0;
 double meter_lon = 0;
-double antenna_height = 200;
+
 int antenna_active = 0;//do we have an antenna connected?
 char msg[256]; //GPS line buffer
 char msg2[256];//PC line buffer
@@ -135,7 +126,6 @@
 double speed_km;
 double speed_m_s = 0;
 double velocity; // speed in knot
-int gps_baud = 38400; //default at 115200, but FGPS will pass the real baud-rate.
 int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
 int print_gsa = 0;//FGPS request GSA printing
@@ -152,13 +142,6 @@
 int active_AB = 0;
 double compensation_vector;
 char output[256];
-//offsets
-double w_xBias;
-double w_yBias;
-double w_zBias;
-double a_xBias;
-double a_yBias;
-double a_zBias;
 
 double yaw;
 double pitch;
@@ -365,7 +348,7 @@
 
 void activate_antenna()
 {
-    b.baud(gps_baud);
+    gps.baud(gps_baud);
     antenna_active = 1;
 }
 
@@ -522,7 +505,7 @@
 void tilt_compensate()
 {
     roll = imuFilter.getRoll();
-    compensation_vector = antenna_height * sin(roll);
+    compensation_vector = antennaheight * sin(roll);
     compensation.SetX(compensation_vector * cos((toDegrees(imuFilter.getYaw()) *  -1 - 90)/57.295779513));
     compensation.SetY(compensation_vector * sin((toDegrees(imuFilter.getYaw()) *  -1 - 90)/57.295779513));
 }
@@ -558,7 +541,7 @@
     }
     if ( height ) 
     {
-        antenna_height = atof(height);
+        antennaheight = atof(height);
     }
 }
 
@@ -672,9 +655,9 @@
        // yaw_compensate();
         position = point_add(position,compensation);
         modify_rmc();
-        look_ahead_distance = look_ahead_time * speed_m_s;
+        double lookaheaddistance = lookaheadtime * speed_m_s;
         
-        get_latlon_byangle(position.GetX(),position.GetY(),look_ahead_distance,track,look_ahead_lon,look_ahead_lat);
+        get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track,look_ahead_lon,look_ahead_lat);
         looked_ahead.SetX(look_ahead_lat);
         looked_ahead.SetY(look_ahead_lon);
         double filtering = 111.111 / 2.0 + 111.111 * cos(decimal_latitude)/2.0;
@@ -793,12 +776,12 @@
     }
     if ( phase && center && scl && avg && ahead ) 
     {
-        look_ahead_time = atof(ahead);
+        lookaheadtime = atof(ahead);
         scale = atof(scl);
         phaseadv = atof(phase);
-        avg_pos = atof(avg);
-        _Tcenter = atof(center);
-         sprintf(output, "$SETTINGS:%f\r\n", look_ahead_time);
+        avgpos = atof(avg);
+        tcenter = atof(center);
+         sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
             pc.puts(output);
     }
 }
@@ -810,6 +793,7 @@
     char *asteer_speed  = (char *)NULL;
     char *asteer_time = (char *)NULL;
 
+
     token = strtok(asteer, ",");
     while (token) 
     {
@@ -829,17 +813,17 @@
     {
         motorspeed = atof(asteer_speed);
         enable_time = atof(asteer_time);
-        autosteer_timeout.attach(autosteer_done,(double)enable_time / (double)1000);
-        if ( motorspeed > 127 ) 
+        autosteer_timeout.attach_us(autosteer_done,(double)enable_time *  (double)1000.0);
+        if ( motorspeed > 127.0 ) 
         {
-            pwm2_speed = 0;
-            pwm1_speed = ((double)motorspeed - (double)127) / 127;
+            pwm2_speed = 0.0;
+            pwm1_speed = ((double)motorspeed - (double)127.0) / 127.0;
             enable_motor = 1;
         } 
-        else if ( motorspeed < 127 ) 
+        else if ( motorspeed < 127.0 ) 
         {
-            pwm2_speed = ((double)motorspeed / 127 );
-            pwm1_speed = 0;
+            pwm2_speed = ( ((double) 127-(double)motorspeed) / 127.0 );
+            pwm1_speed = 0.0;
             enable_motor = 1;
         } 
         else 
@@ -848,10 +832,13 @@
             pwm2_speed = 0;
             enable_motor = 0;
         }
+        sprintf(output,"STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);     
+        pc.puts(output);
         pwm1 = pwm1_speed;
         pwm2 = pwm2_speed;
     }
 }
+            
 
 void process_initstring(char* init)
 {
@@ -874,9 +861,9 @@
     {
         if ( antenna_active == 1 ) 
         {
-            while(!b.writeable()) {}   //disable uart3 interrupt (p9,p10)
+            while(!gps.writeable()) {}   //disable uart3 interrupt (p9,p10)
              sprintf(output,"$%s",init_string);
-            b.puts(output);
+            gps.puts(output);
             gps_connecting.start();
             gps_connecting.reset();
             connecting = 1;
@@ -933,12 +920,12 @@
           i++;
           if (i>255) break; //protect msg buffer!
           c=pc_string[i];
-          b.putc(c); 
+          gps.putc(c); 
           pc.putc(c);   
        }
       // sprintf(output,"%s\r\n",pc_string);
       // pc.puts(output);
-      // b.puts(output);
+      // gpsputs(output);
     } 
     else if (!strncmp(pc_string, "$PRINT_VTG=0",12)) 
     {
@@ -1120,11 +1107,11 @@
 {
     while (1)
     {    
-        if( !b.readable() )
+        if( !gps.readable() )
         {
             break;
         }
-        char c = b.getc();
+        char c = gps.getc();
         if (forward) //simply forward all to Bluetooth
         {
            bluetooth.putc(c);    
@@ -1170,22 +1157,22 @@
 
 void keyPressedHeld( void )
 {
-//    motor_enable_state = "$ENABLE=1\r\n";
-    motor_enable_tosend = 1;
-  //  led1=1;
+    motor_enable_state = "$ENABLE,1\r\n";
+    motor_enable = 1;
+    ledGREEN=1; //show green for being ready to steer
 }
 
 void keyReleasedHeld( void )
 {
- //   motor_enable_state = "$ENABLE=0\r\n";
-    motor_enable_tosend = 0;
-//    led1=0;
+    motor_enable_state = "$ENABLE,0\r\n";
+    motor_enable = 0;
+    ledGREEN=0; 
 }
 
 void boom1PressedHeld( void )
 {
   
-   // led1=1;
+   // ledGREEN=1;
  boom18=boom18 & 0xFE;
    //  sprintf(output,"Boom1 Pressed %d",boom18);     
    //  pc.puts(output);
@@ -1193,7 +1180,7 @@
 
 void boom1ReleasedHeld( void )
 {
-  led1=0;  
+  //ledGREEN=0;  
   boom18=boom18 | 0x01;
 
 }
@@ -1261,13 +1248,18 @@
 {
     int x=0;
     bluetooth.baud(115200);
-    b.baud(38400);
+    gps.baud(38400);
 pc.baud(38400);
 
 //JH prepare and send version info
     vTimer.start();
     vTimer.reset();
-
+    motTimer.start();
+    motTimer.reset();
+    btTimer.start();
+    btTimer.reset();
+    lastgetBT=  btTimer.read_ms();
+    
     pc.puts(version);
     bluetooth.puts(version);
     lastsend_version=vTimer.read_ms();
@@ -1294,7 +1286,10 @@
     boom4.setSamplesTillHeld(5);
     boom4.setSampleFrequency();
        
-       
+    Config_Startup();
+  //  overide_config(gyro_position,gps_baud,antenna_height, a_zBias, a_yBias, a_zBias,w_xBias, w_yBias,w_zBias, copy_FREEPILOT);
+   // int _ID=Config_SetID();
+  //  Config_Save(_ID,_btMode,phaseadv,tcenter,filter_gain,scale,avg_pos,gyro_position,gps_baud,antenna_height, a_zBias, a_yBias, a_zBias,w_xBias, w_yBias,w_zBias);
     motor_switch.setSampleFrequency(10000);
     motor_switch.attach_asserted_held( &keyPressedHeld );
     motor_switch.attach_deasserted_held( &keyReleasedHeld );
@@ -1312,7 +1307,7 @@
     {
     
         //JH send version information every 10 seconds to keep Bluetooth alive
-        if ((vTimer.read_ms()-lastsend_version)>10000) 
+        if ((vTimer.read_ms()-lastsend_version)>15000) 
         {
        
             pc.puts(version);
@@ -1325,7 +1320,7 @@
           //  pc.puts(output);
         } 
         
-        if ( antenna_active == 1 && b.readable()) 
+        if ( antenna_active == 1 && gps.readable()) 
         {
             if (getline(true)) 
             {
@@ -1339,6 +1334,8 @@
         {
            if (getline2()) 
            {
+               btTimer.reset();
+               lastgetBT=  btTimer.read_ms();
                x++;
               //  trim(msg2,"\r");
              //   trim(msg2,"\n");
@@ -1352,13 +1349,21 @@
             }
         }
 
-        if ( motor_enable_tosend == 1 ) //&& reading == 0 ) 
+       if (  btTimer.read_ms()-lastgetBT>1000)
+       {
+           //we did not get any commands over BT
+           ledRED=1; //turn red
+       }
+       else ledRED=0;
+       
+       if ( ((motTimer.read_ms()-lastsend_motorstate)>5000) || (motor_enable!=lastmotor_enable)) 
         {
-            sprintf(output,"BT %f %f",pwm1_speed,pwm2_speed);     
-            pc.puts(output);
-                
+       
             bluetooth.puts(motor_enable_state);
-            motor_enable_tosend = 0;
+            pc.puts(motor_enable_state);
+            motTimer.reset();
+            lastsend_motorstate=motTimer.read_ms();
+            lastmotor_enable=motor_enable;
         }
 
         //bounces too much