Commit 65cecb06 authored by jxchenlu's avatar jxchenlu
Browse files

modify threshold

parent 9653a1f3
...@@ -39,8 +39,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){ ...@@ -39,8 +39,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
//del_thresh Needs to be tuned //del_thresh Needs to be tuned
//IF DON'T WANT GYRO, SET IT AS F! //IF DON'T WANT GYRO, SET IT AS F!
float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES); float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
float del_thresh = 0.125/DT; float del_thresh = 0.3; //0.125/DT;
printf("del_thresh", del_thresh);
boolean Gyro = T; boolean Gyro = T;
float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters; float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
...@@ -58,8 +57,8 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){ ...@@ -58,8 +57,8 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT); float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT);
float del_go = del_theta_gyro - del_theta; float del_go = del_theta_gyro - del_theta;
if (Gyro == T){ if (Gyro == T){
if (abs(del_go) >= 0.0){mb_odometry->theta += del_theta_gyro * DT;} if (abs(del_go) >= del_thresh){mb_odometry->theta += del_theta_gyro;}
else{mb_odometry->theta += del_theta * DT;} else{mb_odometry->theta += del_theta;}
} }
else{// this is after we update the del_x and y else{// this is after we update the del_x and y
mb_odometry->theta += del_theta;} mb_odometry->theta += del_theta;}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment