This repo is for driving a differential-drived robot.
Dependencies: mbed ros_lib_kinetic_1 Encoder
Diff: odometry.cpp
- Revision:
- 0:b21301d612b9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/odometry.cpp Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,45 @@ +#include "odometry.h" +#include <math.h> +Odom::Odom() { +} + +void Odom::init(){ + // Hardware Parameters + radius = 0.075; + width = 0.045; + // Odom Variables + x = 0.0; + y = 0.0; + th = 0.0; + vx = 0.0; + vy = 0.0; + vth = 0.0; + // Calculation Parameters + enc_left = 0.0; + enc_right = 0.0; + delta_distance_left = 0.0; + delta_distance_right = 0.0; +} + +void Odom::get_vel(float v, float w){ + vx = v * cos(th); + vy = v * sin(th); + vth = w; +} + +void Odom::get_distance(float new_enc_left, float new_enc_right){ + float delta_enc_left = new_enc_left - enc_left; + delta_distance_left = delta_enc_left * radius; + enc_right = new_enc_left; + + float delta_enc_right = new_enc_right - enc_right; + delta_distance_right = delta_enc_right * radius; + enc_right = new_enc_right; +} + +void Odom::odom_calculation(){ + float delta_distance_center = ( delta_distance_right - delta_distance_left ) / 2; + x += delta_distance_center * cos(th); + y += delta_distance_center * sin(th); + th += ( delta_distance_right - delta_distance_left ) / width; +} \ No newline at end of file