Commit 63a15082 authored by gxiang's avatar gxiang
Browse files

added controller

parent 138d20dd
......@@ -17,11 +17,14 @@ int mb_initialize_controller(){
left_wheel_low_pass = rc_filter_empty();
right_wheel_low_pass = rc_filter_empty();
//put variable for left pid and right pid
float kp = 0.65;
float ki = 0.02;
float kd = 0.15;
rc_filter_pid(&left_wheel_velocity_pid,kp,ki,kd,0.1,DT);
rc_filter_pid(&right_wheel_velocity_pid,kp,ki,kd,0.1,DT);
float kpL = 2;
float kiL = 7;
float kdL = 0.2;
float kpR = 2;
float kiR = 7;
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);
......@@ -76,14 +79,15 @@ int mb_load_controller_config(){
*******************************************************************************/
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
//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_error = mb_setpoints->fwd_velocity - LEFT_MOTOR_POL*mb_state->left_velocity;
float right_error = mb_setpoints->fwd_velocity - RIGHT_MOTOR_POL*mb_state->right_velocity;
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;
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 = 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 );
return 0;
}
......
......@@ -20,13 +20,13 @@
// TODO: Add convienient defines to define things like motor and encoder polarity here
#define LEFT_MOTOR_POL 1
#define RIGHT_MOTOR_POL -1
#define LEFT_ENC_POL 1
#define RIGHT_ENC_POL -1
#define LEFT_ENC_POL -1
#define RIGHT_ENC_POL 1
// TODO: Fill in physical propeties of robot
#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.0 // wheel separation distance in meters
#define WHEEL_BASE 0.157 // 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)
......
......@@ -145,14 +145,14 @@ void read_mb_sensors(){
}
// Read encoders
mb_state.left_encoder_delta = rc_encoder_read(LEFT_MOTOR);
mb_state.right_encoder_delta = rc_encoder_read(RIGHT_MOTOR);
mb_state.left_encoder_delta = LEFT_ENC_POL*rc_encoder_read(LEFT_MOTOR);
mb_state.right_encoder_delta = RIGHT_ENC_POL*rc_encoder_read(RIGHT_MOTOR);
mb_state.left_encoder_total += mb_state.left_encoder_delta;
mb_state.right_encoder_total += mb_state.right_encoder_delta;
rc_encoder_write(LEFT_MOTOR,0);
rc_encoder_write(RIGHT_MOTOR,0);
float enc2meters = (WHEEL_DIAMETER*3.1415) / (GEAR_RATIO*ENCODER_RES);
mb_state.left_velocity = enc2meters * mb_state.left_encoder_delta / DT;
mb_state.left_velocity =enc2meters * mb_state.left_encoder_delta / DT;
mb_state.right_velocity = enc2meters * mb_state.right_encoder_delta / DT;
//unlock state mutex
pthread_mutex_unlock(&state_mutex);
......@@ -193,8 +193,8 @@ void publish_mb_msgs(){
wheel_ctrl_msg.right_motor_pwm_cmd = mb_state.right_cmd;
wheel_ctrl_msg.left_motor_vel_cmd =mb_setpoints.fwd_velocity;
wheel_ctrl_msg.right_motor_vel_cmd =mb_setpoints.fwd_velocity;
wheel_ctrl_msg.left_motor_vel = -1*mb_state.left_velocity;
wheel_ctrl_msg.right_motor_vel = mb_state.right_velocity;
wheel_ctrl_msg.left_motor_vel = LEFT_MOTOR_POL*mb_state.left_velocity;
wheel_ctrl_msg.right_motor_vel = RIGHT_ENC_POL*mb_state.right_velocity;
//TODO: Create Odometry LCM message
......
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