Diff: main.cpp

Revision:

19:19d72dc64b43

Parent:

18:ee252b8f7430

Child:

20:a7cba632d0b1

--- a/main.cpp	Wed Mar 22 21:01:07 2017 +0000
+++ b/main.cpp	Wed Mar 22 21:41:07 2017 +0000
@@ -3,16 +3,11 @@
     Authors: Arvind Ramesh and Carter Sharer
 */
 
-//Added communication protocol v1 (no type selection)
-//Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67
-//Knob1 13.418 Knob2 28.848 Knob3 9.45  Knob4 42.29 Good
-//Knob1 15.373 Knob2 28.261 Knob3 10.42 Knob4 40.97 Smoother
-
 /******************************* README USAGE *******************************
 * This robot must be powered on while it is laying down flat on a still table
 * This allows the robot to calibrate the IMU (~5 seconds)
 * The motors are DISABLED when the robot tilts more then +-45 degrees from
-* vertical.  To ENABLE the motors you must lift the robot to < +- 45 degres and
+* vertical.  To ENABLE the motors you must lift the robot to 0 degres and
 * press the joystick button.
 * To reset the motor positions you must press the josystick button anytime.
 ******************************************************************************/
@@ -35,29 +30,15 @@
 #include "stepper_motors.h"
 #include "MRF24J40.h"
 #include "communication.h"
-#include "waypoint.h"
 
 //Angle Offset is used to set the natural balance point of your robot.
 //You should adjust this offset so that your robots balance point is near 0
-#define ANGLE_OFFSET 107
-#define THETA_OFFSET 0//165
+#define ANGLE_OFFSET 107 //THIS NEEDS TO BE CHANGED FOR YOUR ROBOT
+#define THETA_OFFSET 0
 
 //Set Channel to (Group Number - 1). Possible Channels are 0-15
 #define MRF_CHANNEL 2
 
-//For RF Communication
-#define JSTICK_H 8
-#define JSTICK_V 9
-#define SPACE 10
-#define KNOB1 11
-#define KNOB2 12
-#define KNOB3 13
-#define KNOB4 14
-#define ANGLE 15
-#define BUTTON 16
-#define JSTICK_OFFSET 100
-#define TX_BUFFER_LEN 18
-#define TX_ANGLE_OFFSET 100
 //Knobs
 #define POT1 p17
 #define POT2 p18
@@ -70,16 +51,11 @@
 //PID
 #define MAX_THROTTLE 580
 #define MAX_STEERING 150
-#define MAX_TARGET_ANGLE 12
-#define KP 0.19
-#define KD 28
-#define KP_THROTTLE 0.01 //0.07    
-#define KI_THROTTLE 0//0.04
-#define ITERM_MAX_ERROR 25   // Iterm windup constants for PI control //40
-#define ITERM_MAX 8000       // 5000
+#define MAX_TARGET_ANGLE 12     
 
-#define THROTTLE_DAMPNER 100
-#define STREEING_DAMPNER 10000
+//Joystick Control
+#define THROTTLE_DAMPNER 30
+#define STREEING_DAMPNER 10
 #define VELOCITY_SAMPLES 10
 
 //*********** Local Function Definations BEGIN **************//
@@ -107,8 +83,6 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-//Blue = IMU Intr
-
 //Used to set angle upon startup
 bool initilizeAngle;
 bool initilizeTheta;
@@ -127,7 +101,6 @@
     pc.printf("Starting IMU Thread..\r\n");
     while (1) {
         osSignalWait(IMU_UPDATE_SIGNAL,osWaitForever);
-        pin_30 = 1;
         if(mpuInterrupt) {
             mpuInterrupt = false;
             led3 = !led3;
@@ -200,7 +173,6 @@
             else{
                 pc.printf("\t\t\t IMU Error. MPU Status: %d!!\r\n",mpuIntStatus);
             }
-            pin_30 = 0;
             /********************* All IMU Handling DO NOT MODIFY *****************/
         }//End of if(mpuInterrupt) loop
         osSignalClear(imu_update_thread_ID,IMU_UPDATE_SIGNAL);
@@ -227,7 +199,6 @@
     float kd_term = 0;
     float throttle = 0;
     float steering = 0;
-    float steering_output = 0;
     float control_output = 0;
     int robot_pos = 0;
     //int loop_counter = 0;
@@ -245,15 +216,9 @@
     // -- For Velocity Controller --- //
     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;
-    float angle_old = 0;
     float velocitySum = 0;
-    float theta_error = 0;
-    float target_theta = 0;
     memset(velocitySamples,0,sizeof(velocitySamples));
     
     pc.printf("Starting PID Thread..\r\n");
@@ -268,9 +233,6 @@
             motor1 = 0;
             motor2 = 0;
             control_output = 0;
-            totalTheta = 0;
-            target_theta = 0;
-            steering_output = 0;
             fallen = false;
         }
         
