Diff: main.cpp

Revision:

20:a7cba632d0b1

Parent:

19:19d72dc64b43

--- a/main.cpp	Wed Mar 22 21:41:07 2017 +0000
+++ b/main.cpp	Wed Mar 22 21:59:10 2017 +0000
@@ -200,6 +200,7 @@
     float throttle = 0;
     float steering = 0;
     float control_output = 0;
+    float pos_control_output = 0;
     int robot_pos = 0;
     //int loop_counter = 0;
     int velocitySamplesCounter = 0;
@@ -277,6 +278,13 @@
             velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
             velocity = velocitySum/VELOCITY_SAMPLES;
             
+            /*##################################################################
+            ########## ALL CODING FOR THIS LAB WILL BE COMPLETED BELOW #########
+            ######### THERE ARE 2 PARTS, PART 1 MUST BE COMPLETED FIRST ########
+            ####### IT IS NOT RECOMENDED TO MODIFY CODE OUTSIDE THIS LOOP ######
+            ##################################################################*/
+            
+            //PART 2
             // ***************** Position Controller *********************
             //Inputs: robot_position, target_pos 
             //Error: distance from target position 
@@ -285,19 +293,22 @@
             float Kp1P = Kp2/1000;
             float Kd1P = Kd2/100;
             
-            //Calculate the Position Error using robot_pos and target_pos
-            pos_error = robot_pos - target_pos; //robot_pos - target_pos;
+            //(2.0) Calculate the Position Error using robot_pos and target_pos
+            pos_error = 0;
 
-            //Calculate the Proportional Term (P Term) Using The Error
-            kp_pos_term = Kp1P * pos_error;
+            //(2.1) Calculate the Proportional Term (P Term) Using The Error
+            kp_pos_term = 0;
 
-            //Calculate the Derivative Term (D Term) Using The Error
-            change_in_target_pos = target_pos - target_pos_old;
-            kd_pos_term = ((Kd1P * change_in_target_pos) + (Kd1P*velocity));
-            target_velocity = kp_pos_term + -kd_pos_term;
-            target_velocity = CAP(target_velocity, 100);
-            target_angle = -target_velocity;
+            //(2.2) Calculate the Derivative Term (D Term) Using The Error
+            change_in_target_pos = 0;
+            kd_pos_term = 0; //Use change_in_target_pos and velocity
+            pos_control_output = 0;  //Add the two terms together (P + D)
+            pos_control_output = CAP(pos_control_output, 100); //Cap the values
+            
+            //This Control Output Will Feed Into The Stability Controller
+            target_angle = -pos_control_output;
          
+            //PART 1 (Do this before the Position Controller)
             //************ PD Stability CONTROLLER HERE ****************
             //Inputs: angle, target_angle
             //Error: distance from target_angle
@@ -307,20 +318,19 @@
             float Kp1S = Kp1/100; 
             float Kd1S = Kd1*100; 
             
-            //Calculate the Angle Error Using target_angle and angle 
-            error = target_angle - angle;
+            //(1.0) Calculate the Angle Error Using target_angle and angle 
+            error = 0;
             
-            //Calculate the Proportional Term (P term) Using the Error
-            kp_term = Kp1S * error;
+            //(1.1) Calculate the Proportional Term (P term) Using the Error
+            kp_term = 0;
 
-            //Calculate the Derivatve Term (D term)
-            change_in_target_angle = target_angle - target_angle_old; //add
-            change_in_angle = angle - angle_old2; //add
-            kd_term = ((Kd1S * change_in_target_angle) - (Kd1S*change_in_angle)) / dt;
-            //kd_term = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
+            //(1.2) Calculate the Derivatve Term (D term)
+            change_in_target_angle = 0;
+            change_in_angle = 0; //Look 2 samples back (use angle_old2)
+            kd_term = 0; 
 
-            //Calculate the Control Output
-            control_output += kp_term + kd_term; //-100 to 100
+            //(1.3) Calculate the Control Output
+            control_output += 0; //Add the P and D terms together here
             control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
             
             //*************** Set Motor Speed *************************