![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
my new gear...
SLE.cpp
- Committer:
- yootee
- Date:
- 2022-02-25
- Revision:
- 0:1456b6f84c75
File content as of revision 0:1456b6f84c75:
#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; }