Diff: SLE.cpp

Revision:

0:1456b6f84c75

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SLE.cpp	Fri Feb 25 05:20:11 2022 +0000
@@ -0,0 +1,49 @@
+#include <SLE.hpp>
+
+SLE::SLE(Port enc0,Port enc1,Port enc2,Port enc3,float resolution,float ensyu,float interval):wheel_ensyu(ensyu),dt(interval)
+{
+    odometer[0].specinit(enc0.pin[0],enc0.pin[1],1,resolution,4);
+    odometer[1].specinit(enc1.pin[0],enc1.pin[1],1,resolution,4);
+    odometer[2].specinit(enc2.pin[0],enc2.pin[1],1,resolution,4);
+    odometer[3].specinit(enc3.pin[0],enc3.pin[1],1,resolution,4);
+    
+    //wheel_ensyu = ensyu;
+    //interval_time = interval;
+    reset();
+}
+
+void SLE::update(double theta)
+{
+    double encV[4];
+    for(int i = 0; i < 4; ++i) {
+        encV[i] = (odometer[i].getSpeed())*wheel_ensyu/10;
+    }
+    Vx[NOW] = ((encV[3] * -sin(theta)) +(encV[0] * -sin(theta + M_PI/2)) +(encV[1] * -sin(theta + M_PI)) +(encV[2] * -sin(theta + 3*(M_PI/2))))/2;
+    Vy[NOW] = ((encV[2] *  sin(theta)) +(encV[3] *  sin(theta + M_PI/2)) +(encV[0] *  sin(theta + M_PI)) +(encV[1] *  sin(theta + 3*(M_PI/2))))/2;
+
+    position[X_DATA] -= ((Vx[NOW] + Vx[PREV])/2)*dt;
+    position[Y_DATA] -= ((Vy[NOW] + Vy[PREV])/2)*dt;
+    velocity[X_DATA] = Vx[NOW];
+    velocity[Y_DATA] = Vy[NOW];
+    
+    Vx[PREV] = Vx[NOW];
+    Vy[PREV] = Vy[NOW];
+}
+
+void SLE::reset()
+{
+    Vx[NOW]  = 0;
+    Vx[PREV] = 0;
+    Vy[NOW]  = 0;
+    Vy[PREV] = 0;
+    position[X_DATA] = 0;
+    position[Y_DATA] = 0;
+    velocity[X_DATA] = 0;
+    velocity[Y_DATA] = 0;
+}
+
+void SLE::setPos(float x_pos,float y_pos)
+{
+    position[X_DATA] = x_pos;
+    position[Y_DATA] = y_pos;
+}
\ No newline at end of file