Commit b7fd65a9 authored by Demo User's avatar Demo User
Browse files

odometry

parent 2b120f53
......@@ -11,7 +11,7 @@
*******************************************************************************/
int mb_initialize_controller(){
mb_load_controller_config();
//mb_load_controller_config();
return 0;
}
......
......@@ -10,7 +10,7 @@
#include <math.h>
#define PI 3.14159265358979323846
float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
//float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
/*******************************************************************************
* mb_initialize_odometry()
*
......@@ -35,15 +35,17 @@ 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 previous veh's position and orientation
// Reference: https://www.hmc.edu/lair/ARW/ARW-Lecture01-Odometry.pdf P28
float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
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_left + del_right) / 2;
float del_theta = mb_clamp_radians((del_right - del_left) / WHEEL_BASE);
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;
mb_odometry->theta += del_theta;
mb_odometry->theta = mb_clamp_radians(mb_odometry->theta);
}
......
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