yotaro morizumi
/
zoomy_customLibrary
my new gear...
control_theory/SLE.cpp@6:e7f2335456c8, 2022-04-09 (annotated)
- Committer:
- yootee
- Date:
- Sat Apr 09 03:20:19 2022 +0000
- Revision:
- 6:e7f2335456c8
- Parent:
- 2:e7b09385d197
highwaystar
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yootee | 0:1456b6f84c75 | 1 | #include <SLE.hpp> |
yootee | 0:1456b6f84c75 | 2 | |
yootee | 6:e7f2335456c8 | 3 | SLE::SLE(Port enc0,Port enc1,Port enc2,Port enc3,int resolution,float ensyu,float interval):wheel_ensyu(ensyu),dt(interval) |
yootee | 0:1456b6f84c75 | 4 | { |
yootee | 0:1456b6f84c75 | 5 | odometer[0].specinit(enc0.pin[0],enc0.pin[1],1,resolution,4); |
yootee | 0:1456b6f84c75 | 6 | odometer[1].specinit(enc1.pin[0],enc1.pin[1],1,resolution,4); |
yootee | 0:1456b6f84c75 | 7 | odometer[2].specinit(enc2.pin[0],enc2.pin[1],1,resolution,4); |
yootee | 0:1456b6f84c75 | 8 | odometer[3].specinit(enc3.pin[0],enc3.pin[1],1,resolution,4); |
yootee | 0:1456b6f84c75 | 9 | |
yootee | 0:1456b6f84c75 | 10 | //wheel_ensyu = ensyu; |
yootee | 0:1456b6f84c75 | 11 | //interval_time = interval; |
yootee | 0:1456b6f84c75 | 12 | reset(); |
yootee | 0:1456b6f84c75 | 13 | } |
yootee | 0:1456b6f84c75 | 14 | |
yootee | 0:1456b6f84c75 | 15 | void SLE::update(double theta) |
yootee | 0:1456b6f84c75 | 16 | { |
yootee | 0:1456b6f84c75 | 17 | double encV[4]; |
yootee | 0:1456b6f84c75 | 18 | for(int i = 0; i < 4; ++i) { |
yootee | 0:1456b6f84c75 | 19 | encV[i] = (odometer[i].getSpeed())*wheel_ensyu/10; |
yootee | 0:1456b6f84c75 | 20 | } |
yootee | 0:1456b6f84c75 | 21 | 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; |
yootee | 0:1456b6f84c75 | 22 | 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; |
yootee | 0:1456b6f84c75 | 23 | |
yootee | 0:1456b6f84c75 | 24 | position[X_DATA] -= ((Vx[NOW] + Vx[PREV])/2)*dt; |
yootee | 0:1456b6f84c75 | 25 | position[Y_DATA] -= ((Vy[NOW] + Vy[PREV])/2)*dt; |
yootee | 0:1456b6f84c75 | 26 | velocity[X_DATA] = Vx[NOW]; |
yootee | 0:1456b6f84c75 | 27 | velocity[Y_DATA] = Vy[NOW]; |
yootee | 0:1456b6f84c75 | 28 | |
yootee | 0:1456b6f84c75 | 29 | Vx[PREV] = Vx[NOW]; |
yootee | 0:1456b6f84c75 | 30 | Vy[PREV] = Vy[NOW]; |
yootee | 0:1456b6f84c75 | 31 | } |
yootee | 0:1456b6f84c75 | 32 | |
yootee | 0:1456b6f84c75 | 33 | void SLE::reset() |
yootee | 0:1456b6f84c75 | 34 | { |
yootee | 0:1456b6f84c75 | 35 | Vx[NOW] = 0; |
yootee | 0:1456b6f84c75 | 36 | Vx[PREV] = 0; |
yootee | 0:1456b6f84c75 | 37 | Vy[NOW] = 0; |
yootee | 0:1456b6f84c75 | 38 | Vy[PREV] = 0; |
yootee | 0:1456b6f84c75 | 39 | position[X_DATA] = 0; |
yootee | 0:1456b6f84c75 | 40 | position[Y_DATA] = 0; |
yootee | 0:1456b6f84c75 | 41 | velocity[X_DATA] = 0; |
yootee | 0:1456b6f84c75 | 42 | velocity[Y_DATA] = 0; |
yootee | 0:1456b6f84c75 | 43 | } |
yootee | 0:1456b6f84c75 | 44 | |
yootee | 0:1456b6f84c75 | 45 | void SLE::setPos(float x_pos,float y_pos) |
yootee | 0:1456b6f84c75 | 46 | { |
yootee | 0:1456b6f84c75 | 47 | position[X_DATA] = x_pos; |
yootee | 0:1456b6f84c75 | 48 | position[Y_DATA] = y_pos; |
yootee | 0:1456b6f84c75 | 49 | } |