File content as of revision 28:65daccc10f31:

#include "LOCOMOTION.h"
 
LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4):
    _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
{
    s=0;
    setMotors(0);
    _m1dir=0;
    _m2dir=0;
    _en=1;
    _led1=1;
    wait(0.01);
    _led1=0;
}
 
inline void LOCOMOTION::setMotors(float x)
{
    _m1f=x;
    _m1b=x;
    _m2f=x;
    _m2b=x;
}

inline void LOCOMOTION::setMotors12(float x,float y)
{
    _m1f=x;
    _m1b=x;
    _m2f=y;
    _m2b=y;
}

inline float LOCOMOTION::compG(float speed, bool dir, int angle)
{
    if(dir)
        return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
    else
        return -1*(cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
}
 
inline int LOCOMOTION::wrap(int num)
{
    return num%360;
}
 
void LOCOMOTION::eStop(void)
{
    //Stop Motors
    setMotors(0);
    //Disable Motor Drivers
    _en=0;
}
 
bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
{
    if(abs(current-target)<=error)
        s=SPEED_X_MIN;
    else
        s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
    if(current>target+error) {
        if(angle==0) {
            _m1dir=1;
            _m2dir=1;
        } else {
            _m1dir=0;
            _m2dir=0;
        }
        setMotors(s);
    } else if(current<target-error) {
        if(angle==0) {
            _m1dir=0;
            _m2dir=0;
        } else {
            _m1dir=1;
            _m2dir=1;
        }
        setMotors(s);
    } else {
        setMotors(0);
        return true;
    }
    return false;
}
 
bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
{
    if(abs(current-target)<=error)
        s=Y_BIAS_MIN;
    else
        s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN;
    if(current>target+error) {
        //_m1dir=1;
        //_m2dir=1;
        if(angle==0) {
            _m1f=_m1f*(1+s);
            _m1b=_m1b*(1+s);
        } else {
            _m2f=_m2f*(1+s);
            _m2b=_m2b*(1+s);
        }
    } else if(current<target-error) {
        //_m1dir=0;
        //_m2dir=0;
        if(angle==0) {
            _m2f=_m2f*(1+s);
            _m2b=_m2b*(1+s);
        } else {
            _m1f=_m1f*(1+s);
            _m1b=_m1b*(1+s);
        }
    } else {
        s=0;
        return true;
    }
    return false;
}
 
bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
{
    s = 0;
    int diff = 0;
    diff = 180-wrap(target);
    if(abs(wrap(current+diff)-180)<=error)
        s=SPEED_TURN_MIN;
    else
        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
    switch(mode) {
        default:
        case ANGLE_TURN:
            if(wrap(current+diff)>180+error) {
                _m1dir=1;
                _m2dir=0;
                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
            } else if(wrap(current+diff)<180-error) {
                _m1dir=0;
                _m2dir=1;
                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
            } else {
                setMotors(0);
                return true;
            }
            break;
    }
    return false;
}
 
void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
{
    if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
        if(angleGood && xGood) {
            *xCurrState=X_BACKWARDS;
            wait(0.5);
 
        }
    }
 
    else if(xGood && angleGood && *xCurrState==X_BACKWARDS) {
        if(*angleGoal==0) {
            *xCurrState=ROTATE_1;
            wait(0.5);
        } else {
            *xCurrState=ROTATE_2;
            wait(0.5);
        }
    }
 
    else if (xGood && angleGood) {
        if (*xCurrState==ROTATE_1) {
            *xCurrState=X_INCREASE;
            wait(0.5);
        } else {
            *xCurrState=X_DECREASE;
            wait(0.5);
        }
    }
 
    switch(*xCurrState) {
        case X_INCREASE:
            *angleGoal=180;
            *xTarget=X_FAR_GOAL;
            _m1dir=1;
            _m2dir=1;
            break;
 
        case X_DECREASE:
            *angleGoal=0;
            *xTarget=X_NEAR_GOAL;
            //_m1dir=1;
            //_m2dir=1;
            break;
 
        case X_BACKWARDS:
            if (*angleGoal==0) {
                *xTarget=X_NEAR_GOAL+BACKOFF;
            } else {
                *xTarget=X_FAR_GOAL-BACKOFF;
            }
            //_m1dir=0;
            //_m2dir=0;
            break;
 
        case ROTATE_1:
            *xTarget=*xTarget;
            *angleTol=2;
            *angleGoal=180;
            break;
 
        case ROTATE_2:
            *xTarget=*xTarget;
            *angleTol=2;
            *angleGoal=0;
            break;
 
    }
}
void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
{
    if (xGood||yGood) {
        *angleTol=2;
    } else {
        *angleTol=10;
    }
}
 
void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
{
    if (yGood) {
        *yTarget+=Y_INCREMENT;
    }
}
 
void LOCOMOTION::check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr)
{
    *xGood=abs(xya.x-xGoal)<xErr;
    *yGood=abs(xya.y-yGoal)<yErr;
    *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
}
 
void LOCOMOTION::mStop(void)
{
    setMotors(0);
}
void LOCOMOTION::moveF(int angle)
{
    _m1dir=1;
    _m2dir=1;
    setMotors12(compG(0.5,_m1dir,angle),compG(0.5,_m2dir,angle)); 
}

void LOCOMOTION::moveB(void)
{
    _m1dir=1;
    _m2dir=1;
    setMotors(0.15) ;
}