yotaro morizumi
/
zoomy_customLibrary
my new gear...
Diff: control_theory/SLE.cpp
- Revision:
- 2:e7b09385d197
- Parent:
- 0:1456b6f84c75
- Child:
- 6:e7f2335456c8
diff -r 744e1f66d5dc -r e7b09385d197 control_theory/SLE.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/control_theory/SLE.cpp Sun Mar 27 04:39:16 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