Added Back ToF code
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-6 Version1-7
Diff: wheelchair.cpp
- Revision:
- 18:663b6d693252
- Parent:
- 17:7f3b69300bb6
- Child:
- 19:71a6621ee5c3
diff -r 7f3b69300bb6 -r 663b6d693252 wheelchair.cpp --- a/wheelchair.cpp Tue Aug 28 23:24:08 2018 +0000 +++ b/wheelchair.cpp Wed Aug 29 16:51:33 2018 +0000 @@ -7,10 +7,11 @@ double curr_yaw; double encoder_distance; char myString[64]; -double Setpoint, Output, Input; -double pid_yaw, Distance, Setpoint2; +double Setpoint, Output, Input, Input2; +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(&encoder_distance, &Output, &Setpoint2, 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); //QEI wheel_right(D0, D1, NC, 450); void Wheelchair::compass_thread() { curr_yaw = imu->yaw(); @@ -223,13 +224,13 @@ } } myString[i-1] = 0; - double tempor = atof(myString); + 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; @@ -249,20 +250,85 @@ void Wheelchair::pid_reverse(double mm) { qei.putc('r'); + out->printf("pid reverse\r\n"); - /* double tempor; + double tempor; + Setpoint2 = mm; + // Setpoint = wheel_right.getDistance(37.5)+mm; myPIDDistance.SetTunings(5,0, 0.004); - myPIDDistance.SetOutputLimits(0,high-def); - myPIDDistance.SetControllerDirection(DIRECT); + myPIDDistance.SetOutputLimits(0,def); + myPIDDistance.SetControllerDirection(REVERSE); y->write(def); - while(Distance < Setpoint-5){//pid_yaw < Setpoint + 2) { - tempor = Output + 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 tempor = 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); + myPIDDistance.Compute(); + + qei.putc('k'); + //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", tempor2); + + if(abs(tempor - encoder_distance) < 500) + { + encoder_distance = tempor; + out->printf("this is fine\r\n"); + } + if(abs(tempor - encoder_distance2) < 500) + { + encoder_distance = tempor; + out->printf("this is fine\r\n"); + } + for(i = 0; i < 64; i++) + { + myString[i] = 0; + } + tempor = Output; x->write(tempor); - // out->printf("distance %f\r\n", wheel_right.getDistance(37.5)); - // Distance = wheel_right.getDistance(37.5); - wait(.05); - } */ + out->printf("distance %f\r\n", encoder_distance); + } } double Wheelchair::turn_right(int deg) {