Commit 9653a1f3 authored by jxchenlu's avatar jxchenlu
Browse files

modify gyrodometry

parent deb6a1d2
...@@ -89,12 +89,8 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){ ...@@ -89,12 +89,8 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
float right_error = right_vel - mb_state->right_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->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->right_cmd = rc_filter_march(&right_wheel_velocity_pid, right_error);
<<<<<<< HEAD
=======
// mb_state->left_cmd = rc_filter_march(&left_wheel_low_pass,mb_state->left_cmd); // mb_state->left_cmd = rc_filter_march(&left_wheel_low_pass,mb_state->left_cmd);
//mb_state->right_cmd = rc_filter_march(&right_wheel_low_pass, mb_state->right_cmd ); //mb_state->right_cmd = rc_filter_march(&right_wheel_low_pass, mb_state->right_cmd );
>>>>>>> origin/odometry
return 0; return 0;
} }
......
...@@ -19,21 +19,14 @@ ...@@ -19,21 +19,14 @@
// TODO: Add convienient defines to define things like motor and encoder polarity here // TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_MOTOR_POL 1 #define LEFT_MOTOR_POL 1
#define RIGHT_MOTOR_POL -1 #define RIGHT_MOTOR_POL 1
#define LEFT_ENC_POL -1 #define LEFT_ENC_POL -1
#define RIGHT_ENC_POL 1 #define RIGHT_ENC_POL 1
// TODO: Fill in physical propeties of robot // TODO: Fill in physical propeties of robot
<<<<<<< HEAD
#define GEAR_RATIO 78 // gear ratio of motor
#define ENCODER_RES 20 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.08 // diameter of wheel in meters
#define WHEEL_BASE 0.157 // wheel separation distance in meters
=======
#define GEAR_RATIO 78.0 // gear ratio of motor #define GEAR_RATIO 78.0 // gear ratio of motor
#define ENCODER_RES 20.0 // encoder counts per motor shaft revolution #define ENCODER_RES 20.0 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.084 // diameter of wheel in meters #define WHEEL_DIAMETER 0.084 // diameter of wheel in meters
#define WHEEL_BASE 0.158 // wheel separation distance in meters #define WHEEL_BASE 0.158 // wheel separation distance in meters
>>>>>>> origin/odometry
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s) #define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s) #define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
......
...@@ -39,13 +39,14 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){ ...@@ -39,13 +39,14 @@ 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_thesh = 0.125/DT; float del_thresh = 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;
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_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_theta = mb_clamp_radians((del_right - del_left) / WHEEL_BASE);
float del_x = del_s * cos(mb_odometry->theta + del_theta/2); float del_x = del_s * cos(mb_odometry->theta + del_theta/2);
float del_y = del_s * sin(mb_odometry->theta + del_theta/2); float del_y = del_s * sin(mb_odometry->theta + del_theta/2);
mb_odometry->x += del_x; mb_odometry->x += del_x;
...@@ -57,7 +58,7 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){ ...@@ -57,7 +58,7 @@ 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) > del_thesh){mb_odometry->theta += del_theta_gyro * DT;} if (abs(del_go) >= 0.0){mb_odometry->theta += del_theta_gyro * DT;}
else{mb_odometry->theta += del_theta * DT;} else{mb_odometry->theta += del_theta * DT;}
} }
else{// this is after we update the del_x and y else{// this is after we update the del_x and y
......
Supports Markdown
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