wheelchair.cpp

00001 #include "wheelchair.h"
00002 
00003 // Note: 'def' is short for 'Default Position'
00004 
00005 // Initialize Wheelchair Pins
00006 Wheelchair::Wheelchair (PinName xPin, PinName yPin)
00007 {
00008     x = new PwmOut(xPin);
00009     y = new PwmOut(yPin);
00010     imu = new chair_imu();
00011 }
00012 
00013 /*
00014 *   Joystick has AnalogOut of 200-700, scale values between 1.3 and 3.3
00015 */
00016 
00017 // Convert Joystick Values to Move the Wheelchair
00018 void Wheelchair::move(float x_coor, float y_coor)
00019 {
00020    
00021     float scaled_x = ((x_coor * 1.6f) + 1.7f) / 3.3f;
00022     float scaled_y = (3.3f - (y_coor * 1.6f)) / 3.3f;
00023     x->write(scaled_x);
00024     y->write(scaled_y); 
00025 }
00026 
00027 // Turn 90 degrees to the Right
00028 void Wheelchair::turn_right(){
00029     double start = imu->yaw();
00030     double final = start + 90;
00031     if(final > 360)
00032         final -= 360;
00033    
00034     while(imu->yaw() <= final) {
00035         Wheelchair::right();
00036         } 
00037 }
00038 
00039 // Turn 90 degrees to the Left
00040 void Wheelchair::turn_left(){
00041     double start = imu->yaw();
00042     double final = start - 90;
00043     if(final <0)
00044         final += 360;
00045    
00046     while(imu->yaw() >= final) {
00047         Wheelchair::left();
00048         } 
00049 }
00050 
00051 void Wheelchair::forward()
00052 {
00053     x->write(high);
00054     y->write(def+offset);
00055 }
00056  
00057 void Wheelchair::backward()
00058 {
00059     x->write(low);
00060     y->write(def);
00061 }
00062  
00063 void Wheelchair::right()
00064 {
00065     x->write(def);
00066     y->write(high);
00067 }
00068  
00069 void Wheelchair::left()
00070 {
00071     x->write(def);
00072     y->write(low);
00073 }
00074  
00075 void Wheelchair::stop()
00076 {
00077     x->write(def);
00078     y->write(def);
00079 }