File content as of revision 19:2dd81b864e14:

#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;
}

void LOCOMOTION::stopMotors(void)
{
    _m1f=0;
    _m1b=0;
    _m2f=0;
    _m2b=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 {
        stopMotors();
        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) {
        //_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) {
        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 {
                stopMotors();
                return true;
            }
            break;
        case ANGLE_BIAS:

            break;
    }
    return false;
}

inline int LOCOMOTION::wrap(int num)
{
    return num%360;
}