Commit 837959cc authored by gxiang's avatar gxiang
Browse files

add filter for set points, increase I, changed Odometry

parent 2131759f
......@@ -19,10 +19,10 @@ 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 kiL = 20;
float kdL = 0.15;
float kpR = 3.5;
float kiR = 15;
float kiR = 25;
float kdR = 0.15;
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);
......@@ -31,6 +31,8 @@ int mb_initialize_controller(){
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_set_low_pass,DT,0.1);
rc_filter_first_order_lowpass(&right_set_low_pass,DT,0.1);
return 0;
}
......@@ -83,6 +85,7 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints,int*
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
......@@ -94,8 +97,11 @@ 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_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);
......
......@@ -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;
/***********
......
......@@ -33,13 +33,13 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
* publish new odometry to lcm ODOMETRY_CHANNEL
*
*******************************************************************************/
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state,float* prev){
// mb_odometry is the input of previenc2metersous veh's position and orientation
// Reference: https://www.hmc.edu/lair/ARW/ARW-Lecture01-Odometry.pdf P28
//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 = 100; //0.125/DT;
boolean Gyro = T;
float del_left = mb_state->left_encoder_delta * 1.0 * enc2meters;
......@@ -54,7 +54,10 @@ 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 theta_gyro_now = mb_clamp_radians(mb_state->gyro[2] * (PI / 180.0));
float del_theta_gyro = theta_gyro_now - *prev;
*prev = theta_gyro_now;
float del_go = del_theta_gyro - del_theta;
if (Gyro == T){
if (abs(del_go) >= del_thresh){mb_odometry->theta += del_theta_gyro;}
......
......@@ -8,7 +8,7 @@
#include "mb_structs.h"
void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta);
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state);
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state,float* prev);
float mb_clamp_radians(float angle);
float mb_angle_diff_radians(float angle1, float angle2);
......
......@@ -15,7 +15,7 @@
*
*******************************************************************************/
int main(){
prev = 0;
rc_led_set(RC_LED_GREEN, LED_OFF);
rc_led_set(RC_LED_RED, LED_ON);
//set cpu freq to max performance
......@@ -233,7 +233,7 @@ void mobilebot_controller(){
read_mb_sensors();
// 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_update_odometry(&mb_odometry, &mb_state,&prev);
mb_controller_update(&mb_state,&mb_setpoints,&reset);
publish_mb_msgs();
rc_motor_set(LEFT_MOTOR,LEFT_MOTOR_POL*mb_state.left_cmd);
......
......@@ -52,6 +52,7 @@ mb_state_t mb_state;
mb_setpoints_t mb_setpoints;
mb_odometry_t mb_odometry;
int reset;
float prev;
int64_t now;
int64_t time_offset;
int time_offset_initialized;
......
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