LOCOMOTION.cpp

00001 #include "LOCOMOTION.h"
00002 
00003 LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
00004     _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
00005 {
00006     _m1f=0;
00007     _m1b=0;
00008     _m2f=0;
00009     _m2b=0;
00010     _m1dir=0;
00011     _m2dir=0;
00012 }
00013 
00014 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
00015 {
00016     //s = 0;
00017     if(abs(current-target)<=error)
00018         s=0.07;
00019     else
00020         s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
00021     if(current>target+error) {
00022         if(angle==0) {
00023             _m1dir=1;
00024             _m2dir=1;
00025         } else {
00026             _m1dir=0;
00027             _m2dir=0;
00028         }
00029         _m1f=s;
00030         _m1b=s;
00031         _m2f=s;
00032         _m2b=s;
00033     } else if(current<target-error) {
00034         if(angle==0) {
00035             _m1dir=0;
00036             _m2dir=0;
00037         } else {
00038             _m1dir=1;
00039             _m2dir=1;
00040         }
00041         _m1f=s;
00042         _m1b=s;
00043         _m2f=s;
00044         _m2b=s;
00045     } else {
00046         s=0;
00047         _m1f=s;
00048         _m1b=s;
00049         _m2f=s;
00050         _m2b=s;
00051         return true;
00052     }
00053     return false;
00054 }
00055 
00056 bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
00057 {
00058     //float s = 0;
00059     if(abs(current-target)<=error)
00060         s=0.50;
00061     else
00062         s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
00063     if(current>target+error) {
00064         //_m1dir=1;
00065         //_m2dir=1;
00066         if(angle==0) {
00067             _m1f=_m1f*(1+s);
00068             _m1b=_m1b*(1+s);
00069         } else {
00070             _m2f=_m2f*(1+s);
00071             _m2b=_m2b*(1+s);
00072         }
00073     } else if(current<target-error) {
00074         //_m1dir=0;
00075         //_m2dir=0;
00076         if(angle==0) {
00077             _m2f=_m2f*(1+s);
00078             _m2b=_m2b*(1+s);
00079         } else {
00080             _m1f=_m1f*(1+s);
00081             _m1b=_m1b*(1+s);
00082         }
00083     } else {
00084         s=0;
00085 
00086         return true;
00087     }
00088     return false;
00089 }
00090 
00091 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
00092 {
00093     s = 0;
00094     int diff = 0;
00095     diff = 180-wrap(target);
00096     if(abs(wrap(current+diff)-180)<=error)
00097         s=SPEED_TURN_MIN;
00098     else
00099         s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
00100     switch(mode) {
00101         case ANGLE_TURN:
00102             if(wrap(current+diff)>180+error) {
00103                 _m1dir=1;
00104                 _m2dir=0;
00105                 _m1f=s;
00106                 _m1b=s;
00107                 _m2f=s;
00108                 _m2b=s;
00109             } else if(wrap(current+diff)<180-error) {
00110                 _m1dir=0;
00111                 _m2dir=1;
00112                 _m1f=s;
00113                 _m1b=s;
00114                 _m2f=s;
00115                 _m2b=s;
00116             } else {
00117                 s=0;
00118                 _m1f=s;
00119                 _m1b=s;
00120                 _m2f=s;
00121                 _m2b=s;
00122                 return true;
00123             }
00124             break;
00125         case ANGLE_BIAS:
00126 
00127             break;
00128     }
00129     return false;
00130 }
00131 
00132 int LOCOMOTION::wrap(int num)
00133 {
00134     return num%360;
00135 }