Commit 8f2d327a authored by gxiang's avatar gxiang
Browse files

fix commit error

parents ee3be1b6 65cecb06
......@@ -100,6 +100,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
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);
if(*reset == 1)
{
*reset = 0;
......@@ -109,6 +110,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
rc_filter_reset(&right_wheel_low_pass);
}
return 0;
}
......
......@@ -39,11 +39,11 @@ 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_thesh = 0.125/DT;
float del_thresh = 0.3; //0.125/DT;
boolean Gyro = T;
float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
float del_right =mb_state->right_encoder_delta * 1.0 * enc2meters;
float del_right = mb_state->right_encoder_delta * 1.0 * enc2meters;
float del_s = (del_right + del_left) / 2;//float del_s = (del_left + del_right) / 2;
float del_theta = mb_clamp_radians((del_right - del_left) / WHEEL_BASE);
float del_x = del_s * cos(mb_odometry->theta + del_theta/2);
......@@ -57,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) > del_thesh){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;}
......
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