wheelchair class
Dependencies: QEI chair_BNO055 pid ros_lib_kinetic
Dependents: wheelchaircontrolrealtime
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
Generated on Sat Jul 16 2022 09:09:16 by 1.7.2