wheelchair.cpp

00001 #include "wheelchair.h"
00002  
00003 bool manual_drive = false;                                                             // Variable Changes between joystick and auto drive
00004 double curr_yaw, curr_velS;                                                             // Variable that contains current relative angle
00005 double encoder_distance;                                                               // Keeps distanse due to original position
00006  
00007 volatile double Setpoint, Output, Input, Input2;                                       // Variables for PID
00008 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;              // Variables for PID
00009 volatile double vIn, vOut, vDesired;
00010 volatile double vInS, vOutS, vDesiredS;
00011 volatile double yIn, yOut, yDesired;
00012 
00013 double dist_old, curr_pos;
00014 
00015  
00016 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
00017 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT);       // Distance PID object constructor
00018 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT);
00019 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);
00020 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);
00021  
00022 void Wheelchair::compass_thread() {                                                    // Thread that measures which angle we are at
00023      curr_yaw = imu->yaw();
00024      z_angular = curr_yaw;
00025     }
00026 void Wheelchair::velosity_thread() {
00027     curr_vel = wheel->getVelosity();
00028      curr_velS = wheelS->getVelosity();
00029      curr_pos = wheel->getDistance(53.975);
00030      }
00031 void Wheelchair::rosCom_thread(){
00032 }
00033 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS)   // Function Constructor for Wheelchair class
00034 {
00035     x_position = 0;
00036     y_position = 0;
00037     //Initializes X and Y variables to Pins
00038     x = new PwmOut(xPin);                                                               
00039     y = new PwmOut(yPin);
00040     // Initializes IMU Library
00041     imu = new chair_BNO055(pc, time);
00042     Wheelchair::stop();                                                                 // Wheelchair is not moving when initializing
00043     imu->setup();                                                                       // turns on the IMU
00044     out = pc;                                                                           // "out" is called for serial monitor
00045     wheelS = qeiS;                                                              // "wheel" is called for encoder
00046     wheel = qei;          
00047     out->printf("wheelchair setup done \r\n");                                          // make sure it initialized
00048     ti = time;
00049     myPID.SetMode(AUTOMATIC);                                                           // set PID to automatic
00050 }
00051  
00052 void Wheelchair::move(float x_coor, float y_coor)                                       // moves the chair with joystick on manual
00053 {
00054  
00055     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;                                     // Scales one joystic measurement to the
00056     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;                                     // chair's joystic measurement
00057     
00058     x->write(scaled_x);                                                                 // Sends the scaled joystic values to the chair
00059     y->write(scaled_y);
00060 }
00061  
00062 void Wheelchair::forward()                                                              // In auto to move foward
00063 {
00064     x->write(high);
00065     y->write(def+offset);
00066 }
00067  
00068 void Wheelchair::backward()                                                             // In auto to move reverse
00069 {
00070     x->write(low);
00071     y->write(def);
00072 }
00073  
00074 void Wheelchair::right()                                                                // In auto to move right
00075 {
00076     x->write(def);
00077     y->write(low);
00078 }
00079  
00080 void Wheelchair::left()                                                                 // In auto to move left
00081 {
00082     x->write(def);
00083     y->write(high);
00084 }
00085  
00086 void Wheelchair::stop()                                                                 // Stops the chair
00087 {
00088     x->write(def);
00089     y->write(def);
00090 }
00091 // counter clockwise is -
00092 // clockwise is +
00093 void Wheelchair::pid_right(int deg)                                                     // Takes in degree and turns right
00094 {
00095     bool overturn = false;                                                              //Boolean if we have to turn over relative 360˚
00096     
00097     out->printf("pid right\r\r\n");                                
00098     x->write(def);                                                                      // Not moving fowards or reverse
00099     Setpoint = curr_yaw + deg;                                                          // Relative angle we want to turn
00100     pid_yaw = curr_yaw;                                                                 // Sets input to current angle(pid_yaw = input)
00101     
00102     if(Setpoint > 360) {                                                                //Turns on overturn boolean if setpoint over 360˚
00103         overturn = true;
00104     }
00105     
00106     myPID.SetTunings(5.5,0, 0.0035);                                                    // Sets the constants for P and D
00107     myPID.SetOutputLimits(0, def-low-.15);                                              // Limits to the differnce between def and low
00108     myPID.SetControllerDirection(DIRECT);                                               // PID mode Direct
00109     
00110     while(pid_yaw < Setpoint - 3){                                                      // Tells PID to stop when reaching
00111                                                                                         // a little less than desired angle
00112         if(overturn && curr_yaw < Setpoint-deg-1)                                       // Sets PID yaw to coterminal angle if necesary
00113         {
00114             pid_yaw = curr_yaw + 360;
00115         }   
00116         else
00117             pid_yaw = curr_yaw;
00118             
00119         myPID.Compute();                                                                // Does PID calculations
00120         double tempor = -Output+def;                                                    // Temporary value with the voltage output
00121         y->write(tempor);                                                               // Sends to chair y output command
00122         
00123         out->printf("curr_yaw %f\r\r\n", curr_yaw);                              
00124         out->printf("Setpoint = %f \r\n", Setpoint);
00125  
00126         wait(.05);                                                                      // Small delay
00127         }
00128     Wheelchair::stop();                                                                 // Safety Stop
00129     out->printf("done \r\n");
00130 }
00131  
00132 void Wheelchair::pid_left(int deg)                                                      // Takes in degree and turns left
00133 {
00134     bool overturn = false;                                                              //Boolean if we have to turn under relative 0˚
00135     
00136     out->printf("pid Left\r\r\n");                                                     
00137     x->write(def);                                                                      // Not moving fowards or reverse
00138     Setpoint = curr_yaw - deg;                                                          // Relative angle we want to turn
00139     pid_yaw = curr_yaw;                                                                 // Sets input to current angle(pid_yaw = input)
00140     if(Setpoint < 0) {                                                                  //Turns on overturn boolean if setpoint under 0˚
00141         overturn = true;
00142     }
00143     myPID.SetTunings(5,0, 0.004);                                                       // Sets the constants for P and D
00144     myPID.SetOutputLimits(0,high-def-.12);                                              // Limits to the differnce between High and Def
00145     myPID.SetControllerDirection(REVERSE);                                              // PID mode Reverse
00146     while(pid_yaw > Setpoint+3){                                                        // Tells PID to stop when reaching
00147                                                                                         // a little more than desired angle
00148        if(overturn && curr_yaw > Setpoint+deg+1)                                        // Sets PID yaw to coterminal angle if necesary
00149        {
00150           pid_yaw = curr_yaw - 360;
00151         }   
00152         else
00153             pid_yaw = curr_yaw;
00154             
00155         myPID.Compute();                                                                // Does PID calculations
00156         double tempor = Output+def;                                                     // Temporary value with the voltage output
00157         y->write(tempor);                                                               // Sends to chair y output command
00158         
00159         out->printf("curr_yaw %f\r\n", curr_yaw);
00160         wait(.05);                                                                      // Small Delay
00161         }
00162     Wheelchair::stop();                                                                 // Safety Stop
00163 }
00164  
00165 void Wheelchair::pid_turn(int deg) {                                                    // Determine wether we are turn right or left
00166  
00167     if(deg > 180) {                                                                     // If deg > 180 turn left: coterminal angle
00168         deg -= 360;
00169     }
00170  
00171     else if(deg < -180) {                                                               // If deg < -180 turn right: coterminal angle
00172         deg+=360;
00173     }  
00174     
00175     int turnAmt = abs(deg);                                                             // Makes sure input angle is positive
00176  
00177     if(deg >= 0){
00178         Wheelchair::pid_right(turnAmt);                                                 // Calls PID right if positive degree
00179     }
00180     else {
00181         Wheelchair::pid_left(turnAmt);                                                  // Calls PID left if negative degree
00182     }
00183 }
00184 void Wheelchair::pid_forward(double mm)
00185 {
00186     mm -= 20;                                                                           // Makes sure distance does not overshoot
00187     Input = 0;                                                                          // Initializes imput to cero: Test latter w/o
00188     wheel->reset();                                                                     // Resets encoders so that they start at 0
00189     out->printf("pid foward\r\n");
00190  
00191     double tempor;                                                                      // Initializes Temporary variable for x input
00192     Setpoint = mm;                                                                      // Initializes the setpoint to desired value
00193  
00194     myPIDDistance.SetTunings(5.5,0, 0.0015);                                            // Sets constants for P and D
00195     myPIDDistance.SetOutputLimits(0,high-def-.15);                                      // Limits to the differnce between High and Def 
00196     myPIDDistance.SetControllerDirection(DIRECT);                                       // PID to Direct
00197     y->write(def+offset);                                                               // Sets chair to not turn
00198     
00199     while(Input < Setpoint){                                                            // Stop moving when reaching setpoint
00200     
00201         if(out->readable())                                                             // Emergency Break
00202             break;
00203             
00204         Input = wheel->getDistance(53.975);                                             // Gets Distance from Encoder onto PID
00205         wait(.05);                                                                      // Slight Delay: *****Test without
00206         myPIDDistance.Compute();                                                        // Compute Output for chair
00207  
00208         tempor = Output + def;                                                          // Temporary output variable                    
00209         x->write(tempor);                                                               // Sends to chair x output
00210         out->printf("distance %f\r\n", Input);
00211         }
00212     
00213 }   
00214 void Wheelchair::pid_reverse(double mm)
00215 {
00216  
00217 }   
00218 void Wheelchair::pid_twistA()
00219 {
00220     char c;
00221     double temporA = def;
00222     y->write(def); 
00223     x->write(def); 
00224  
00225     PIDAngularV.SetTunings(.00015,0, 0.00);                                                    // Sets the constants for P and D
00226     PIDAngularV.SetOutputLimits(-.1, .1);                                              // Limits to the differnce between def and low
00227     PIDAngularV.SetControllerDirection(DIRECT);                                               // PID mode Direct
00228     while(1)
00229     {
00230         if(out->readable())
00231             c = out->getc();
00232         if(c == '0')
00233         {
00234             yDesired = 0;
00235         }
00236         if(c == '1')
00237             yDesired = 30;
00238         if(c == '2')
00239             yDesired = 90;
00240         if(c == '9')
00241             yDesired = -30;
00242         if(c == '8')
00243             yDesired = -90;
00244         if(c == 'r')
00245         {
00246             yDesired = 0;
00247             return;
00248         }
00249           
00250         yIn = imu->gyro_z(); 
00251         PIDAngularV.Compute();
00252         temporA += yOut;                                                     // Temporary value with the voltage output
00253         y->write(temporA); 
00254         //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
00255         wait(.05);
00256     }
00257 }    
00258 void Wheelchair::pid_twistV()
00259 {
00260     double temporV = def;
00261     double temporS = def;
00262     vDesiredS = 0;
00263     char c;
00264     x->write(def);
00265     y->write(def);
00266     wheel->reset();
00267     PIDVelosity.SetTunings(.00005,0, 0.00);                                                    // Sets the constants for P and D
00268     PIDSlaveV.SetTunings(.004,0.000001, 0.000001);                                                    // Sets the constants for P and D
00269     PIDVelosity.SetOutputLimits(-.005, .005);                                              // Limits to the differnce between def and low
00270     PIDSlaveV.SetOutputLimits(-.002, .002);                                              // Limits to the differnce between def and low
00271     PIDVelosity.SetControllerDirection(DIRECT); 
00272     PIDSlaveV.SetControllerDirection(DIRECT); 
00273     while(1)
00274     {
00275         if(out->readable())
00276             c = out->getc();        
00277         if(c == '0')
00278             vDesired = 0;
00279         if(c == '3')
00280             vDesired = 100;
00281         if(c == '4')
00282             vDesired = 150;
00283         if(c == '7')
00284             vDesired = -50; 
00285         if(c == '6')
00286             vDesired = -100; 
00287         if(c == 'r')
00288         {
00289             yDesired = 0;
00290             dist_old = 0;
00291             return;
00292         }
00293         if(vDesired >= 0)
00294         {
00295             PIDVelosity.SetTunings(.000004,0, 0.00);                                                    // Sets the constants for P and D
00296             PIDVelosity.SetOutputLimits(-.002, .002);                                              // Limits to the differnce between def and low
00297         }
00298         else
00299         {
00300             PIDVelosity.SetTunings(.000015,0, 0.00);                                                    // Sets the constants for P and D
00301             PIDVelosity.SetOutputLimits(-.0005, .0005);                                              // Limits to the differnce between def and low
00302         }    
00303         if(temporV >= 1)
00304             temporV = 1;
00305         vIn = curr_vel*100;
00306         vInS = curr_vel-curr_velS;
00307         PIDVelosity.Compute();
00308         PIDSlaveV.Compute();
00309         temporV += vOut;
00310         temporS += vOutS;
00311         x->write(temporV);
00312         y->write(temporS);
00313         //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
00314         Wheelchair::odomMsg();
00315         wait(.01);
00316     }
00317 }
00318 void Wheelchair::odomMsg(){
00319     double dist_new = curr_pos;
00320     double dist = dist_new-dist_old;
00321     double temp_x = dist*sin(z_angular*3.14159/180);
00322     double temp_y = dist*cos(z_angular*3.14159/180);
00323     
00324     x_position += temp_x;
00325     y_position += temp_y;
00326     
00327     dist_old = dist_new;
00328  }
00329  void Wheelchair::showOdom(){
00330      out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular);
00331 }
00332 float Wheelchair::getDistance() {
00333     return wheel->getDistance(Diameter);
00334     }
00335     
00336 void Wheelchair::resetDistance(){
00337     wheel->reset();
00338     }
00339 double Wheelchair::getTwistZ()
00340 {
00341     return imu->gyro_z();
00342 
00343 }  
00344 /*Predetermined paths For Demmo*/    
00345 void Wheelchair::desk() {
00346     Wheelchair::pid_forward(5461);
00347     Wheelchair::pid_right(87);
00348     Wheelchair::pid_forward(3658);
00349     Wheelchair::pid_right(87);
00350     Wheelchair::pid_forward(3658);
00351     }
00352  
00353 void Wheelchair::kitchen() {
00354     Wheelchair::pid_forward(5461);
00355     Wheelchair::pid_right(87);
00356     Wheelchair::pid_forward(3658);
00357     Wheelchair::pid_left(90);
00358     Wheelchair::pid_forward(305);
00359     }
00360  
00361 void Wheelchair::desk_to_kitchen(){
00362     Wheelchair::pid_right(180);
00363     Wheelchair::pid_forward(3700);
00364     }
00365  
00366