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