Diff: main.cpp

Revision:

8:8389c0a9339e

Parent:

6:62cdb7482b50

Child:

9:fb671fa0c0be

--- a/main.cpp	Sat Dec 17 23:26:39 2016 +0000
+++ b/main.cpp	Sat Dec 17 23:58:58 2016 +0000
@@ -2,6 +2,7 @@
 //Author: Carter Sharer
 //Date: 10/13/2016
 //Added communication protocol v1 (no type selection)
+//Jstick_v: 0 Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67 Button: 0
 
 //BroBot Begin
 #include "pin_assignments.h"
@@ -158,7 +159,8 @@
 float change_in_target_pos;
 float target_pos, target_pos_old;
 float change_in_pos;
-float robot_pos_old, robot_pos_old1, robot_pos_old2;
+float robot_pos_old1, robot_pos_old2;
+float velocity;
 
 bool fallen = true;
 int main()
@@ -238,7 +240,9 @@
                 robot_pos = (pos_M1 + pos_M2) / 2;
                 target_pos += throttle/2;
                 
-                //Position error
+                velocity = motor1 + motor2 / 2;                
+                
+                //CS ***************** Position error *************************
                 pos_error = robot_pos - target_pos; //robot_pos - target_pos;
                 
                 //KP Term  
@@ -247,23 +251,25 @@
                 //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*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_old;
+                robot_pos_old1 = robot_pos;
                 
-                //CS PD Stability CONTROLLER HERE
+                //CS ************ PD Stability CONTROLLER HERE ****************
                 error = target_angle - angle;
                 kp_term = Kp1 * 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 = ((Kd1 * change_in_target_angle) - (Kd1*velocity));
+                
                 //pc.printf("dAngle:%f\r\n", angle-angle_old1);
     
                 //Control Output
@@ -296,7 +302,7 @@
             //Fast Loop 
             if(loop_counter >= 5) {
                 loop_counter = 0;
-                pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f  Kp2: %0.2f Kd2: %0.3f  tang: %0.2f dt:%d pos_M1:%d pos_M2:%d rob_pos: %d\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, pos_M1, pos_M2, robot_pos);
+                pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f  Kp2: %0.2f Kd2: %0.3f  tang: %0.2f dt:%d rob_pos: %d V: %f\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, robot_pos, velocity);
                 //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);
             }