Commit deb6a1d2 authored by gxiang's avatar gxiang
Browse files

merge

parents 63a15082 9bb053ff
......@@ -12,6 +12,7 @@
int mb_initialize_controller(){
//mb_load_controller_config();
left_wheel_velocity_pid = rc_filter_empty();
right_wheel_velocity_pid = rc_filter_empty();
left_wheel_low_pass = rc_filter_empty();
......@@ -30,6 +31,7 @@ int mb_initialize_controller(){
rc_filter_first_order_lowpass(&left_wheel_low_pass,DT,0.1);
rc_filter_first_order_lowpass(&right_wheel_low_pass,DT,0.1);
return 0;
}
......@@ -87,7 +89,12 @@ int mb_controller_update(mb_state_t* mb_state, mb_setpoints_t* mb_setpoints){
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
=======
// 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;
}
......
......@@ -23,10 +23,17 @@
#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)
......
......@@ -8,9 +8,10 @@
#include "../mobilebot/mobilebot.h"
#include "mb_defs.h"
#include <math.h>
#include <stdlib.h>
typedef enum { F, T } boolean;
#define PI 3.14159265358979323846
//float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
/*******************************************************************************
* mb_initialize_odometry()
*
......@@ -19,7 +20,9 @@
*
*******************************************************************************/
void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float theta){
mb_odometry->x = x;
mb_odometry->y = y;
mb_odometry->theta = theta;
}
......@@ -31,6 +34,35 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
*
*******************************************************************************/
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
// 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_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_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;
mb_odometry->y += del_y;
// 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;
if (Gyro == T){
if (abs(del_go) > del_thesh){mb_odometry->theta += del_theta_gyro * DT;}
else{mb_odometry->theta += del_theta * DT;}
}
else{// this is after we update the del_x and y
mb_odometry->theta += del_theta;}
mb_odometry->theta = mb_clamp_radians(mb_odometry->theta);
}
......
......@@ -168,6 +168,7 @@ void read_mb_sensors(){
void publish_mb_msgs(){
mbot_imu_t imu_msg;
mbot_encoder_t encoder_msg;
odometry_t odo_msg;
mbot_wheel_ctrl_t wheel_ctrl_msg;
// odometry_t odo_msg;
......@@ -198,10 +199,15 @@ void publish_mb_msgs(){
//TODO: Create Odometry LCM message
odo_msg.utime = now;
odo_msg.x = mb_odometry.x;
odo_msg.y = mb_odometry.y;
odo_msg.theta = mb_odometry.theta;
//publish IMU & Encoder Data to LCM
mbot_imu_t_publish(lcm, MBOT_IMU_CHANNEL, &imu_msg);
mbot_encoder_t_publish(lcm, MBOT_ENCODER_CHANNEL, &encoder_msg);
odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
mbot_wheel_ctrl_t_publish(lcm,"MBOT_WHEEL_CTRL",&wheel_ctrl_msg);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
}
......@@ -222,6 +228,9 @@ void publish_mb_msgs(){
void mobilebot_controller(){
update_now();
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_controller_update(&mb_state,&mb_setpoints);
publish_mb_msgs();
rc_motor_set(LEFT_MOTOR,LEFT_MOTOR_POL*mb_state.left_cmd);
......
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