wheelchair.cpp

00001 #include "wheelchair.h"
00002 
00003 bool manual_drive = false;
00004 volatile float north;
00005 //volatile double curr_yaw;
00006 double curr_yaw;
00007 double Setpoint, Output;
00008 PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT);
00009 
00010 void Wheelchair::compass_thread() {
00011     
00012      //north = lowPass(imu->angle_north());
00013      //Input = (double)curr_yaw;
00014      curr_yaw = imu->yaw();
00015      north = boxcar(imu->angle_north());
00016     //out->printf("curr_yaw %f\n", curr_yaw);
00017      //out->printf("%f\n", curr_yaw);
00018      //out->printf("%f gyroz\n",imu->gyro_z());
00019      //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw);
00020     
00021         //out->printf("Yaw is %f\n", imu->yaw());
00022         //out->printf("north is %f\n", imu->angle_north());
00023     
00024     }
00025     
00026 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time )
00027 {
00028     x = new PwmOut(xPin);
00029     y = new PwmOut(yPin);
00030     imu = new chair_BNO055(pc, time);
00031     //imu = new chair_MPU9250(pc, time);
00032     Wheelchair::stop();
00033     imu->setup();
00034     out = pc;
00035     out->printf("wheelchair setup done \n");
00036     ti = time;
00037     wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate);
00038     myPID.SetMode(AUTOMATIC);
00039 }
00040 
00041 /*
00042 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
00043 */
00044 void Wheelchair::move(float x_coor, float y_coor)
00045 {
00046 
00047     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;
00048     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;
00049     
00050    // lowPass(scaled_x);
00051     //lowPass(scaled_y);
00052     
00053     x->write(scaled_x);
00054     y->write(scaled_y);
00055     
00056     //out->printf("yaw %f\n", imu->yaw());
00057 
00058 }
00059 
00060 void Wheelchair::forward()
00061 {
00062     x->write(high);
00063     y->write(def+offset);
00064 }
00065 
00066 void Wheelchair::backward()
00067 {
00068     x->write(low);
00069     y->write(def);
00070 }
00071 
00072 void Wheelchair::right()
00073 {
00074     x->write(def);
00075     y->write(low);
00076 }
00077 
00078 void Wheelchair::left()
00079 {
00080     x->write(def);
00081     y->write(high);
00082 }
00083 
00084 void Wheelchair::stop()
00085 {
00086     x->write(def);
00087     y->write(def);
00088 }
00089 // counter clockwise is -
00090 // clockwise is +
00091 void Wheelchair::pid_right(int deg) 
00092 {
00093     bool overturn = false;
00094     
00095     out->printf("pid right\n");
00096     x->write(def);
00097     Setpoint = curr_yaw + deg;
00098     
00099     if(Setpoint > 360) {
00100         Setpoint -= 360;
00101         overturn = true;
00102     }
00103     
00104     myPID.SetOutputLimits(low, def);
00105     myPID.SetControllerDirection(REVERSE);
00106     out->printf("setpoint %f curr_yaw %f input %f output %i  %i same address\n", Setpoint, curr_yaw, *(myPID.myInput), *(myPID.myOutput), (myPID.myInput==&curr_yaw), (myPID.myOutput == &Output) );
00107     out->printf("addresses %i %i\n", myPID.myInput, &curr_yaw);
00108     while(myPID.Compute()) {
00109         y->write(Output);
00110         out->printf("curr_yaw %f\n", curr_yaw);
00111         }
00112     Wheelchair::stop();
00113     out->printf("done \n");
00114     }
00115 
00116 void Wheelchair::pid_left(int deg) 
00117 {
00118     bool overturn = false;
00119     
00120     x->write(def);
00121     Setpoint = curr_yaw - deg;
00122     
00123     if(Setpoint < 0) {
00124         Setpoint += 360;
00125         overturn = true;
00126     }
00127     myPID.SetOutputLimits(def,high);
00128     myPID.SetControllerDirection(DIRECT);
00129     while(myPID.Compute()) {
00130         y->write(Output);
00131         out->printf("curr_yaw %f\n", curr_yaw);
00132         }
00133     Wheelchair::stop();
00134     }
00135 
00136 void Wheelchair::pid_turn(int deg) {
00137     if(deg > 180) {
00138         deg -= 360;
00139     }
00140 
00141     else if(deg < -180) {
00142         deg+=360;
00143     }  
00144     
00145     int turnAmt = abs(deg);
00146     ti->reset();
00147 
00148     if(deg >= 0){
00149         Wheelchair::pid_right(turnAmt);
00150         }
00151     else {
00152         Wheelchair::pid_left(turnAmt);
00153         }
00154     }
00155     
00156 double Wheelchair::turn_right(int deg)
00157 {
00158     bool overturn = false;
00159     out->printf("turning right\n");
00160 
00161     double start = curr_yaw;
00162     double final = start + deg;
00163 
00164     if(final > 360) {
00165         final -= 360;
00166         overturn = true;
00167     }
00168 
00169     out->printf("start %f, final %f\n", start, final);
00170 
00171     double curr = -1;
00172     while(curr <= final - 30) {
00173         Wheelchair::right();
00174         if( out->readable()) {
00175             out->printf("stopped\n");
00176             Wheelchair::stop();
00177             return;
00178         }
00179         curr = curr_yaw;
00180         if(overturn && curr > (360 - deg) ) {
00181             curr = 0;
00182         }
00183     }
00184     
00185     out->printf("done turning start %f final %f\n", start, final);
00186     Wheelchair::stop();
00187     
00188      //delete me
00189     wait(5);
00190     
00191     float correction = final - curr_yaw;
00192     out->printf("final pos %f actual pos %f\n", final, curr_yaw);
00193     Wheelchair::turn_left(abs(correction));
00194     Wheelchair::stop();
00195     
00196     wait(5);
00197     out->printf("curr_yaw %f\n", curr_yaw);
00198     return final;
00199 }
00200 
00201 double Wheelchair::turn_left(int deg)
00202 {
00203     bool overturn = false;
00204     out->printf("turning left\n");
00205 
00206     double start = curr_yaw;
00207     double final = start - deg;
00208 
00209     if(final < 0) {
00210         final += 360;
00211         overturn = true;
00212     }
00213 
00214     out->printf("start %f, final %f\n", start, final);
00215 
00216     double curr = 361;
00217     while(curr >= final) {
00218         Wheelchair::left();
00219         if( out->readable()) {
00220             out->printf("stopped\n");
00221             Wheelchair::stop();
00222             return;
00223         }
00224         curr = curr_yaw;
00225 
00226         if(overturn && curr >= 0 && curr <= start ) {
00227             curr = 361;
00228         }
00229     }
00230 
00231     out->printf("done turning start %f final %f\n", start, final);
00232     Wheelchair::stop();
00233     
00234     //delete me
00235     wait(2);
00236     /*
00237     float correction = final - curr_yaw;
00238     out->printf("final pos %f actual pos %f\n", final, curr_yaw);
00239     Wheelchair::turn_right(abs(correction));
00240     Wheelchair::stop();
00241 */
00242     return final;
00243 }
00244 
00245 void Wheelchair::turn(int deg)
00246 {
00247     if(deg > 180) {
00248         deg -= 360;
00249     }
00250 
00251     else if(deg < -180) {
00252         deg+=360;
00253     }  
00254     
00255     double finalpos;
00256     int turnAmt = abs(deg);
00257     //ti->reset();
00258     /*
00259     if(deg >= 0){
00260         finalpos = Wheelchair::turn_right(turnAmt);
00261         }
00262     else {
00263         finalpos = Wheelchair::turn_left(turnAmt);
00264         }
00265     */
00266     wait(2);
00267     
00268     float correction = finalpos - curr_yaw;
00269     out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw);
00270     
00271     
00272     //if(abs(correction) > turn_precision) {
00273         out->printf("correcting %f\n", correction);
00274         //ti->reset();
00275         Wheelchair::turn_left(curr_yaw - finalpos);
00276         return;
00277         //} 
00278     
00279 }
00280 
00281 float Wheelchair::getDistance() {
00282     return wheel->getDistance(Diameter);
00283     }
00284     
00285 void Wheelchair::resetDistance(){
00286     wheel->reset();
00287     }
00288 
00289