@@ -292,7 +254,7 @@
         //Calculate the delta time
         dt = (timer_value - timer_old);
         timer_old = timer_value;
-        angle_old = angle;
+        //angle_old = angle;
         
         // STANDING: Motor Control Enabled
         //******************** PID Control BEGIN ********************** //
@@ -300,7 +262,9 @@
 
             //Robot Position
             robot_pos = (pos_M1 + pos_M2) / 2;
-            target_pos += throttle/2;
+            
+            //Target Position Incremented with Joystick
+            target_pos += throttle;
     
             //*************** Velocity Computation **********************
             velocitySamplesCounter++;
@@ -313,67 +277,55 @@
             velocitySum = velocitySum - runningSumReplaceVal + newVelocity;
             velocity = velocitySum/VELOCITY_SAMPLES;
             
-            // ***************** Angular Controller *********************
-            float Kp1A = 0;//Kp1/100;
-            target_theta += steering;
-            theta_error =  totalTheta - target_theta;
-            steering_output = -Kp1A * theta_error;
+            // ***************** Position Controller *********************
+            //Inputs: robot_position, target_pos 
+            //Error: distance from target position 
+            //Output: target_angle
             
-            // ***************** Position Controller *********************
             float Kp1P = Kp2/1000;
             float Kd1P = Kd2/100;
-            //Kp1P = ;//5.73/100;
-            //target_pos = 0;
+            
+            //Calculate the Position Error using robot_pos and target_pos
             pos_error = robot_pos - target_pos; //robot_pos - target_pos;
 
-            //KP Term
+            //Calculate the Proportional Term (P Term) Using The Error
             kp_pos_term = Kp1P * pos_error;
 
-            //KD Term
+            //Calculate the Derivative Term (D Term) Using The Error
             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 = ((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;
+         
+            //************ PD Stability CONTROLLER HERE ****************
+            //Inputs: angle, target_angle
+            //Error: distance from target_angle
+            //Output: control_output (motor speed) 
             
-            //***************** Velocity Controller *********************
-            /*
-            float Kp2V = Kp2/100;
-            float Kd2V = Kd2;
-            Kp2V = 31.81/100;
-            Kd2V = 17.5;
-            //target_velocity = throttle;
-            velocity_error = target_velocity - velocity;
-            change_in_velocity = velocity - velocitySamples[(velocitySamplesCounter + 2)%VELOCITY_SAMPLES];
-            d_velocity = (Kd2V * change_in_velocity)/dt;
+            //Scale the Knob Values;
+            float Kp1S = Kp1/100; 
+            float Kd1S = Kd1*100; 
             
-            target_angle = (-velocity_error * Kp2V)+ d_velocity;    
-            */
+            //Calculate the Angle Error Using target_angle and angle 
+            error = target_angle - angle;
             
-            //************ PD Stability CONTROLLER HERE ****************
-            float Kp1S = Kp1/10; //7.373/1000.0;
-            float Kd1S = Kd1*100; //50.986/1.0;
-           // Kp1S = 3.48/1000;
-            //Kd1S = 44.5;
-            
-            error = target_angle - angle;
+            //Calculate the Proportional Term (P term) Using the Error
             kp_term = Kp1S * error;
 
+            //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));
 
-            // Control Output
+            //Calculate the Control Output
             control_output += kp_term + kd_term; //-100 to 100
             control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
             
-            //pc.printf("dAngle:%f\r\n", angle-angle_old1);
-
             //*************** Set Motor Speed *************************
-            motor1 = (int16_t)(control_output + (steering_output));
-            motor2 = (int16_t)(control_output - (steering_output));
+            motor1 = (int16_t)(control_output + (steering));
+            motor2 = (int16_t)(control_output - (steering));
             motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
             motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
             setMotor1Speed(-motor1);