Commit ee3be1b6 authored by gxiang's avatar gxiang
Browse files

debug controler

parent deb6a1d2
......@@ -18,20 +18,21 @@ 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 kpL = 2;
float kiL = 7;
float kdL = 0.2;
float kpR = 2;
float kiR = 7;
float kdR = 0.2;
float kpL = 4;
float kiL = 8;
float kdL = 0.05;
float kpR = 4;
float kiR = 8;
float kdR = 0.05;
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.1);
rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.1);
rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.06);
rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.06);
left_prev = 0;
right_prev = 0;
return 0;
}
......@@ -80,21 +81,34 @@ int mb_load_controller_config(){
*
*******************************************************************************/
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int* reset){
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;
rc_filter_reset(&left_wheel_velocity_pid);
}
if (right_prev != right_vel){
right_prev = right_vel;
rc_filter_reset(&right_wheel_velocity_pid);
}*/
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);
<<<<<<< HEAD
if(*reset == 1)
{
*reset = 0;
rc_filter_reset(&right_wheel_velocity_pid);
rc_filter_reset(&left_wheel_velocity_pid);
rc_filter_reset(&left_wheel_low_pass);
rc_filter_reset(&right_wheel_low_pass);
}
=======
// 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 );
>>>>>>> origin/odometry
return 0;
}
......
......@@ -7,7 +7,7 @@
int mb_initialize_controller();
int mb_load_controller_config();
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints);
int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int* reset);
int mb_destroy_controller();
/************
......@@ -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;
int left_prev;
int right_prev;
/***********
* For each PID filter you want to load from settings
* add a pid_parameter_t or filter_parameter_t
......
......@@ -23,17 +23,12 @@
#define LEFT_ENC_POL -1
#define RIGHT_ENC_POL 1
// 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 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
>>>>>>> origin/odometry
#define MAX_FWD_VEL 0.8 // maximum forwad speed (m/s)
#define MAX_TURN_VEL 2.5 // maximum turning speed (rad/s)
......
......@@ -41,11 +41,11 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
float del_thesh = 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_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_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);
float del_y = del_s * sin(mb_odometry->theta + del_theta/2);
mb_odometry->x += del_x;
......
......@@ -15,7 +15,7 @@
*
*******************************************************************************/
int main(){
rc_led_set(RC_LED_GREEN, LED_OFF);
rc_led_set(RC_LED_RED, LED_ON);
//set cpu freq to max performance
......@@ -64,6 +64,7 @@ int main(){
//attach controller function to IMU interrupt
printf("initializing controller...\n");
reset = 0;
mb_initialize_controller();
printf("initializing motors...\n");
......@@ -192,8 +193,10 @@ void publish_mb_msgs(){
wheel_ctrl_msg.utime = now;
wheel_ctrl_msg.left_motor_pwm_cmd = mb_state.left_cmd;
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;
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;
wheel_ctrl_msg.left_motor_vel_cmd = left_vel;
wheel_ctrl_msg.right_motor_vel_cmd = right_vel;
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;
......@@ -231,7 +234,7 @@ void mobilebot_controller(){
// calculate odometry after update reading and defining sensors readings during each call-back
// mb_odometry x,y,theta will be updated in mb_odoemtry.c
mb_update_odometry(&mb_odometry, &mb_state) ;
mb_controller_update(&mb_state,&mb_setpoints);
mb_controller_update(&mb_state,&mb_setpoints,&reset);
publish_mb_msgs();
rc_motor_set(LEFT_MOTOR,LEFT_MOTOR_POL*mb_state.left_cmd);
rc_motor_set(RIGHT_MOTOR,RIGHT_MOTOR_POL*mb_state.right_cmd);
......@@ -275,8 +278,7 @@ void motor_command_handler(const lcm_recv_buf_t *rbuf, const char *channel,
const mbot_motor_command_t *msg, void *user){
mb_setpoints.fwd_velocity = msg->trans_v;
mb_setpoints.turn_velocity = msg->angular_v;
reset = 1;
}
......
......@@ -51,6 +51,7 @@ pthread_mutex_t state_mutex;
mb_state_t mb_state;
mb_setpoints_t mb_setpoints;
mb_odometry_t mb_odometry;
int reset;
int64_t now;
int64_t time_offset;
int time_offset_initialized;
......
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