for ros
Dependencies: QEI chair_BNO055 pid ros_lib_kinetic
Dependents: wheelchaircontrolrealtimeROS
Fork of wheelchaircontrol by
Revision 21:69df88af7c46, committed 2018-08-31
- Comitter:
- ryanlin97
- Date:
- Fri Aug 31 19:41:42 2018 +0000
- Parent:
- 20:b7a5e4019cae
- Commit message:
- for jesus
Changed in this revision
wheelchair.cpp | Show annotated file Show diff for this revision Revisions of this file |
wheelchair.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/wheelchair.cpp Thu Aug 30 03:44:03 2018 +0000 +++ b/wheelchair.cpp Fri Aug 31 19:41:42 2018 +0000 @@ -33,18 +33,18 @@ return atoi(input); } -Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time ) +Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei ) { x = new PwmOut(xPin); y = new PwmOut(yPin); imu = new chair_BNO055(pc, time); - //imu = new chair_MPU9250(pc, time); + Wheelchair::stop(); imu->setup(); out = pc; out->printf("wheelchair setup done \n"); ti = time; - wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate); + wheel = qei; myPID.SetMode(AUTOMATIC); } @@ -111,8 +111,8 @@ overturn = true; } myPID.SetTunings(5.5,0, 0.00345); - myPID.SetOutputLimits(0, def-low); - myPID.SetControllerDirection(REVERSE); + myPID.SetOutputLimits(0, def - low - .15); + myPID.SetControllerDirection(DIRECT); while(pid_yaw < Setpoint - 3){ if(overturn && curr_yaw < Setpoint-deg-1) @@ -122,7 +122,7 @@ else pid_yaw = curr_yaw; myPID.Compute(); - double tempor = Output+low; + double tempor = -Output+def; y->write(tempor); out->printf("curr_yaw %f\r\r\n", curr_yaw); out->printf("Setpoint = %f \r\n", Setpoint); @@ -147,7 +147,7 @@ } myPID.SetTunings(5,0, 0.004); - myPID.SetOutputLimits(0,high-def); + myPID.SetOutputLimits(0,high - def -.12); myPID.SetControllerDirection(REVERSE); while(pid_yaw > Setpoint + 3){ @@ -318,47 +318,51 @@ void Wheelchair::pid_forward(double mm) { - y->write(def); - qei.putc('r'); + y->write(def + offset); + wheel->reset(); + Input = 0; out->printf("pid foward\r\n"); - double tempor; - Setpoint = mm; + + Setpoint = mm-20; // Setpoint = wheel_right.getDistance(37.5)+mm; myPIDDistance.SetTunings(5,0, 0.004); - myPIDDistance.SetOutputLimits(0,high-def); + myPIDDistance.SetOutputLimits(0,high - def - 0.15); myPIDDistance.SetControllerDirection(DIRECT); - while(encoder_distance < Setpoint-5){//pid_yaw < Setpoint + 2) { - ti->reset(); - - qei.putc('h'); - double curr_dist = readEncoder(); - out->printf("displacement = %f\r\n", curr_dist); - if(abs(curr_dist - encoder_distance) < 500) - { - encoder_distance = curr_dist; - out->printf("this is fine\r\n"); - } - - Input = encoder_distance; - out->printf("input foward %f\r\n", Input); - - wait(process); - + while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) { + if(out->readable()){ + Wheelchair::stop(); + break;} + Input = wheel->getDistance(53.975); + //out->printf("input foward %d\r\n", wheel->getPulses()); + wait(.05); myPIDDistance.Compute(); - out->printf("distance %f\r\n", encoder_distance); - - x->write(Output+def); + + x->write(Output + def); + out->printf("distance %f\r\n", Input); } } +void Wheelchair::desk() { + Wheelchair::pid_forward(5461); + Wheelchair::pid_right(87); + Wheelchair::pid_forward(3658); + Wheelchair::pid_left(87); + Wheelchair::pid_forward(3734); + } + void Wheelchair::kitchen() { Wheelchair::pid_forward(5461); - Wheelchair::pid_right(90); + Wheelchair::pid_right(87); Wheelchair::pid_forward(3658); - Wheelchair::pid_left(90); - Wheelchair::pid_forward(3734); + Wheelchair::pid_left(87); + Wheelchair::pid_forward(305); + } + +void Wheelchair::back() { + Wheelchair::pid_right(177); + Wheelchair::pid_forward(3700); } \ No newline at end of file
--- a/wheelchair.h Thu Aug 30 03:44:03 2018 +0000 +++ b/wheelchair.h Fri Aug 31 19:41:42 2018 +0000 @@ -12,7 +12,7 @@ #define turn_precision 10 #define def (2.5f/3.3f) #define high (3.3f-.15f)/3.3f -#define offset .028f +#define offset .02742f #define low (1.7f/3.3f) #define process .1 @@ -40,7 +40,7 @@ /** Create Wheelchair Object with x,y pin for analog dc output * serial for printout, and timer */ - Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time); + Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei); /** move using the joystick */ void move(float x_coor, float y_coor); @@ -84,8 +84,9 @@ void turn_off(); void pid_forward(double mm); void kitchen(); - void office(); + void desk(); void follow(); + void back(); private: double readEncoder();