Diff: main.cpp

Revision:

12:16d1a5390022

Parent:

11:2553f5798f84

Child:

13:a18d41a60d0b

--- a/main.cpp	Thu Jan 19 19:18:54 2017 +0000
+++ b/main.cpp	Tue Jan 24 01:30:16 2017 +0000
@@ -79,6 +79,8 @@
 
 #define THROTTLE_DAMPNER 1000
 
+#define VELOCITY_SAMPLES 1000
+
 
 
 
@@ -90,37 +92,19 @@
 
 
 
-//PID Default control values from constant definitions
-float Kp1 = KP;
-float Kd1 = KD;
-float Kp2 = KP_THROTTLE;
-float Ki2 = KI_THROTTLE;
-float Kd2; //Added for CS Pos contorl
-float PID_errorSum;
-float PID_errorOld = 0;
-float PID_errorOld2 = 0;
-float setPointOld = 0;
-float target_angle;
-float throttle = 0;
-float steering = 0;
-float max_throttle = MAX_THROTTLE;
-float max_steering = MAX_STEERING;
-float max_target_angle = MAX_TARGET_ANGLE;
-float control_output;
-int16_t actual_robot_speed;        // overall robot speed (measured from steppers speed)
-int16_t actual_robot_speed_old;
-float estimated_speed_filtered;    // Estimated robot speed
-int robot_pos = 0;
-float velocity_error;
+
+
+
+
+
+
+
+
 
 Timer timer;
-int timer_value; //maybe make this a long
-int timer_old; //maybe make this a long
-int dt;
-
-//uint8_t slow_loop_counter;
-//uint8_t medium_loop_counter;
-uint8_t loop_counter;
+int timer_value = 0;
+int timer_old = 0;
+float velocitySamples[VELOCITY_SAMPLES + 1] = {0.0};
 
 Serial pc(USBTX, USBRX);
 
@@ -142,25 +126,6 @@
 //Used to set angle upon startup
 bool initilizeAngle = true;
 
-//CS PID CONTROLLER TEST
-float target_angle_old = 0;
-float change_in_target_angle = 0;
-float change_in_angle = 0;
-float angle_old1 = 0;
-float angle_old2 = 0;
-float kp_term = 0;
-float kd_term = 0;
-float error;
-//For Position controller
-float pos_error = 0;
-float kp_pos_term = 0;
-float kd_pos_term = 0;
-float change_in_target_pos;
-float target_pos, target_pos_old;
-float change_in_pos;
-float robot_pos_old1, robot_pos_old2;
-float velocity;
-bool fallen = true;
 
 // ================================================================
 // ===                      IMU Thread                          ===
