Diff: Odmetry.cpp

Revision:

5:e189d55ef292

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odmetry.cpp	Fri Jan 28 15:12:04 2022 +0000
@@ -0,0 +1,60 @@
+#include "Odmetry.hpp"
+
+Odmetry::Odmetry(){
+    dt = 0.05;
+    Radius = 395;
+    Wradius = 25;
+    XPos = 0;
+    YPos = 0;
+}
+
+void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){
+    pulse_a[0] = PULSE1;
+    pulse_a[1] = PULSE2;
+    pulse_a[2] = PULSE3;
+    pulse_a[3] = PULSE4;
+    
+    if(PULSE1 == PULSE2 == PULSE3 == PULSE4){
+        return;
+    }
+    else{
+        CalcSpd();
+        Angular();
+        transform();
+        CalcPosition();
+    }
+}
+
+void Odmetry::pass_angle(double THETA){
+    Theta = THETA - PI / 4;
+}
+    
+
+void Odmetry::CalcSpd(){
+    for(int i = 0;i < 4;i ++){
+        diff[i] = pulse_a[i] - pulse_b[i];
+        spd[i] = Co * diff[i] / dt;
+        pulse_b[i] = pulse_a[i];
+    }
+}
+
+void Odmetry::Angular(){
+    for(int i = 0;i < 4;i ++){
+        Omega[i] = spd[i] / Radius;
+    }
+}
+
+void Odmetry::transform(){
+    Xpower = Wradius * (Omega[1] - Omega[3]);
+    Ypower = Wradius * (Omega[0] - Omega[2]);
+}
+
+void Odmetry::CalcPosition(){
+    Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower;
+    Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower;
+    
+    XPos += Xspd * dt;
+    YPos += Yspd * dt;
+}
+
+void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }