This repo is for driving a differential-drived robot.
Dependencies: mbed ros_lib_kinetic_1 Encoder
odometry.cpp
- Committer:
- r08522622
- Date:
- 2021-03-10
- Revision:
- 0:b21301d612b9
File content as of revision 0:b21301d612b9:
#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; }