File content as of revision 6:0cd57bdd8fbc:

#include "wheelchair.h"

Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc)
{
    x = new PwmOut(xPin);
    y = new PwmOut(yPin);
    //imu = new chair_BNO055();
    imu = new chair_MPU9250(pc);
    
}

/*
* joystick has analog out of 200-700, scale values between 1.3 and 3.3
*/
void Wheelchair::move(float x_coor, float y_coor)
{

    float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
    float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
    x->write(scaled_x);
    y->write(scaled_y);

}
void Wheelchair::forward()
{
    x->write(high);
    y->write(def+offset);
}

void Wheelchair::backward()
{
    x->write(low);
    y->write(def);
}

void Wheelchair::right()
{
    x->write(def);
    y->write(high);
}

void Wheelchair::left()
{
    x->write(def);
    y->write(low);
}

void Wheelchair::stop()
{
    x->write(def);
    y->write(def);
}

void Wheelchair::turn_right(Serial out)
{
    bool stop = false;
    double start = imu->yaw();
    double final = start + 90;
    if(final > 360)
        final -= 360;

    while((imu->yaw() <= final)&& (stop == false)) {
        Wheelchair::right();
        out.printf("turning right");
        if( out.readable()) {
            out.printf("stopped\n");
            Wheelchair::stop();
            return;
        }
    }
}

void Wheelchair::turn_left()
{
    double start = imu->yaw();
    double final = start - 90;
    if(final <0)
        final += 360;

    while(imu->yaw() >= final) {
        Wheelchair::left();
    }
}