@@ -230,6 +195,43 @@
 // ================================================================
 void pid_update_thread(void const *args)
 {
+
+    float Kp1 = 0;
+    float Kd1 = 0;
+    float Kp2 = 0;
+    float Kd2 = 0;
+    float target_angle = 0;
+    float target_angle_old = 0;
+    float change_in_target_angle = 0;
+    float change_in_angle = 0;
+    float angle_old1 = 0;
+    float angle_old2 = 0;
+    float kp_term = 0;
+    float kd_term = 0;
+    float throttle = 0;
+    float steering = 0;
+    float control_output = 0;
+    int robot_pos = 0;
+    int loop_counter = 0;
+    int velocitySamplesCounter = 0;
+    int dt = 0;
+    float error = 0;
+    float velocity = 0;
+    bool fallen = true;
+    
+    // --- For Position controller  --- //
+    float pos_error = 0;
+    float kp_pos_term = 0;
+    float kd_pos_term = 0;
+    float change_in_target_pos = 0;
+    float target_pos = 0;
+    float target_pos_old = 0;
+    
+    float runningSumReplaceVal = 0;
+    float newVelocity = 0;
+    
+    float velocitySum = 0;
+    memset(velocitySamples,0,sizeof(velocitySamples));
     pc.printf("Starting PID Thread..\r\n");
     while(1) {
         gpioMonitorA = 1;
@@ -257,10 +259,10 @@
 
 
         //Preset Knob Values for Green Balance Bot
-        //knob1 = 42.157;
-        //knob2 = 41.135;
-        //knob3 = 8.82;
-        //knob4 = 20.68;
+        //knob1 = 1.132;
+        //knob2 = 47.284;
+        //knob3 = 18.46;
+        //knob4 = 17.07;
 
         //Set Gainz with knobs
         Kp1 = ((float)knob1) / 1000.0;
@@ -276,22 +278,28 @@
         dt = (timer_value - timer_old);
         timer_old = timer_value;
         angle_old = angle;
-        
-        //loopcounter
-        loop_counter++;
 
         // STANDING: Motor Control Enabled
         //******************** PID Control BEGIN ********************** //
         if(((angle < 45) && (angle > -45)) && (fallen == false)) {
-
+            
+            loop_counter++;
             //CS Pd Target Angle Contoller Goes Here
 
             //Robot Position
             robot_pos = (pos_M1 + pos_M2) / 2;
             target_pos += throttle/2;
-
-            //Robot Current Velocity
-            velocity = (motor1 + motor2) / 2;
+    
+            //Velocity Computation
+            velocitySamplesCounter++;
+            if(velocitySamplesCounter == VELOCITY_SAMPLES){
+                velocitySamplesCounter = 0;
+            }
+            runningSumReplaceVal = velocitySamples[velocitySamplesCounter]; //value to replace
+            newVelocity = (motor1 + motor2) / 2;           
+            velocitySamples[velocitySamplesCounter] = newVelocity; //replace value with
+            velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
+            velocity = velocitySum/VELOCITY_SAMPLES;
 
             //CS ***************** Velocity Controller *********************
             //float target_velocity = 0;
@@ -305,17 +313,11 @@
 
             //KD Term
             change_in_target_pos = target_pos - target_pos_old;
-            change_in_pos = robot_pos - robot_pos_old2;
             //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);
-
-            //Update values
-            target_pos_old = target_pos;
-            robot_pos_old2 = robot_pos_old1;
-            robot_pos_old1 = robot_pos;
-
+            
             //CS ************ PD Stability CONTROLLER HERE ****************
             error = target_angle - angle;
             kp_term = Kp1 * error;
@@ -328,7 +330,7 @@
             //pc.printf("dAngle:%f\r\n", angle-angle_old1);
 
             //Control Output
-            control_output += kp_term + kd_term; //-100 to 100 
+            control_output += kp_term + kd_term; //-100 to 100
             control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
             motor1 = (int16_t)(control_output + (steering));
             motor2 = (int16_t)(control_output - (steering));
@@ -346,7 +348,7 @@
             //setMotor1Speed(20);
             //setMotor2Speed(40);
             setMotor2Speed(-motor2);
-            robot_pos += (-motor1 + -motor2) / 2;
+            //robot_pos += (-motor1 + -motor2) / 2;
             //pc.printf("m1: %d m2: %d angle: %0.1f, controlout: %f tAngle: %f dt: %f timer: %d \r\n", motor1, motor2, angle, control_output, target_angle, dt, timer_value);
         } else { //[FALLEN}
             //Disable Motors
@@ -355,16 +357,17 @@
             //Set fallen flag
             fallen = true;
         }
-        
-         if(loop_counter >= 5) {
+
+        if(loop_counter >= 20) {
             gpioMonitorC = 1;
             loop_counter = 0;
+            pc.printf("Avg Vel: %f\r\n",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);
             //wait_us(200);
             gpioMonitorC = 0;
-            
+
         }
         gpioMonitorA = 0;
     } //end main loop
@@ -468,8 +471,6 @@
     gpioMonitorB = 0;
     gpioMonitorC = 0;
     gpioMonitorD = 0;
-    
-    loop_counter = 0;
 
     enable = ENABLE; //Enable Motors
 }