Updated with Statistics Library
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 19:71a6621ee5c3
- Parent:
- 18:663b6d693252
- Child:
- 20:f42db4ae16f0
diff -r 663b6d693252 -r 71a6621ee5c3 wheelchair.cpp --- a/wheelchair.cpp Wed Aug 29 16:51:33 2018 +0000 +++ b/wheelchair.cpp Fri Aug 31 17:09:56 2018 +0000 @@ -1,5 +1,4 @@ #include "wheelchair.h" -Serial qei(D1, D0); bool manual_drive = false; volatile float north; @@ -7,43 +6,21 @@ double curr_yaw; double encoder_distance; char myString[64]; -double Setpoint, Output, Input, Input2; -double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; +volatile double Setpoint, Output, Input, Input2; +volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); -PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); -PID myPIDDistance2(&Input2, &Output2, &Setpoint2, 5.5, .00, 0.0036, P_ON_E, DIRECT); +PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0025, P_ON_E, DIRECT); + + +//PID myPIDDistance2(&Input2, &Output2, &Setpoint2, 5.5, .00, 0.0036, P_ON_E, DIRECT); //QEI wheel_right(D0, D1, NC, 450); void Wheelchair::compass_thread() { curr_yaw = imu->yaw(); north = boxcar(imu->angle_north()); } -void Wheelchair::distance_thread() { -/* int i; - qei.putc('h'); - //qei.gets(myString, 10); - ti->reset(); - - for (i=0; myString[i-1] != '\n'; i++) { - while (true) { - //pc.printf("%f\r\n", ti.read()); - if (ti->read() > .02) break; - if (qei.readable()) { - myString[i]= qei.getc(); - break; - } - } - } - myString[i-1] = 0; - encoder_distance = atof(myString); - //out->printf("displacement = %f\r\n", encoder_distance); - for(i = 0; i < 64; i++) - { - myString[i] = 0; - } */ -} -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); @@ -52,9 +29,10 @@ Wheelchair::stop(); imu->setup(); out = pc; + wheel = qei; out->printf("wheelchair setup done \r\n"); ti = time; - wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate); + //wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate); myPID.SetMode(AUTOMATIC); } @@ -121,9 +99,9 @@ // Setpoint -= 360; overturn = true; } - myPID.SetTunings(5.5,0, 0.00345); - myPID.SetOutputLimits(0, def-low); - myPID.SetControllerDirection(REVERSE); + myPID.SetTunings(5.5,0, 0.0035); + myPID.SetOutputLimits(0, def-low-.15); + myPID.SetControllerDirection(DIRECT); while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) { if(overturn && curr_yaw < Setpoint-deg-1) { @@ -132,7 +110,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); @@ -156,9 +134,9 @@ overturn = true; } 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){//pid_yaw < Setpoint + 2) { + while(pid_yaw > Setpoint+3){//pid_yaw < Setpoint + 2) { myPID.Compute(); if(overturn && curr_yaw > Setpoint+deg+1) { @@ -194,62 +172,39 @@ Wheelchair::pid_left(turnAmt); } } -void Wheelchair::pid_foward(double mm) +void Wheelchair::pid_forward(double mm) { - qei.putc('r'); + mm -= 20; + Input = 0; + wheel->reset(); out->printf("pid foward\r\n"); double tempor; - Setpoint2 = mm; + Setpoint = mm; // Setpoint = wheel_right.getDistance(37.5)+mm; myPIDDistance.SetTunings(5,0, 0.004); - myPIDDistance.SetOutputLimits(0,high-def); + myPIDDistance.SetOutputLimits(0,high-def-.15); myPIDDistance.SetControllerDirection(DIRECT); - y->write(def); - while(encoder_distance < Setpoint2-5){//pid_yaw < Setpoint + 2) { - int i; - qei.putc('h'); - //qei.gets(myString, 10); - ti->reset(); - - for (i=0; myString[i-1] != '\n'; i++) { - while (true) { - //pc.printf("%f\r\n", ti.read()); - if (ti->read() > .02) break; - if (qei.readable()) { - myString[i]= qei.getc(); - break; - } - } - } - myString[i-1] = 0; - double tempor2 = atof(myString); - out->printf("displacement = %f\r\n", tempor); - if(abs(tempor - encoder_distance) < 500) - { - encoder_distance = tempor; - out->printf("this is fine\r\n"); - } - for(i = 0; i < 64; i++) - { - myString[i] = 0; - } - Input = encoder_distance; - out->printf("input foward %f\r\n", Input); - wait(.1); + y->write(def+offset); + while(Input < Setpoint-5){//pid_yaw < Setpoint + 2) { + if(out->readable()) + break; + Input = wheel->getDistance(53.975); + //out->printf("input foward %d\r\n", wheel->getPulses()); + wait(.05); myPIDDistance.Compute(); tempor = Output + def; x->write(tempor); - out->printf("distance %f\r\n", encoder_distance); + out->printf("distance %f\r\n", Input); } } void Wheelchair::pid_reverse(double mm) { - qei.putc('r'); + /* qei.putc('r'); out->printf("pid reverse\r\n"); double tempor; @@ -293,8 +248,8 @@ wait(.1); myPIDDistance.Compute(); + // get value from encoder2 qei.putc('k'); - //qei.gets(myString, 10); ti->reset(); for (i=0; myString[i-1] != '\n'; i++) { @@ -316,9 +271,9 @@ encoder_distance = tempor; out->printf("this is fine\r\n"); } - if(abs(tempor - encoder_distance2) < 500) + if(abs(tempor2 - encoder_distance2) < 500) { - encoder_distance = tempor; + encoder_distance2 = tempor2; out->printf("this is fine\r\n"); } for(i = 0; i < 64; i++) @@ -328,7 +283,7 @@ tempor = Output; x->write(tempor); out->printf("distance %f\r\n", encoder_distance); - } + }*/ } double Wheelchair::turn_right(int deg) { @@ -462,5 +417,24 @@ void Wheelchair::resetDistance(){ wheel->reset(); } +void Wheelchair::desk() { + Wheelchair::pid_forward(5461); + Wheelchair::pid_right(87); + Wheelchair::pid_forward(3658); + Wheelchair::pid_right(87); + Wheelchair::pid_forward(3658); + } +void Wheelchair::kitchen() { + Wheelchair::pid_forward(5461); + Wheelchair::pid_right(85); + Wheelchair::pid_forward(3658); + Wheelchair::pid_left(90); + Wheelchair::pid_forward(305); + } +void Wheelchair::desk_to_kitchen(){ + Wheelchair::pid_right(180); + Wheelchair::pid_forward(3700); + } +