File content as of revision 8:381a4ec3fef8:

#include "wheelchair.h"

Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
{
    x = new PwmOut(xPin);
    y = new PwmOut(yPin);
    //imu = new chair_BNO055();
    imu = new chair_MPU9250(pc, time);
    imu->setup();
    out = pc;
    out->printf("wheelchair setup done \n");

}

/*
* 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);
}
// counter clockwise is +
// clockwise is -

void Wheelchair::turn_right()
{
    bool overturn = false;
    out->printf("turning right\n");

    double start = imu->yaw();
    double final = start - 90;
    
    if(final < 0) {
        final += 360;
        overturn = true;
    }

    out->printf("start %f, final %f\n", start, final);
    
    double curr = 361;
    while(curr >= final) {
        Wheelchair::right();
        if( out->readable()) {
            out->printf("stopped\n");
            Wheelchair::stop();
            return;
        }
        curr = imu->yaw();
        //out->printf("curr %f \n", curr);
        
        if(overturn && curr >= 0 && curr <= start ) {
            curr = 360;
        }
    }

    out->printf("done turning\n");

}

void Wheelchair::turn_left()
{
    bool overturn = false;
    out->printf("turning left\n");

    double start = imu->yaw();
    double final = start + 90;

    if(final > 360) {
        final -= 360;
        overturn = true;
    }

out->printf("start %f, final %f\n", start, final);

    double curr = -1;
    while(curr <= final) {
        Wheelchair::left();
        if( out->readable()) {
            out->printf("stopped\n");
            Wheelchair::stop();
            return;
        }
        curr = imu->yaw();
        if(overturn && curr > (360 - 90) ) {
            curr = 0;
        }
        out->printf("curr %f \n", curr);
    }

    out->printf("done turning\n");
}