Diff: main.cpp

Revision:

14:94c65d1c8bad

Parent:

13:a18d41a60d0b

Child:

15:d6d7623a17f8

--- a/main.cpp	Tue Jan 24 01:33:59 2017 +0000
+++ b/main.cpp	Tue Jan 24 23:09:10 2017 +0000
@@ -77,7 +77,7 @@
 #define GPIOC p23
 #define GPIOD p24
 
-#define THROTTLE_DAMPNER 1000
+#define THROTTLE_DAMPNER 100
 
 #define VELOCITY_SAMPLES 1000
 
@@ -226,6 +226,10 @@
     float change_in_target_pos = 0;
     float target_pos = 0;
     float target_pos_old = 0;
+    float target_velocity = 0;
+    float velocity_error = 0;
+    float change_in_velocity = 0;
+    float d_velocity = 0;
     
     float runningSumReplaceVal = 0;
     float newVelocity = 0;
@@ -265,13 +269,13 @@
         //knob4 = 17.07;
 
         //Set Gainz with knobs
-        Kp1 = ((float)knob1) / 1000.0;
+        Kp1 = ((float)knob1) / 100.0;
         Kd1 = ((float)knob2) / 1.0;
         Kp2 = ((float)knob3) / 1000.0;
         Kd2 = ((float)knob4) / 100.0;
 
         //Joystick control
-        throttle = (float)jstick_v  /THROTTLE_DAMPNER;
+        throttle = (float)jstick_v / THROTTLE_DAMPNER;
         steering = (float)jstick_h / 10.0;
 
         //Calculate the delta time
@@ -300,31 +304,46 @@
             velocitySamples[velocitySamplesCounter] = newVelocity; //replace value with
             velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
             velocity = velocitySum/VELOCITY_SAMPLES;
-
-            //CS ***************** Velocity Controller *********************
-            //float target_velocity = 0;
-            //velocity_error = target_velocity - velocity;
-            //target_angle = -velocity_error * Kp2 * 100;
+            
             //CS ***************** Position Controller *********************
+            //target_pos = 0;
             pos_error = robot_pos - target_pos; //robot_pos - target_pos;
 
             //KP Term
-            kp_pos_term = -Kp2 * pos_error;
+            kp_pos_term = Kp1 * pos_error;
 
             //KD Term
             change_in_target_pos = target_pos - target_pos_old;
             //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
-            kd_pos_term = ((Kd2 * change_in_target_pos) + (Kd2*velocity));
-            target_angle = kp_pos_term + kd_pos_term;
-            target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
+            kd_pos_term = ((Kd1 * change_in_target_pos) + (Kd1*velocity));
+            target_velocity = kp_pos_term; // + kd_pos_term;
+            target_velocity = CAP(target_velocity, 100);
+
+            //CS ***************** Velocity Controller *********************
+            //target_velocity = throttle;
+            velocity_error = target_velocity - velocity;
+            change_in_velocity = velocity - velocitySamples[(velocitySamplesCounter + 500)%VELOCITY_SAMPLES];
+            d_velocity = (Kd2 * change_in_velocity)/dt;
             
+            /*
+            if(target_velocity <= 3 && target_velocity >= -3) {
+                target_angle = 0;    
+            }
+            else{
+                
+            }
+            */
+            target_angle = (-velocity_error * Kp2 * 50); //+ d_velocity;    
             //CS ************ PD Stability CONTROLLER HERE ****************
+            //Knob1 7.373/1000.0 Knob2 50.986/1.0
+            float Kp1S = 7.373/1000.0;
+            float Kd1S = 50.986/1.0;
             error = target_angle - angle;
-            kp_term = Kp1 * error;
+            kp_term = Kp1S * error;
 
             change_in_target_angle = target_angle - target_angle_old; //add
             change_in_angle = angle - angle_old2; //add
-            kd_term = ((Kd1 * change_in_target_angle) - (Kd1*change_in_angle)) / dt;
+            kd_term = ((Kd1S * change_in_target_angle) - (Kd1S*change_in_angle)) / dt;
             //kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
 
             //pc.printf("dAngle:%f\r\n", angle-angle_old1);
@@ -361,7 +380,7 @@
         if(loop_counter >= 20) {
             gpioMonitorC = 1;
             loop_counter = 0;
-            pc.printf("Avg Vel: %f\r\n",velocity);            
+            pc.printf("Target Vel: %f\r\n",target_velocity);            
             //pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f  Kp2: %0.2f Kd2: %0.3f  tang: %0.2f dt:%d rob_pos: %d VE: %f\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, robot_pos, velocity_error);
             //pc.printf("Jstick_h: %d Jstick_v: %d Knob1 %d Knob2 %d Knob3 %d Knob4 %d Button: %d\r\n", jstick_h, jstick_v, knob1, knob2, knob3, knob4, button);
             //pc.printf("CAngle: %d, TAngle: %d,Pos: %d, dt: %d\r\n",(int)angle,target_angle,robot_pos,dt);