fixed controller

common/mb_controller.c

@@ -19,18 +19,20 @@ int mb_initialize_controller(){
     right_wheel_low_pass = rc_filter_empty();
     //put variable for left pid and right pid
     float kpL = 3;
-    float kiL = 15;
-    float kdL = 0.15;
+    float kiL = 40;
+    float kdL = 0.2;
      float kpR = 3.5;
-    float kiR = 15;
-    float kdR = 0.15;
+    float kiR = 40;
+    float kdR = 0.2;
     rc_filter_pid(&left_wheel_velocity_pid,kpL,kiL,kdL,0.1,DT);
     rc_filter_pid(&right_wheel_velocity_pid,kpR,kiR,kdR,0.1,DT);
     rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
     rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
     
-    rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.06);
-    rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.06);
+    rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.08);
+    rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.08);
+    rc_filter_first_order_lowpass(& left_set_low_pass,DT,0.08);
+    rc_filter_first_order_lowpass(&right_set_low_pass,DT,0.08);
     return 0;
 }
 
@@ -80,11 +82,18 @@ int mb_load_controller_config(){
 *******************************************************************************/
 
 int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int* reset){
+float kL = 1.5275;
+float bL = 0;
+float kR = 1.479313;
+float bR = 0; 
     
     mb_state->left_velocity = rc_filter_march(&left_wheel_low_pass,mb_state->left_velocity);
     mb_state->right_velocity = rc_filter_march(&right_wheel_low_pass, mb_state->right_velocity);
+    
+   
     float left_vel = (2*mb_setpoints->fwd_velocity  - mb_setpoints->turn_velocity*WHEEL_BASE)/2;
     float right_vel = (2*mb_setpoints->fwd_velocity + mb_setpoints->turn_velocity*WHEEL_BASE)/2;
+    
     //reset filter with a new point
     /*if (left_prev != left_vel){
         left_prev = left_vel;
@@ -94,11 +103,19 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
         right_prev = right_vel;
         rc_filter_reset(&right_wheel_velocity_pid);
     }*/
+    left_vel = rc_filter_march(& left_set_low_pass, left_vel);
+    right_vel = rc_filter_march(& right_set_low_pass, right_vel);
+     float left_cmd_calibration = (left_vel - bL)/kL; 
+    float right_cmd_calibration =  (right_vel - bR)/kR;
     float left_error = left_vel - mb_state->left_velocity;
     float right_error = right_vel -  mb_state->right_velocity;
+    
     mb_state->left_cmd = rc_filter_march(&left_wheel_velocity_pid, left_error);
     mb_state->right_cmd = rc_filter_march(&right_wheel_velocity_pid, right_error);
 
+    mb_state->left_cmd  = mb_state->left_cmd  + left_cmd_calibration;
+    mb_state->right_cmd  = mb_state->right_cmd  + right_cmd_calibration;
+    
      if(*reset == 1)
     {
         *reset = 0;

common/mb_controller.h

@@ -20,6 +20,8 @@ rc_filter_t left_wheel_velocity_pid;
 rc_filter_t right_wheel_velocity_pid;
 rc_filter_t left_wheel_low_pass;
 rc_filter_t right_wheel_low_pass;
+rc_filter_t left_set_low_pass;
+rc_filter_t right_set_low_pass;
 int left_prev;
 int right_prev;
 /***********

common/mb_defs.h

@@ -15,7 +15,7 @@
 #define BEAGLEBONE_BLUE
 #define DEFAULT_PWM_FREQ        25000 // period of motor drive pwm
 #define LEFT_MOTOR              1     // id of left motor
-#define RIGHT_MOTOR             3 // id of right motor
+#define RIGHT_MOTOR             3     // id of right motor
 
 // TODO: Add convienient defines to define things like motor and encoder polarity here
 #define LEFT_MOTOR_POL 1
@@ -27,7 +27,7 @@
 #define GEAR_RATIO              78.0  // gear ratio of motor
 #define ENCODER_RES             20.0  // encoder counts per motor shaft revolution
 #define WHEEL_DIAMETER          0.084 // diameter of wheel in meters
-#define WHEEL_BASE              0.158  // wheel separation distance in meters
+#define WHEEL_BASE              0.159  // wheel separation distance in meters
 
 #define MAX_FWD_VEL             0.8   // maximum forwad speed (m/s)
 #define MAX_TURN_VEL            2.5   // maximum turning speed (rad/s)

common/mb_odometry.c

@@ -39,7 +39,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
     //del_thresh Needs to be tuned
     //IF DON'T WANT GYRO, SET IT AS F!
     float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
-    float del_thresh = 0.3; //0.125/DT; 
+    float del_thresh = 0.1; //0.125/DT; 
     boolean Gyro = T;
     
     float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
@@ -54,8 +54,11 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
 
     // Gyro: we only need to use yaw
     // Since each time step is small, no need to integrate
-    float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT);
-    float del_go = del_theta_gyro - del_theta;
+    float theta_gyro_now = mb_state->tb_angles[2];//mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0));
+    float del_theta_gyro = theta_gyro_now- mb_state->last_yaw;
+   
+    
+    float del_go = abs(del_theta_gyro - del_theta);
     if (Gyro == T){
         if (abs(del_go) >= del_thresh){mb_odometry->theta += del_theta_gyro;}
         else{mb_odometry->theta += del_theta;}