SLE.cpp

00001 #include <SLE.hpp>
00002 
00003 SLE::SLE(Port enc0,Port enc1,Port enc2,Port enc3,int resolution,float ensyu,float interval):wheel_ensyu(ensyu),dt(interval)
00004 {
00005     odometer[0].specinit(enc0.pin[0],enc0.pin[1],1,resolution,4);
00006     odometer[1].specinit(enc1.pin[0],enc1.pin[1],1,resolution,4);
00007     odometer[2].specinit(enc2.pin[0],enc2.pin[1],1,resolution,4);
00008     odometer[3].specinit(enc3.pin[0],enc3.pin[1],1,resolution,4);
00009     
00010     //wheel_ensyu = ensyu;
00011     //interval_time = interval;
00012     reset();
00013 }
00014 
00015 void SLE::update(double theta)
00016 {
00017     double encV[4];
00018     for(int i = 0; i < 4; ++i) {
00019         encV[i] = (odometer[i].getSpeed())*wheel_ensyu/10;
00020     }
00021     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;
00022     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;
00023 
00024     position[X_DATA] -= ((Vx[NOW] + Vx[PREV])/2)*dt;
00025     position[Y_DATA] -= ((Vy[NOW] + Vy[PREV])/2)*dt;
00026     velocity[X_DATA] = Vx[NOW];
00027     velocity[Y_DATA] = Vy[NOW];
00028     
00029     Vx[PREV] = Vx[NOW];
00030     Vy[PREV] = Vy[NOW];
00031 }
00032 
00033 void SLE::reset()
00034 {
00035     Vx[NOW]  = 0;
00036     Vx[PREV] = 0;
00037     Vy[NOW]  = 0;
00038     Vy[PREV] = 0;
00039     position[X_DATA] = 0;
00040     position[Y_DATA] = 0;
00041     velocity[X_DATA] = 0;
00042     velocity[Y_DATA] = 0;
00043 }
00044 
00045 void SLE::setPos(float x_pos,float y_pos)
00046 {
00047     position[X_DATA] = x_pos;
00048     position[Y_DATA] = y_pos;
00049 }