File content as of revision 20:1da786e205eb:

#include "LOCOMOTION.h"

LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
    _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
{
    _m1f=0;
    _m1b=0;
    _m2f=0;
    _m2b=0;
    _m1dir=0;
    _m2dir=0;
}

bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
{
    //s = 0;
    if(abs(current-target)<=error)
        s=0.07;
    else
        s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
    if(current>target+error) {
        if(angle==0) {
            _m1dir=1;
            _m2dir=1;
        } else {
            _m1dir=0;
            _m2dir=0;
        }
        _m1f=s;
        _m1b=s;
        _m2f=s;
        _m2b=s;
    } else if(current<target-error) {
        if(angle==0) {
            _m1dir=0;
            _m2dir=0;
        } else {
            _m1dir=1;
            _m2dir=1;
        }
        _m1f=s;
        _m1b=s;
        _m2f=s;
        _m2b=s;
    } else {
        s=0;
        _m1f=s;
        _m1b=s;
        _m2f=s;
        _m2b=s;
        return true;
    }
    return false;
}

bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
{
    //float s = 0;
    if(abs(current-target)<=error)
        s=0.50;
    else
        s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
    if(current>target+error) {
        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) {
        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) {
        case ANGLE_TURN:
            if(wrap(current+diff)>180+error) {
                _m1dir=1;
                _m2dir=0;
                _m1f=s;
                _m1b=s;
                _m2f=s;
                _m2b=s;
            } else if(wrap(current+diff)<180-error) {
                _m1dir=0;
                _m2dir=1;
                _m1f=s;
                _m1b=s;
                _m2f=s;
                _m2b=s;
            } else {
                s=0;
                _m1f=s;
                _m1b=s;
                _m2f=s;
                _m2b=s;
                return true;
            }
            break;
        case ANGLE_BIAS:

            break;
    }
    return false;
}

int LOCOMOTION::wrap(int num)
{
    return num%360;
}
void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal)
{
    if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
        if(angleGood && xGood) {
            *xCurrState=X_BACKWARDS;

        }
    }

    if(*xCurrState==X_BACKWARDS) {
        if(xGood && angleGood) {
            if(*angleGoal==0) {
                *xCurrState=X_INCREASE;
            } else {
                *xCurrState=X_DECREASE;
            }
        }
    }
    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;
    }
}

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 (xGood && angleGood && 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=abs(xya.a-angleGoal)<yErr;    
}