File content as of revision 40:9a97c4403c0a:

#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 (sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
    else
        return -1*(sin((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(abs(current-target)<30)
        s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
    else
        s=SPEED_X_MAX;
    if(current>target+error) {
        if(angle==0) {
            _m1dir=1;
            _m2dir=1;
        } else {
            _m1dir=0;
            _m2dir=0;
        }
        setMotors12(s,s);
    } else if(current<target-error) {
        if(angle==0) {
            _m1dir=0;
            _m2dir=0;
        } else {
            _m1dir=1;
            _m2dir=1;
        }
        setMotors12(s,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_HE)+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)
{
    float a = 0;
    int diff = 0;
    diff = 180-wrap(target);
    if(abs(wrap(current+diff)-180)<=error)
        a=SPEED_TURN_MIN;
    else
        a=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
    switch(mode) {
        case ANGLE_TURN:
            if(wrap(current+diff)>180+error) {
                _m1dir=0;
                _m2dir=1;
                setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current));
                //_m2dir=0;
            } else if(wrap(current+diff)<180-error) {
                _m1dir=1;
                _m2dir=0;
                setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current));
                //_m2dir=1;
            } else {
                setMotors(0);
                return true;
            }
            break;
        case ANGLE_BIAS:
            if(wrap(current+diff)>180+error) {
                if(_m1dir==0)
                    setMotors12(s*1.4,s*0.6);
                else
                    setMotors12(s*0.6,s*1.4);
                //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
                //_m2dir=0;
            } else if(wrap(current+diff)<180-error) {
                //_m1dir=1;
                //_m2dir=0;
                if(_m1dir==0)
                    setMotors12(s*0.6,s*1.4);
                else
                    setMotors12(s*1.4,s*0.6);
                //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
                //_m2dir=1;
            } else {
                //setMotors(0);
                return true;
            }         
            break;
    }
    return false;
}

void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget)
{
    if (yTarget>=FRAME_HE-ROB_SIZE/2 && xGood) {
        *xCurrState=X_STOP;
    }
    if (*xCurrState==X_INCREASE) {
        if(angleGood && xGood) {
            *xCurrState=X_DECREASE;
            wait(0.5);
        }
    }
    else if (*xCurrState==X_DECREASE) {
        if(angleGood && xGood) {
            *xCurrState=X_INCREASE;
            wait(0.5);
        }

    }

    switch(*xCurrState) {

        case X_INCREASE:
            *angleGoal=0+TILTZZ;
            _m1dir=0;
            _m2dir=0;
            *xTarget=X_FAR_GOAL;
            break;

        case X_DECREASE:
            *angleGoal=0-TILTZZ;
            _m1dir=1;
            _m2dir=1;
            *xTarget=X_NEAR_GOAL;
            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 && xGood) {
        *yTarget+=Y_INCREMENT;
    }
    if (*yTarget>(FRAME_HE-ROB_SIZE/2)) {
        *yTarget=FRAME_HE-ROB_SIZE/2;
    }
}

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.2,_m1dir,angle),compG(0.2,_m2dir,angle));
}

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