wheelchair.cpp

00001 /*************************************************************************
00002 *             Importing header into wheelchair.cpp                       *
00003 **************************************************************************/
00004 #include "wheelchair.h"
00005 
00006 /*************************************************************************
00007 *                     Defining global variables                          *
00008 **************************************************************************/
00009 bool manual_drive = false;                                                             // Variable changes between joystick and auto drive
00010 double encoder_distance;                                                               // Keeps distanse due to original position
00011 
00012 volatile double Setpoint, Output, Input, Input2;                                       // Variables for PID
00013 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;              // Variables for PID
00014 volatile double vIn, vOut, vDesired;                                                   // Variables for PID Velosity
00015 volatile double vInS, vOutS, vDesiredS;                                                // Variables for PID Slave Wheel
00016 volatile double yIn, yOut, yDesired;                                                   // Variables for PID turn velosity
00017 //    int* ToFDataPointer1;
00018 //    int* ToFDataPointer2;
00019 
00020 int ledgeArrayLF[150];
00021 int ledgeArrayRF[150];
00022 int* ToFDataPointer1 = ledgeArrayLF;
00023 int* ToFDataPointer2 = ledgeArrayRF;
00024 Statistics LFTStats(ToFDataPointer1, 149, 1);
00025 Statistics RFTStats(ToFDataPointer2, 149, 1);
00026 int k = 0;
00027 
00028 int ledgeArrayLB[150];
00029 int ledgeArrayRB[150];
00030 int* ToFDataPointer3 = ledgeArrayLB;
00031 int* ToFDataPointer4 = ledgeArrayRB;
00032 Statistics LBTStats(ToFDataPointer3, 149, 1);
00033 Statistics RBTStats(ToFDataPointer4, 149, 1);
00034 
00035 double dist_old, curr_pos;                                                             // Variables for odometry position
00036 double outlierToF[4];
00037 
00038 /*************************************************************************
00039 *                        Creating PID objects                            *
00040 **************************************************************************/
00041 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
00042 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT);        // Distance PID object constructor
00043 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT);               // Velosity PID Constructor
00044 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);              // Slave Velosity PID Constructor
00045 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);               // Angular Velosity PID Constructor
00046 
00047 
00048 /*************************************************************************
00049 *            Thread measures current angular position                    *
00050 **************************************************************************/
00051 void Wheelchair::compass_thread()
00052 {
00053     curr_yaw = imu->yaw();
00054     z_angular = curr_yaw;
00055 }
00056 
00057 /*************************************************************************
00058 *      Thread measures velocity of wheels and distance traveled          *
00059 **************************************************************************/
00060 void Wheelchair::velocity_thread()
00061 {
00062     curr_vel = wheel->getVelocity();
00063     curr_velS = wheelS->getVelocity();
00064     curr_pos = wheel->getDistance(53.975);
00065 }
00066 
00067 void Wheelchair::emergencyButton_thread ()
00068 {
00069     while(1) {
00070         while(!e_button) {
00071 
00072             //Stop wheelchair
00073             Wheelchair::stop();
00074             printf("E-button has been pressed\r\n");
00075             off->write(high);                              // Turn off PCB
00076             on->write(0);                                  // Make sure PCB not on
00077             //Reset Board
00078             NVIC_SystemReset();
00079 
00080         }
00081 
00082     }
00083 }
00084 
00085 /*************************************************************************
00086 *      Thread checks ToF sensors for safety of wheelchair movement       *
00087 **************************************************************************/
00088 void Wheelchair::ToFSafe_thread()
00089 {
00090     int ToFV[12];
00091     for(int i = 0; i < 6; i++) {                            // Reads from the ToF Sensors
00092         ToFV[i] = (*(ToF+i))->readFromOneSensor();
00093         //out->printf("%d ", ToFV[i]);
00094     }
00095 
00096     //out->printf("\r\n");
00097 
00098     k++;
00099 
00100     if (k == 150) {
00101         k = 1;
00102     }
00103 
00104     ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor();
00105     ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor();
00106 
00107     /* for(int i = 0; i < 100; i++)
00108     {
00109         out->printf("%d, ",ledgeArrayRF[i]);
00110     }
00111     out->printf("\r\n"); */
00112 
00113     outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
00114     outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
00115 
00116     for(int i = 0; i < 2; i++) {                             // Reads from the ToF Sensors
00117         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
00118     }
00119 
00120     int sensor1 = ToFV[0];
00121     int sensor4 = ToFV[3];
00122     if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 ||
00123                         2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) &&
00124                        (sensor1 < 1500 || sensor4 < 1500)) ||
00125             550 > sensor1 || 550 > sensor4) {
00126         if(x->read() > def) {
00127             x->write(def);
00128             forwardSafety = 1;          // You cannot move forward
00129         }
00130     }
00131 
00132     else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 ||
00133                              2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) &&
00134                             (sensor1 < 1500 || sensor4 < 1500)) ||
00135             550 > sensor1 || 550 > sensor4) {
00136         if(x->read() > def) {
00137             x->write(def);
00138             forwardSafety = 1;          // You cannot move forward
00139         }
00140     }
00141 
00142     else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) {
00143         forwardSafety = 1;
00144         out->printf("I'M STOPPING BECAUSE OF A FRONT LEDGE\r\n");
00145     }
00146 
00147     else
00148         forwardSafety = 0;
00149     
00150     ////////////////////////////////////////////////////////////////////////////
00151 
00152     ledgeArrayLB[k] = (*(ToF+1))->readFromOneSensor();
00153     ledgeArrayRB[k] = (*(ToF+4))->readFromOneSensor();
00154 
00155     outlierToF[2] = LBTStats.mean() + 2*LBTStats.stdev();
00156     outlierToF[3] = RBTStats.mean() + 2*RBTStats.stdev();
00157 
00158     for(int i = 2; i < 4; i++) {                             // Reads from the ToF Sensors
00159         runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5);
00160     }
00161 
00162     int sensor7 = ToFV[6];
00163     int sensor10 = ToFV[9];
00164     if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 ||
00165                         2 * maxDecelerationSlow*sensor10 < curr_vel*curr_vel*1000*1000) &&
00166                        (sensor7 < 1500 || sensor10 < 1500)) ||
00167             550 > sensor7 || 550 > sensor10) {
00168         if(x->read() > def) {
00169             x->write(def);
00170             backwardSafety = 1;          // You cannot move backwards
00171         }
00172     }
00173 
00174     else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 ||
00175                              2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) &&
00176                             (sensor7 < 1500 || sensor10 < 1500)) ||
00177             550 > sensor7 || 550 > sensor10) {
00178         if(x->read() > def) {
00179             x->write(def);
00180             backwardSafety = 1;          // You cannot move backwards
00181         }
00182     }
00183 
00184     else if ((runningAverage[2] > outlierToF[2]) || (runningAverage[3] > outlierToF[3])) {
00185         backwardSafety = 1;
00186         out->printf("I'M STOPPING BECAUSE OF A BACK LEDGE\r\n");
00187     }
00188 
00189     else
00190         backwardSafety = 0;
00191         
00192     ////////////////////////////////////////////////////////////////////////////
00193 
00194     /*Side Tof begin*/
00195     int sensor3 = ToFV[2]; //front left
00196     int sensor6 = ToFV[5]; //front right
00197     int sensor9 = ToFV[8]; //back
00198     int sensor12 = ToFV[11]; //back
00199     
00200     double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU
00201     double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads
00202     double arcLength = WheelchairRadius * currAngularVelocity * 
00203                        currAngularVelocity / (2 * maxAngularDeceleration); //S = r*Ө, in cm
00204    
00205     /* Clear the front side first, else continue going straight or can't turn
00206     After clearing the front sideand movinf forward, check if can clear the back 
00207     when turning */
00208         
00209     //When either sensors too close to the wall, can't turn
00210     if(sensor3 <= minWallLength) {
00211         leftSafety = 1;
00212         out-> printf("Detecting wall to the left!\n");
00213     }
00214     else{
00215         leftSafety = 0;
00216     }
00217     
00218     if(sensor6 <= minWallLength) {
00219         rightSafety = 1;
00220         out-> printf("Detecting wall to the right!\n");
00221     }
00222     else {
00223         rightSafety = 0;
00224     }
00225     
00226     /*Check whether safe to keep turning 
00227     Know the exact moment you can stop the chair going at a certain speed before 
00228     its too late*/
00229     if((currAngularVelocity * currAngularVelocity > 2 * 
00230         maxAngularDeceleration * angle) && (sensor3/10 <= arcLength + 10)) {
00231         leftSafety = 1; //Not safe to turn left
00232         out-> printf("Too fast to the left!\n");
00233     }
00234     else{
00235         leftSafety = 0;
00236        }
00237     if((currAngularVelocity * currAngularVelocity > 2 * 
00238         maxAngularDeceleration * angle) && (sensor6/10 <= arcLength + 10)) {
00239         rightSafety = 1; //Not safe to turn right
00240         out-> printf("Too fast to the right!\n");
00241     }
00242     else{
00243         rightSafety = 0;
00244         }
00245       
00246     //Safe to continue turning 
00247     //Check if can turn left and back side sensors
00248     // Staring t road and frequenctly checking
00249     //Check the back sensor
00250     /*int sensor7 = ToFV[0]; //back sensor NOTTT SURE
00251     int sensor8 = ToFV[3]; //back sensor
00252     
00253     if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 || 
00254     2 * maxDecelerationSlow*sensor8 < curr_vel*curr_vel*1000*1000) && 
00255     (sensor7 < 1500 || sensor8 < 1500)) ||
00256     550 > sensor7 || 550 > sensor8)
00257     {
00258         //out->printf("i am in danger\r\n");
00259         if(x->read() > def)
00260         {
00261             x->write(def);
00262             backwardSafety = 1;// You cannot move backward
00263         }
00264     }
00265     //When going to fast to stop from wall
00266     else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor7 < curr_vel*curr_vel*1000*1000 || 
00267     2 * maxDecelerationFast*sensor8 < curr_vel*curr_vel*1000*1000) && 
00268     (sensor7 < 1500 || sensor8 < 1500)) ||
00269     550 > sensor7 || 550 > sensor8)
00270     {
00271         //out->printf("i am in danger\r\n");
00272         if(x->read() > def)
00273         {
00274             x->write(def);
00275             backwardSafety = 1;
00276         }
00277     }*/
00278       
00279     /*Side Tof end*/
00280     
00281 }
00282 
00283 /*************************************************************************
00284 *                   Constructor for Wheelchair class                     *
00285 **************************************************************************/
00286 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS,
00287                        VL53L1X** ToFT)
00288 {
00289     x_position = 0;
00290     y_position = 0;
00291     forwardSafety = 0;
00292     backwardSafety = 0;
00293 
00294     /* Initializes X and Y variables to Pins */
00295     x = new PwmOut(xPin);
00296     y = new PwmOut(yPin);
00297 
00298     /* Initializes IMU Library */
00299     out = pc;                                                                           // "out" is called for serial monitor
00300     out->printf("on\r\n");
00301     imu = new IMUWheelchair(pc, time);
00302     Wheelchair::stop();                                                                 // Wheelchair is initially stationary
00303     imu->setup();                                                                       // turns on the IMU
00304     wheelS = qeiS;                                                                      // "wheel" is called for encoder
00305     wheel = qei;
00306     ToF = ToFT;                                                                         // passes pointer with addresses of ToF sensors
00307 
00308     for(int i = 0; i < 12; i++) {                                                       // initializes the ToF Sensors
00309         (*(ToF+i))->initReading(0x31+((0x02)*i), 50000);
00310     }
00311 
00312     out->printf("wheelchair setup done \r\n");                                          // Make sure it initialized; prints in serial monitor
00313     ti = time;
00314     for(int i = 0; i < 10; i++) {
00315         (*(ToF+1))->readFromOneSensor();
00316         (*(ToF+1))->readFromOneSensor();
00317     }
00318     for(int i = 0; i < 150; i++) {
00319         ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor();
00320         ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor();
00321     }
00322 
00323     outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev();
00324     outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev();
00325 
00326     myPID.SetMode(AUTOMATIC);                                                           // PID mode: Automatic
00327 }
00328 
00329 /*************************************************************************
00330 *               Move wheelchair with joystick on manual mode             *
00331 **************************************************************************/
00332 void Wheelchair::move(float x_coor, float y_coor)
00333 {
00334     /* Scales one joystick measurement to the chair's joystick measurement */
00335     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
00336     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
00337 
00338     /* Sends the scaled joystic values to the chair */
00339     x->write(scaled_x);
00340     y->write(scaled_y);
00341 }
00342 
00343 
00344 /*************************************************************************
00345 *  Automatic mode: move forward and update x,y coordinate sent to chair  *
00346 **************************************************************************/
00347 void Wheelchair::forward()
00348 {
00349     //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS);
00350     if(forwardSafety == 0) {
00351         x->write(high);
00352         y->write(def+offset);
00353     }
00354     out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975));
00355 }
00356 
00357 /*************************************************************************
00358 * Automatic mode: move in reverse and update x,y coordinate sent to chair*
00359 **************************************************************************/
00360 void Wheelchair::backward()
00361 {
00362     if (backwardSafety == 0) {
00363         x->write(low);
00364         y->write(def);
00365     }
00366 }
00367 /*************************************************************************
00368 *   Automatic mode: move right and update x,y coordinate sent to chair   *
00369 **************************************************************************/
00370 void Wheelchair::right()
00371 {
00372     //if safe to move, from ToFSafety
00373     if(rightSafety == 0) {
00374         x->write(def);
00375         y->write(low);
00376     }
00377 }
00378 /*************************************************************************
00379 *   Automatic mode: move left and update x,y coordinate sent to chair   *
00380 **************************************************************************/
00381 void Wheelchair::left()
00382 {
00383     //if safe to move, from ToFSafety
00384     if(leftSafety == 0) {
00385         x->write(def);
00386         y->write(high);
00387     }
00388 }
00389 /*************************************************************************
00390 *                           Stop the wheelchair                          *
00391 **************************************************************************/
00392 void Wheelchair::stop()
00393 {
00394     x->write(def);
00395     y->write(def);
00396 }
00397 /*************************************************************************
00398  * Counter-clockwise is ( - )                                            *
00399  * Clockwise is ( + )                                                    *
00400  * Range of deg: 0 to 360                                                *
00401  * This method takes in an angle from user and adjusts for turning right *
00402 **************************************************************************/
00403 void Wheelchair::pid_right(int deg)
00404 {
00405     bool overturn = false;                                                              // Boolean if angle over 360˚
00406 
00407     out->printf("pid right\r\r\n");
00408     x->write(def);                                                                      // Update x sent to chair to be stationary
00409     Setpoint = curr_yaw + deg;                                                          // Relative angle we want to turn
00410     pid_yaw = curr_yaw;                                                                 // Sets pid_yaw to angle input from user
00411 
00412     /* Turns on overturn boolean if setpoint over 360˚ */
00413     if(Setpoint > 360) {
00414         overturn = true;
00415     }
00416 
00417     myPID.SetTunings(5.5,0, 0.0035);                                                    // Sets the constants for P and D
00418     myPID.SetOutputLimits(0, def-low-.15);                                              // Limit is set to the differnce between def and low
00419     myPID.SetControllerDirection(DIRECT);                                               // PID mode: Direct
00420 
00421     /* PID stops when approaching a litte less than desired angle */
00422     while(pid_yaw < Setpoint - 3) {
00423         /* PID is set to correct angle range if angle greater than 360˚*/
00424         if(overturn && curr_yaw < Setpoint-deg-1) {
00425             pid_yaw = curr_yaw + 360;
00426         } else {
00427             pid_yaw = curr_yaw;
00428         }
00429 
00430         myPID.Compute();                                                                // Does PID calculations
00431         double tempor = -Output+def;                                                    // Temporary value with the voltage output
00432         y->write(tempor);                                                               // Update y sent to chair
00433 
00434         /* Prints to serial monitor the current angle and setpoint */
00435         out->printf("curr_yaw %f\r\r\n", curr_yaw);
00436         out->printf("Setpoint = %f \r\n", Setpoint);
00437 
00438         wait(.05);                                                                      // Small delay (milliseconds)
00439     }
00440 
00441     /* Saftey stop for wheelchair */
00442     Wheelchair::stop();
00443     out->printf("done \r\n");
00444 }
00445 /*************************************************************************
00446 * Counter-clockwise is ( - )                                            *
00447 * Clockwise is ( + )                                                    *
00448 * Range of deg: 0 to 360                                                *
00449 * This method takes in an angle from user and adjusts for turning left  *
00450 **************************************************************************/
00451 void Wheelchair::pid_left(int deg)
00452 {
00453     bool overturn = false;                                                              //Boolean if angle under 0˚
00454 
00455     out->printf("pid Left\r\r\n");
00456     x->write(def);                                                                      // Update x sent to chair to be stationary
00457     Setpoint = curr_yaw - deg;                                                          // Relative angle we want to turn
00458     pid_yaw = curr_yaw;                                                                 // Sets pid_yaw to angle input from user
00459 
00460     /* Turns on overturn boolean if setpoint less than 0˚ */
00461     if(Setpoint < 0) {
00462         overturn = true;
00463     }
00464 
00465     myPID.SetTunings(5,0, 0.004);                                                       // Sets the constants for P and D
00466     myPID.SetOutputLimits(0,high-def-.12);                                              // Limit is set to the differnce between def and low
00467     myPID.SetControllerDirection(REVERSE);                                              // PID mode: Reverse
00468 
00469     /* PID stops when approaching a litte more than desired angle */
00470     while(pid_yaw > Setpoint+3) {
00471         /* PID is set to correct angle range if angle less than 0˚ */
00472         if(overturn && curr_yaw > Setpoint+deg+1) {
00473             pid_yaw = curr_yaw - 360;
00474         } else {
00475             pid_yaw = curr_yaw;
00476         }
00477 
00478         myPID.Compute();                                                                // Does PID calculations
00479         double tempor = Output+def;                                                     // Temporary value with the voltage output
00480         y->write(tempor);                                                               // Update y sent to chair
00481 
00482         /* Prints to serial monitor the current angle and setpoint */
00483         out->printf("curr_yaw %f\r\n", curr_yaw);
00484         out->printf("Setpoint = %f \r\n", Setpoint);
00485 
00486         wait(.05);                                                                      // Small delay (milliseconds)
00487     }
00488 
00489     /* Saftey stop for wheelchair */
00490     Wheelchair::stop();
00491     out->printf("done \r\n");
00492 
00493 }
00494 
00495 /*************************************************************************
00496 *          This method determines whether to turn left or right          *
00497 **************************************************************************/
00498 void Wheelchair::pid_turn(int deg)
00499 {
00500 
00501     /*****************************************************************
00502      * Sets angle to coterminal angle for left turn if deg > 180     *
00503      *  Sets angle to coterminal angle for right turn if deg < -180  *
00504      *****************************************************************/
00505     if(deg > 180) {
00506         deg -= 360;
00507     } else if(deg < -180) {
00508         deg +=360;
00509     }
00510 
00511     /* Makes sure angle inputted to function is positive */
00512     int turnAmt = abs(deg);
00513 
00514     /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */
00515     if(deg >= 0) {
00516         Wheelchair::pid_right(turnAmt);
00517     } else {
00518         Wheelchair::pid_left(turnAmt);
00519     }
00520 
00521 }
00522 
00523 /*************************************************************************
00524 *   This method takes in distance to travel and adjust to move forward   *
00525 **************************************************************************/
00526 void Wheelchair::pid_forward(double mm)
00527 {
00528     mm -= 20;                                                                           // Makes sure distance does not overshoot
00529     Input = 0;                                                                          // Initializes input to zero: Test latter w/o
00530     wheel->reset();                                                                     // Resets encoders so that they start at 0
00531 
00532     out->printf("pid foward\r\n");
00533 
00534     double tempor;                                                                      // Initializes Temporary variable for x input
00535     Setpoint = mm;                                                                      // Initializes the setpoint to desired value
00536 
00537     myPIDDistance.SetTunings(5.5,0, 0.0015);                                            // Sets constants for P and D
00538     myPIDDistance.SetOutputLimits(0,high-def-.15);                                      // Limit set to difference between high and def
00539     myPIDDistance.SetControllerDirection(DIRECT);                                       // PID mode: Direct
00540 
00541     y->write(def+offset);                                                               // Update y to make chair stationary
00542 
00543     /* Chair stops moving when Setpoint is reached */
00544     while(Input < Setpoint) {
00545 
00546         if(out->readable()) {                                                           // Emergency Break
00547             break;
00548         }
00549 
00550         Input = wheel->getDistance(53.975);                                             // Gets distance from Encoder into PID
00551         wait(.05);                                                                      // Slight Delay: *****Test without
00552         myPIDDistance.Compute();                                                        // Compute distance traveled by chair
00553 
00554         tempor = Output + def;                                                          // Temporary output variable
00555         x->write(tempor);                                                               // Update x sent to chair
00556 
00557         /* Prints to serial monitor the distance traveled by chair */
00558         out->printf("distance %f\r\n", Input);
00559     }
00560 
00561 }
00562 
00563 /*************************************************************************
00564 *        This method returns the relative angular position of chair      *
00565 **************************************************************************/
00566 double Wheelchair::getTwistZ()
00567 {
00568     return imu->gyro_z();
00569 }
00570 
00571 /*************************************************************************
00572 *    This method computes the relative angle for Twist message in ROS    *
00573 **************************************************************************/
00574 void Wheelchair::pid_twistA()
00575 {
00576     /* Initialize variables for angle and update x,y sent to chair */
00577     char c;
00578     double temporA = def;
00579     y->write(def);
00580     x->write(def);
00581 
00582     PIDAngularV.SetTunings(.00015,0, 0.00);                                             // Sets the constants for P and D
00583     PIDAngularV.SetOutputLimits(-.1, .1);                                               // Limit set to be in range specified
00584     PIDAngularV.SetControllerDirection(DIRECT);                                         // PID mode: Direct
00585 
00586     /* Computes angular position of wheelchair while turning */
00587     while(1) {
00588         yDesired = angularV;
00589 
00590         /* Update and set all variable so that the chair is stationary
00591          * if the desired angle is zero
00592          */
00593         if(yDesired == 0) {
00594             x->write(def);
00595             y->write(def);
00596             yDesired = 0;
00597             return;
00598         }
00599 
00600         /* Continuously updates with current angle measured by IMU */
00601         yIn = imu->gyro_z();
00602         PIDAngularV.Compute();
00603         temporA += yOut;                                                                // Temporary value with the voltage output
00604         y->write(temporA);                                                              // Update y sent to chair
00605 
00606         //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
00607         wait(.05);                                                                      // Small delay (milliseconds)
00608     }
00609 
00610 }
00611 
00612 /*************************************************************************
00613 *   This method computes the relative velocity for Twist message in ROS  *
00614 **************************************************************************/
00615 void Wheelchair::pid_twistV()
00616 {
00617     /* Initializes variables as default */
00618     double temporV = def;
00619     double temporS = def+offset;
00620     vDesiredS = 0;
00621     x->write(def);
00622     y->write(def);
00623     wheel->reset();                                                                     // Resets the encoders
00624     /* Sets the constants for P and D */
00625     PIDVelosity.SetTunings(.0005,0, 0.00);
00626     PIDSlaveV.SetTunings(.005,0.000001, 0.000001);
00627 
00628     /* Limits to the range specified */
00629     PIDVelosity.SetOutputLimits(-.005, .005);
00630     PIDSlaveV.SetOutputLimits(-.002, .002);
00631 
00632     /* PID mode: Direct */
00633     PIDVelosity.SetControllerDirection(DIRECT);
00634     PIDSlaveV.SetControllerDirection(DIRECT);
00635 
00636     while(1) {
00637         linearV = .7;
00638         test1 = linearV*100;
00639         vel = curr_vel;
00640         vDesired = linearV*100;
00641         if(out->readable())
00642             return;
00643         /* Update and set all variable so that the chair is stationary
00644         * if the velocity is zero
00645         */
00646         if(linearV == 0) {
00647             x->write(def);
00648             y->write(def);
00649 
00650             vel = 0;
00651             vDesired = 0;
00652             dist_old = 0;
00653             return;
00654         }
00655 
00656         if(vDesired >= 0) {
00657             PIDVelosity.SetTunings(.000004,0, 0.00);                                    // Sets the constants for P and D
00658             PIDVelosity.SetOutputLimits(-.002, .002);                                   // Limits to the range specified
00659         } else {
00660             PIDVelosity.SetTunings(.000015,0, 0.00);                                    // Sets the constants for P and D
00661             PIDVelosity.SetOutputLimits(-.0005, .0005);                                 // Limits to range specified
00662         }
00663 
00664         /* Sets maximum value of variable to 1 */
00665         if(temporV >= 1.5) {
00666             temporV = 1.5;
00667         }
00668         /* Scales and makes some adjustments to velocity */
00669         vIn = curr_vel*100;
00670         vInS = curr_vel-curr_velS;
00671         PIDVelosity.Compute();
00672         PIDSlaveV.Compute();
00673         if(forwardSafety == 0) {
00674             temporV += vOut;
00675             temporS += vOutS;
00676 
00677             /* Updates x,y sent to Wheelchair and for Odometry message in ROS */
00678             x->write(temporV);
00679             test2 = temporV;
00680             y->write(temporS);
00681         } else {
00682             x->write(def);
00683             y->write(def);
00684         }
00685         //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
00686         Wheelchair::odomMsg();
00687         wait(.01);                                                                      // Small delay (milliseconds)
00688     }
00689 }
00690 
00691 /*************************************************************************
00692 * This method calculates the relative position of the chair everytime the*
00693 * encoders reset by setting its old position as the origin to calculate  *
00694 * the new position                                                       *
00695 **************************************************************************/
00696 void Wheelchair::odomMsg()
00697 {
00698     double dist_new = curr_pos;
00699     double dist = dist_new-dist_old;
00700     double temp_x = dist*sin(z_angular*3.14159/180);
00701     double temp_y = dist*cos(z_angular*3.14159/180);
00702 
00703     x_position += temp_x;
00704     y_position += temp_y;
00705 
00706     dist_old = dist_new;
00707 }
00708 
00709 /**************************************************************************
00710 *     This method prints the Odometry message to the serial monitor       *
00711 ***************************************************************************/
00712 void Wheelchair::showOdom()
00713 {
00714     out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular);
00715 }
00716 
00717 /**************************************************************************
00718 * This method returns the approximate distance based on the wheel diameter*
00719 ***************************************************************************/
00720 float Wheelchair::getDistance()
00721 {
00722     return wheel->getDistance(Diameter);
00723 }
00724 
00725 /**************************************************************************
00726 *                   This method resets the Encoder's                      *
00727 ***************************************************************************/
00728 void Wheelchair::resetDistance()
00729 {
00730     wheel->reset();
00731 }
00732 
00733 /*---------------------------------------------------------------------------------------------------*/
00734 /*---------------------------------------------------------------------------------------------------*/
00735 /*---------------------------------------------------------------------------------------------------*/
00736 /*---------------------------------------------------------------------------------------------------*/
00737 /*---------------------------------------------------------------------------------------------------*/
00738 
00739 /*Predetermined paths For Demmo*/
00740 void Wheelchair::desk()
00741 {
00742     Wheelchair::pid_forward(5461);
00743     Wheelchair::pid_right(87);
00744     Wheelchair::pid_forward(3658);
00745     Wheelchair::pid_right(87);
00746     Wheelchair::pid_forward(3658);
00747 }
00748 
00749 void Wheelchair::kitchen()
00750 {
00751     Wheelchair::pid_forward(5461);
00752     Wheelchair::pid_right(87);
00753     Wheelchair::pid_forward(3658);
00754     Wheelchair::pid_left(90);
00755     Wheelchair::pid_forward(305);
00756 }
00757 
00758 void Wheelchair::desk_to_kitchen()
00759 {
00760     Wheelchair::pid_right(180);
00761     Wheelchair::pid_forward(3700);
00762 }