File content as of revision 15:7729da55873a:

#include "LOCOMOTION.h"

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

bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
{
    float 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;
    if(wrap(current+diff)>180+error) {
        
    } else if(wrap(current+diff)<180-error) {
        
    } else {
        s = 0;
    }
    switch(mode) {
        case ANGLE_TURN:

            break;
        case ANGLE_BIAS:

            break;
    }
}

int LOCOMOTION::wrap(int num)
{
    return num%360;
}
/*
void setAngle(int angle)
{
    float s = 0;
    int diff = 0;
    diff = 180-wrap(angle);
    if(abs(wrap(xya.a+diff)-180)<=5)
        s=SPEED_TURN_MIN;
    else
        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
    motor1F=s;
    motor1B=s;
    motor2F=s;
    motor2B=s;
    if(wrap(xya.a+diff)>180+2) {
        dir1=1;
        dir2=0;
    } else if(wrap(xya.a+diff)<180-2) {
        dir1=0;
        dir2=1;
    } else {
        motor1F=0;
        motor1B=0;
        motor2F=0;
        motor2B=0;
    }*/