modify threshold

common/mb_odometry.c

@@ -39,8 +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.125/DT; 
-    printf("del_thresh", del_thresh);
+    float del_thresh = 0.3; //0.125/DT; 
     boolean Gyro = T;
 
     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){
     float del_theta_gyro = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0) * DT);
     float del_go = del_theta_gyro - del_theta;
     if (Gyro == T){
-        if (abs(del_go) >= 0.0){mb_odometry->theta += del_theta_gyro * DT;}
-        else{mb_odometry->theta += del_theta * DT;}
+        if (abs(del_go) >= del_thresh){mb_odometry->theta += del_theta_gyro;}
+        else{mb_odometry->theta += del_theta;}
     }
     else{// this is after we update the del_x and y 
         mb_odometry->theta += del_theta;}