add filter for set points, increase I, changed Odometry

common/mb_controller.c

@@ -19,10 +19,10 @@ 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 kiL = 20;
     float kdL = 0.15;
      float kpR = 3.5;
-    float kiR = 15;
+    float kiR = 25;
     float kdR = 0.15;
     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);
@@ -31,6 +31,8 @@ int mb_initialize_controller(){
     
     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_set_low_pass,DT,0.1);
+    rc_filter_first_order_lowpass(&right_set_low_pass,DT,0.1);
     return 0;
 }
 
@@ -83,6 +85,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
     
     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
@@ -94,8 +97,11 @@ 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_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);
 

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_odometry.c

@@ -33,13 +33,13 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
 *       publish new odometry to lcm ODOMETRY_CHANNEL
 *
 *******************************************************************************/
-void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
+void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state,float* prev){
     // mb_odometry is the input of previenc2metersous veh's position and orientation
     // Reference: https://www.hmc.edu/lair/ARW/ARW-Lecture01-Odometry.pdf  P28
     //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 = 100; //0.125/DT; 
     boolean Gyro = T;
     
     float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
@@ -54,7 +54,10 @@ 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 theta_gyro_now = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0));
+    float del_theta_gyro = theta_gyro_now - *prev;
+    *prev = theta_gyro_now;
+    
     float del_go = del_theta_gyro - del_theta;
     if (Gyro == T){
         if (abs(del_go) >= del_thresh){mb_odometry->theta += del_theta_gyro;}

common/mb_odometry.h

@@ -8,7 +8,7 @@
 #include "mb_structs.h"
 
 void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta);
-void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state);
+void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state,float* prev);
 float mb_clamp_radians(float angle);
 float mb_angle_diff_radians(float angle1, float angle2);
 

mobilebot/mobilebot.c

@@ -15,7 +15,7 @@
 *
 *******************************************************************************/
 int main(){
-    
+    prev = 0;
     rc_led_set(RC_LED_GREEN, LED_OFF);
     rc_led_set(RC_LED_RED, LED_ON);
 	//set cpu freq to max performance
@@ -233,7 +233,7 @@ void mobilebot_controller(){
     read_mb_sensors();
     // calculate odometry after update reading and defining sensors readings during each call-back
     // mb_odometry x,y,theta will be updated in mb_odoemtry.c
-    mb_update_odometry(&mb_odometry, &mb_state) ;
+    mb_update_odometry(&mb_odometry, &mb_state,&prev);
     mb_controller_update(&mb_state,&mb_setpoints,&reset);
     publish_mb_msgs();
     rc_motor_set(LEFT_MOTOR,LEFT_MOTOR_POL*mb_state.left_cmd);

mobilebot/mobilebot.h

@@ -52,6 +52,7 @@ mb_state_t mb_state;
 mb_setpoints_t mb_setpoints;
 mb_odometry_t mb_odometry;
 int reset;
+float prev;
 int64_t now;
 int64_t time_offset;
 int time_offset_initialized;