wheelchair.cpp

00001 #include "wheelchair.h"
00002 Serial qei(D1, D0);
00003 
00004 DigitalOut on(D12);
00005 DigitalOut off(D11);
00006 DigitalOut up(D2);
00007 DigitalOut down(D3);
00008 
00009 bool manual_drive = false;
00010 double encoder_distance;
00011 
00012 double curr_yaw;
00013 double Setpoint, Output, Input;
00014 PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT);
00015 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);
00016 
00017 void Wheelchair::compass_thread() {
00018      curr_yaw = imu->yaw();
00019     }
00020     
00021 double Wheelchair::readEncoder() {
00022     char input[64] = {0};
00023     for (int i=0; input[i-1] != '\n'; i++) {
00024             while (true) {
00025                 //pc.printf("%f\r\n", ti.read());
00026                 if (ti->read() > .02) break;
00027                 if (qei.readable()) {
00028                     input[i]= qei.getc();
00029                     break;
00030                     }
00031                 }
00032         }
00033     return atoi(input);
00034     }
00035     
00036 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei )
00037 {
00038     x = new PwmOut(xPin);
00039     y = new PwmOut(yPin);
00040     imu = new chair_BNO055(pc, time);
00041    
00042     Wheelchair::stop();
00043     imu->setup();
00044     out = pc;
00045     out->printf("wheelchair setup done \n");
00046     ti = time;
00047     wheel = qei;
00048     myPID.SetMode(AUTOMATIC);
00049 }
00050 
00051 /*
00052 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
00053 */
00054 void Wheelchair::move(float x_coor, float y_coor)
00055 {
00056 
00057     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
00058     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
00059     
00060    // lowPass(scaled_x);
00061     //lowPass(scaled_y);
00062     
00063     x->write(scaled_x);
00064     y->write(scaled_y);
00065     
00066     //out->printf("yaw %f\n", imu->yaw());
00067 
00068 }
00069 
00070 void Wheelchair::forward()
00071 {
00072     x->write(high);
00073     y->write(def+offset);
00074 }
00075 
00076 void Wheelchair::backward()
00077 {
00078     x->write(low);
00079     y->write(def);
00080 }
00081 
00082 void Wheelchair::right()
00083 {
00084     x->write(def);
00085     y->write(low);
00086 }
00087 
00088 void Wheelchair::left()
00089 {
00090     x->write(def);
00091     y->write(high);
00092 }
00093 
00094 void Wheelchair::stop()
00095 {
00096     x->write(def);
00097     y->write(def);
00098 }
00099 // counter clockwise is -
00100 // clockwise is +
00101 void Wheelchair::pid_right(int deg) 
00102 {
00103     float pid_yaw;
00104     bool overturn = false;
00105     
00106     out->printf("pid right\r\r\n");
00107     x->write(def);
00108     Setpoint = curr_yaw + deg;
00109     pid_yaw = curr_yaw;
00110     if(Setpoint > 360) {
00111         overturn = true;
00112     }
00113     myPID.SetTunings(5.5,0, 0.00345);
00114     myPID.SetOutputLimits(0, def - low - .15);
00115     myPID.SetControllerDirection(DIRECT);
00116     
00117     while(pid_yaw < Setpoint - 3){
00118         if(overturn && curr_yaw < Setpoint-deg-1)
00119         {
00120             pid_yaw = curr_yaw + 360;
00121         }   
00122         else
00123             pid_yaw = curr_yaw;
00124         myPID.Compute();
00125         double tempor = -Output+def;
00126         y->write(tempor);
00127         out->printf("curr_yaw %f\r\r\n", curr_yaw);
00128         out->printf("Setpoint = %f \r\n", Setpoint);
00129         wait(.05);
00130         }
00131         
00132     Wheelchair::stop();
00133     out->printf("done \r\n");
00134     }
00135     
00136 void Wheelchair::pid_left(int deg) 
00137 {
00138     float pid_yaw;
00139     bool overturn = false;
00140     
00141     out->printf("pid Left\r\r\n");
00142     x->write(def);
00143     Setpoint = curr_yaw - deg;
00144     pid_yaw = curr_yaw;
00145     if(Setpoint < 0) {
00146         overturn = true;
00147     }
00148     
00149     myPID.SetTunings(5,0, 0.004);
00150     myPID.SetOutputLimits(0,high - def -.12);
00151     myPID.SetControllerDirection(REVERSE);
00152     
00153     while(pid_yaw > Setpoint + 3){
00154        myPID.Compute();
00155        if(overturn && curr_yaw > Setpoint+deg+1)
00156        {
00157           pid_yaw = curr_yaw - 360;
00158         }   
00159         else
00160             pid_yaw = curr_yaw;
00161        double tempor = Output+def;
00162 
00163         y->write(tempor);
00164         out->printf("curr_yaw %f\r\n", curr_yaw);
00165         wait(.05);
00166         }
00167         Wheelchair::stop();
00168     }
00169 
00170 void Wheelchair::pid_turn(int deg) {
00171     if(deg > 180) {
00172         deg -= 360;
00173     }
00174 
00175     else if(deg < -180) {
00176         deg+=360;
00177     }  
00178     
00179     int turnAmt = abs(deg);
00180     ti->reset();
00181 
00182     if(deg >= 0){
00183         Wheelchair::pid_right(turnAmt);
00184         }
00185     else {
00186         Wheelchair::pid_left(turnAmt);
00187         }
00188     }
00189     
00190 double Wheelchair::turn_right(int deg)
00191 {
00192     bool overturn = false;
00193     out->printf("turning right\n");
00194 
00195     double start = curr_yaw;
00196     double final = start + deg;
00197 
00198     if(final > 360) {
00199         final -= 360;
00200         overturn = true;
00201     }
00202 
00203     out->printf("start %f, final %f\n", start, final);
00204 
00205     double curr = -1;
00206     while(curr <= final - 30) {
00207         Wheelchair::right();
00208         if( out->readable()) {
00209             out->printf("stopped\n");
00210             Wheelchair::stop();
00211             return;
00212         }
00213         curr = curr_yaw;
00214         if(overturn && curr > (360 - deg) ) {
00215             curr = 0;
00216         }
00217     }
00218     
00219     out->printf("done turning start %f final %f\n", start, final);
00220     Wheelchair::stop();
00221     
00222      //delete me
00223     wait(5);
00224     
00225     float correction = final - curr_yaw;
00226     out->printf("final pos %f actual pos %f\n", final, curr_yaw);
00227     Wheelchair::turn_left(abs(correction));
00228     Wheelchair::stop();
00229     
00230     wait(5);
00231     out->printf("curr_yaw %f\n", curr_yaw);
00232     return final;
00233 }
00234 
00235 double Wheelchair::turn_left(int deg)
00236 {
00237     bool overturn = false;
00238     out->printf("turning left\n");
00239 
00240     double start = curr_yaw;
00241     double final = start - deg;
00242 
00243     if(final < 0) {
00244         final += 360;
00245         overturn = true;
00246     }
00247 
00248     out->printf("start %f, final %f\n", start, final);
00249 
00250     double curr = 361;
00251     while(curr >= final) {
00252         Wheelchair::left();
00253         if( out->readable()) {
00254             out->printf("stopped\n");
00255             Wheelchair::stop();
00256             return;
00257         }
00258         curr = curr_yaw;
00259 
00260         if(overturn && curr >= 0 && curr <= start ) {
00261             curr = 361;
00262         }
00263     }
00264 
00265     out->printf("done turning start %f final %f\n", start, final);
00266     Wheelchair::stop();
00267     
00268    
00269     return final;
00270 }
00271 
00272 void Wheelchair::turn(int deg)
00273 {
00274     if(deg > 180) {
00275         deg -= 360;
00276     }
00277 
00278     else if(deg < -180) {
00279         deg+=360;
00280     }  
00281     
00282     double finalpos;
00283     int turnAmt = abs(deg);
00284 
00285     
00286     float correction = finalpos - curr_yaw;
00287     out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
00288     
00289     
00290     //if(abs(correction) > turn_precision) {
00291         out->printf("correcting %f\n", correction);
00292         //ti->reset();
00293         Wheelchair::turn_left(curr_yaw - finalpos);
00294         return;
00295         //} 
00296     
00297 }
00298 
00299 float Wheelchair::getDistance() {
00300     return wheel->getDistance(Diameter);
00301     }
00302     
00303 void Wheelchair::resetDistance(){
00304     wheel->reset();
00305     }
00306 
00307 void Wheelchair::turn_on() {
00308     on = 1;
00309     wait(1);
00310     on = 0;
00311     }
00312 
00313 void Wheelchair::turn_off() {
00314     off = 1;
00315     wait(1);
00316     off = 0;
00317     }
00318     
00319 void Wheelchair::pid_forward(double mm)
00320 {
00321     y->write(def + offset);
00322     wheel->reset();
00323     Input = 0;
00324     out->printf("pid foward\r\n");
00325 
00326     
00327     Setpoint = mm-20;
00328 
00329   //  Setpoint = wheel_right.getDistance(37.5)+mm;
00330     myPIDDistance.SetTunings(5,0, 0.004);
00331     myPIDDistance.SetOutputLimits(0,high - def - 0.15);
00332     myPIDDistance.SetControllerDirection(DIRECT);
00333     
00334     while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) {
00335         if(out->readable()){
00336             Wheelchair::stop();
00337             break;}
00338         Input = wheel->getDistance(53.975);
00339         //out->printf("input foward %d\r\n", wheel->getPulses());
00340         wait(.05);
00341         myPIDDistance.Compute();
00342  
00343         x->write(Output + def);
00344         out->printf("distance %f\r\n", Input);
00345         }
00346 
00347 }   
00348 
00349 void Wheelchair::desk() {
00350     Wheelchair::pid_forward(5461);
00351     Wheelchair::pid_right(87);
00352     Wheelchair::pid_forward(3658);
00353     Wheelchair::pid_left(87);
00354     Wheelchair::pid_forward(3734);
00355     }
00356 
00357 void Wheelchair::kitchen() {
00358     Wheelchair::pid_forward(5461);
00359     Wheelchair::pid_right(87);
00360     Wheelchair::pid_forward(3658);
00361     Wheelchair::pid_left(87);
00362     Wheelchair::pid_forward(305);
00363     }
00364     
00365 void Wheelchair::back() {
00366     Wheelchair::pid_right(177);
00367     Wheelchair::pid_forward(3700);
00368     }