wheelchair.cpp

00001 #include "wheelchair.h"
00002 
00003 bool manual_drive = false;
00004 volatile float north;
00005 //volatile double curr_yaw;
00006 double curr_yaw;
00007 double Setpoint, Output;
00008 
00009 void Wheelchair::compass_thread() {
00010     
00011     }
00012     
00013 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
00014 {
00015     x = new PwmOut(xPin);
00016     y = new PwmOut(yPin);
00017     //imu = new chair_MPU9250(pc, time);
00018     Wheelchair::stop();
00019     out = pc;
00020     out->printf("wheelchair setup done \n");
00021     ti = time;
00022 }
00023 
00024 void Wheelchair::forward()
00025 {
00026     x->write(high);
00027     y->write(def+offset);
00028 }
00029 
00030 void Wheelchair::backward()
00031 {
00032     x->write(low);
00033     y->write(def);
00034 }
00035 
00036 void Wheelchair::right()
00037 {
00038     x->write(def);
00039     y->write(low);
00040 }
00041 
00042 void Wheelchair::left()
00043 {
00044     x->write(def);
00045     y->write(high);
00046 }
00047 
00048 void Wheelchair::stop()
00049 {
00050     x->write(def);
00051     y->write(def);
00052 }
00053 // counter clockwise is -
00054 // clockwise is +
00055 double Wheelchair::turn_right(int deg)
00056 {
00057     return;
00058 }
00059 
00060 double Wheelchair::turn_left(int deg)
00061 {
00062     return;
00063 }
00064 
00065 void Wheelchair::turn(int deg)
00066 {
00067  return;   
00068 }
00069 
00070 /*
00071 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
00072 */
00073 void Wheelchair::move(float x_coor, float y_coor, bool leftbound, bool rightbound, bool straightbound)
00074 {
00075 
00076 
00077     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
00078     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
00079     
00080     if(leftbound && (scaled_x > 2.5) ) {
00081             Wheelchair::stop();
00082         }
00083     
00084     else if(rightbound && (scaled_x < 2.5) ) { 
00085             Wheelchair::stop();
00086         }
00087     
00088     else if(straightbound && (scaled_y > 2.5) ) { 
00089             Wheelchair::stop();
00090         }
00091         
00092     else {
00093     x->write(scaled_x);
00094     y->write(scaled_y);
00095     }
00096 }
00097