wheelchair.cpp

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