Commit 51a3cc73 authored by gxiang's avatar gxiang
Browse files

fixed controller

parent 2131759f
......@@ -19,18 +19,20 @@ 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 kdL = 0.15;
float kiL = 40;
float kdL = 0.2;
float kpR = 3.5;
float kiR = 15;
float kdR = 0.15;
float kiR = 40;
float kdR = 0.2;
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);
rc_filter_enable_saturation(&left_wheel_velocity_pid,-1.0,1.0);
rc_filter_enable_saturation(&right_wheel_velocity_pid,-1.0,1.0);
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_wheel_low_pass,DT,0.08);
rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.08);
rc_filter_first_order_lowpass(& left_set_low_pass,DT,0.08);
rc_filter_first_order_lowpass(&right_set_low_pass,DT,0.08);
return 0;
}
......@@ -80,11 +82,18 @@ int mb_load_controller_config(){
*******************************************************************************/
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int* reset){
float kL = 1.5275;
float bL = 0;
float kR = 1.479313;
float bR = 0;
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
/*if (left_prev != left_vel){
left_prev = left_vel;
......@@ -94,11 +103,19 @@ 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_cmd_calibration = (left_vel - bL)/kL;
float right_cmd_calibration = (right_vel - bR)/kR;
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);
mb_state->left_cmd = mb_state->left_cmd + left_cmd_calibration;
mb_state->right_cmd = mb_state->right_cmd + right_cmd_calibration;
if(*reset == 1)
{
*reset = 0;
......
......@@ -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;
/***********
......
......@@ -15,7 +15,7 @@
#define BEAGLEBONE_BLUE
#define DEFAULT_PWM_FREQ 25000 // period of motor drive pwm
#define LEFT_MOTOR 1 // id of left motor
#define RIGHT_MOTOR 3 // id of right motor
#define RIGHT_MOTOR 3 // id of right motor
// TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_MOTOR_POL 1
......@@ -27,7 +27,7 @@
#define GEAR_RATIO 78.0 // gear ratio of motor
#define ENCODER_RES 20.0 // encoder counts per motor shaft revolution
#define WHEEL_DIAMETER 0.084 // diameter of wheel in meters
#define WHEEL_BASE 0.158 // wheel separation distance in meters
#define WHEEL_BASE 0.159 // wheel separation distance in meters
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
......
......@@ -39,7 +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.3; //0.125/DT;
float del_thresh = 0.1; //0.125/DT;
boolean Gyro = T;
float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
......@@ -54,8 +54,11 @@ 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 del_go = del_theta_gyro - del_theta;
float theta_gyro_now = mb_state->tb_angles[2];//mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0));
float del_theta_gyro = theta_gyro_now- mb_state->last_yaw;
float del_go = abs(del_theta_gyro - del_theta);
if (Gyro == T){
if (abs(del_go) >= del_thresh){mb_odometry->theta += del_theta_gyro;}
else{mb_odometry->theta += del_theta;}
......
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