yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

control_theory/SLE.cpp@22:394337a4205a, 22 months ago (annotated)

Committer:
yootee
Date:
Sat Oct 15 07:52:25 2022 +0000
Revision:
22:394337a4205a
Parent:
6:e7f2335456c8 
upgrade

Who changed what in which revision?

UserRevisionLine numberNew 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 }