Commit a8304ab2 authored by jxchenlu's avatar jxchenlu
Browse files

gyrodometry sensor fusion

parent aaf6fab6
......@@ -8,7 +8,8 @@
#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);
/*******************************************************************************
......@@ -35,7 +36,12 @@ 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;
......@@ -44,8 +50,19 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
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);
// Gyro: we only need to use yaw
// Since each time step is small, no need to integrate
float del_theta_gyro = 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);
}
......
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