Revision 33:a49197572737, committed 2013-04-10

Comitter:

madcowswe

Date:

Wed Apr 10 19:52:19 2013 +0000

Parent:

32:e3f633620816

Child:

34:e1678450feec

Child:

35:f8e7f0a72a3d

Commit message:

untuned navigation half working

Changed in this revision

--- a/Processes/AI/ai.cpp	Wed Apr 10 18:25:16 2013 +0000
+++ b/Processes/AI/ai.cpp	Wed Apr 10 19:52:19 2013 +0000
@@ -40,8 +40,6 @@
         }
         waypoint_flag_mutex.unlock();
     }
-    
-    Thread::yield(); //TODO!!!!!!!!!!!!!!!!!!!
 }
 
 void setWaypointReached()

--- a/Processes/Kalman/Kalman.h	Wed Apr 10 18:25:16 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Wed Apr 10 19:52:19 2013 +0000
@@ -7,13 +7,6 @@
 namespace Kalman
 {
 
-typedef struct State 
-{
-    float x;
-    float y;
-    float theta;
-} State;
-
 //Accessor function to get the state as one consistent struct
 State getState();
 

--- a/Processes/Motion/motion.cpp	Wed Apr 10 18:25:16 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 19:52:19 2013 +0000
@@ -16,7 +16,6 @@
     Waypoint target_waypoint = *AI::current_waypoint;
     
     // get current state from Kalman
-    using Kalman::State;
     State current_state = Kalman::getState();
     
     float delta_x = target_waypoint.x - current_state.x;
@@ -32,11 +31,14 @@
     if (distance_err < target_waypoint.pos_threshold)
     {
         distance_err = 0;
+        angle_err = constrainAngle(target_waypoint.theta - current_state.theta);
+        
+        if (abs(angle_err) < target_waypoint.angle_threshold)
+        {
+            angle_err = 0;
+        }
     }
-    if (abs(angle_err) < target_waypoint.angle_threshold)
-    {
-        angle_err = 0;
-    }
+    
         
     AI::waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag
     if (distance_err == 0 && angle_err == 0)
@@ -47,9 +49,9 @@
     AI::waypoint_flag_mutex.unlock();
     
     // angular velocity controller
-    const float p_gain_av = 0.3; //TODO: tune
+    const float p_gain_av = 0.8; //TODO: tune
     
-    const float max_av = 0.2*PI; // radians per sec //TODO: tune
+    const float max_av = 0.5*PI; // radians per sec //TODO: tune
     
     // angle error [-pi, pi]
     float angular_v = p_gain_av * angle_err;
@@ -62,7 +64,7 @@
     
     
     // forward velocity controller
-    const float p_gain_fv = 0.3; //TODO: tune
+    const float p_gain_fv = 0.8; //TODO: tune
     
     float max_fv = 0.2; // meters per sec //TODO: tune
     const float angle_envelope_exponent = 8.0; //TODO: tune

--- a/Processes/MotorControl/MotorControl.cpp	Wed Apr 10 18:25:16 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Wed Apr 10 19:52:19 2013 +0000
@@ -17,7 +17,18 @@
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
 
     float testspeed = 0.2;
-    float Pgain = 10;
+    float Fcrit = 1.75;
+    float Pcrit = 10;
+    float Pgain = Pcrit*0.45;
+    float Igain = 1.2f*Pgain*Fcrit*0.2;
+    
+    float testrot = 0.5*PI;
+    float Pcrit_rot = 10;
+    float Pgain_rot = Pcrit_rot*0.45f;
+    float Fcrit_rot = 1.75f;
+    float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.07;
+    
+    //float Dgain = 
     static float lastT = SystemTime.read();
     static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
     static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
@@ -42,12 +53,20 @@
 
     float errfwd = fwdfiltstate - fwdcmd;
     float errtheta = thetafiltstate - omegacmd;
+    
+    static float fwdIstate = 0;
+    static float rotIstate = 0;
+    fwdIstate += errfwd;
+    rotIstate += errtheta;
+    
+    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;
+    float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
 
-    float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
-    float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);
+    float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
+    float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);
 
-    mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
-    mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
 
 }
 

--- a/globals.cpp	Wed Apr 10 18:25:16 2013 +0000
+++ b/globals.cpp	Wed Apr 10 19:52:19 2013 +0000
@@ -3,4 +3,3 @@
 
 //Store global objects here
 pos beaconpos[] = {{-0.040,1}, {3.040,-0.040}, {3.040,2.040}};
-Timer SystemTime;

--- a/globals.h	Wed Apr 10 18:25:16 2013 +0000
+++ b/globals.h	Wed Apr 10 19:52:19 2013 +0000
@@ -19,7 +19,7 @@
 const float angvarpertime = 0;//0.001;
 
 const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this
-const float MOTOR_MAX_POWER = 0.4f;
+const float MOTOR_MAX_POWER = 0.5f;
 
 /*
 PINOUT Sensors
@@ -106,4 +106,11 @@
     float angle_threshold;
 } Waypoint;
 
+typedef struct State 
+{
+    float x;
+    float y;
+    float theta;
+} State;
+
 #endif //GLOBALS_H
\ No newline at end of file