wheelchair code for driver assitance

Dependencies:   mbed

Fork of wheelchairalexa by ryan lin

wheelchair.cpp

Committer:
ryanlin97
Date:
22 months ago
Revision:
12:0e5a0571b497
Parent:
11:75f0f13ff6c1

File content as of revision 12:0e5a0571b497:

#include "wheelchair.h"

bool manual_drive = false;
volatile float north;
//volatile double curr_yaw;
double curr_yaw;
double Setpoint, Output;

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

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(low);
}

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

void Wheelchair::stop()
{
    x->write(def);
    y->write(def);
}
// counter clockwise is -
// clockwise is +
double Wheelchair::turn_right(int deg)
{
    return;
}

double Wheelchair::turn_left(int deg)
{
    return;
}

void Wheelchair::turn(int deg)
{
 return;   
}

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


    float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
    float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
    
    if(leftbound && (scaled_x > 2.5) ) {
            Wheelchair::stop();
        }
    
    else if(rightbound && (scaled_x < 2.5) ) { 
            Wheelchair::stop();
        }
    
    else if(straightbound && (scaled_y > 2.5) ) { 
            Wheelchair::stop();
        }
        
    else {
    x->write(scaled_x);
    y->write(scaled_y);
    }
}