1
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: wheelchairControlSumer2019
Diff: wheelchair.cpp
- Revision:
- 17:7f3b69300bb6
- Parent:
- 14:9caca9fde9b0
- Child:
- 18:663b6d693252
--- a/wheelchair.cpp Mon Aug 13 21:54:31 2018 +0000 +++ b/wheelchair.cpp Tue Aug 28 23:24:08 2018 +0000 @@ -1,27 +1,46 @@ #include "wheelchair.h" +Serial qei(D1, D0); bool manual_drive = false; volatile float north; //volatile double curr_yaw; double curr_yaw; -double Setpoint, Output; -PID myPID(&curr_yaw, &Output, &Setpoint, .1, .1, 5, DIRECT); - +double encoder_distance; +char myString[64]; +double Setpoint, Output, Input; +double pid_yaw, Distance, Setpoint2; +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); +//QEI wheel_right(D0, D1, NC, 450); void Wheelchair::compass_thread() { - - //north = lowPass(imu->angle_north()); - //Input = (double)curr_yaw; curr_yaw = imu->yaw(); north = boxcar(imu->angle_north()); - //out->printf("curr_yaw %f\n", curr_yaw); - //out->printf("%f\n", curr_yaw); - //out->printf("%f gyroz\n",imu->gyro_z()); - //out->printf("yaw is %f, north is %f, curr_yaw is %f\n", comp_yn, north, curr_yaw); - - //out->printf("Yaw is %f\n", imu->yaw()); - //out->printf("north is %f\n", 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 ) { @@ -32,7 +51,7 @@ Wheelchair::stop(); imu->setup(); out = pc; - out->printf("wheelchair setup done \n"); + out->printf("wheelchair setup done \r\n"); ti = time; wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate); myPID.SetMode(AUTOMATIC); @@ -53,7 +72,7 @@ x->write(scaled_x); y->write(scaled_y); - //out->printf("yaw %f\n", imu->yaw()); + //out->printf("yaw %f\r\r\n", imu->yaw()); } @@ -61,6 +80,7 @@ { x->write(high); y->write(def+offset); + // out->printf("distance %f\r\n", wheel_right.getDistance(37.5)); } void Wheelchair::backward() @@ -92,45 +112,66 @@ { bool overturn = false; - out->printf("pid right\n"); + out->printf("pid right\r\r\n"); x->write(def); Setpoint = curr_yaw + deg; - + pid_yaw = curr_yaw; if(Setpoint > 360) { - Setpoint -= 360; + // Setpoint -= 360; overturn = true; } - - myPID.SetOutputLimits(low, def); + myPID.SetTunings(5.5,0, 0.00345); + myPID.SetOutputLimits(0, def-low); myPID.SetControllerDirection(REVERSE); - 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) ); - out->printf("addresses %i %i\n", myPID.myInput, &curr_yaw); - while(myPID.Compute()) { - y->write(Output); - out->printf("curr_yaw %f\n", curr_yaw); + while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) { + if(overturn && curr_yaw < Setpoint-deg-1) + { + pid_yaw = curr_yaw + 360; + } + else + pid_yaw = curr_yaw; + myPID.Compute(); + double tempor = Output+low; + y->write(tempor); + out->printf("curr_yaw %f\r\r\n", curr_yaw); + out->printf("Setpoint = %f \r\n", Setpoint); + + wait(.05); } Wheelchair::stop(); - out->printf("done \n"); + out->printf("done \r\n"); } void Wheelchair::pid_left(int deg) { bool overturn = false; + out->printf("pid Left\r\r\n"); x->write(def); Setpoint = curr_yaw - deg; - + pid_yaw = curr_yaw; if(Setpoint < 0) { - Setpoint += 360; + // Setpoint += 360; overturn = true; } - myPID.SetOutputLimits(def,high); - myPID.SetControllerDirection(DIRECT); - while(myPID.Compute()) { - y->write(Output); - out->printf("curr_yaw %f\n", curr_yaw); + myPID.SetTunings(5,0, 0.004); + myPID.SetOutputLimits(0,high-def); + myPID.SetControllerDirection(REVERSE); + while(pid_yaw > Setpoint + 3){//pid_yaw < Setpoint + 2) { + myPID.Compute(); + if(overturn && curr_yaw > Setpoint+deg+1) + { + pid_yaw = curr_yaw - 360; + } + else + pid_yaw = curr_yaw; + double tempor = Output+def; + + y->write(tempor); + out->printf("curr_yaw %f\r\n", curr_yaw); + wait(.05); } - Wheelchair::stop(); + Wheelchair::stop(); } void Wheelchair::pid_turn(int deg) { @@ -152,11 +193,81 @@ Wheelchair::pid_left(turnAmt); } } +void Wheelchair::pid_foward(double mm) +{ + qei.putc('r'); + out->printf("pid foward\r\n"); + + 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); + 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 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(); + + tempor = Output + def; + x->write(tempor); + out->printf("distance %f\r\n", encoder_distance); + } + +} +void Wheelchair::pid_reverse(double mm) +{ + qei.putc('r'); + + /* double tempor; + // Setpoint = wheel_right.getDistance(37.5)+mm; + myPIDDistance.SetTunings(5,0, 0.004); + myPIDDistance.SetOutputLimits(0,high-def); + myPIDDistance.SetControllerDirection(DIRECT); + y->write(def); + while(Distance < Setpoint-5){//pid_yaw < Setpoint + 2) { + tempor = Output + def; + x->write(tempor); + // out->printf("distance %f\r\n", wheel_right.getDistance(37.5)); + // Distance = wheel_right.getDistance(37.5); + wait(.05); + } */ +} double Wheelchair::turn_right(int deg) { bool overturn = false; - out->printf("turning right\n"); + out->printf("turning right\r\n"); double start = curr_yaw; double final = start + deg; @@ -166,13 +277,13 @@ overturn = true; } - out->printf("start %f, final %f\n", start, final); + out->printf("start %f, final %f\r\n", start, final); double curr = -1; while(curr <= final - 30) { Wheelchair::right(); if( out->readable()) { - out->printf("stopped\n"); + out->printf("stopped\r\n"); Wheelchair::stop(); return; } @@ -182,26 +293,26 @@ } } - out->printf("done turning start %f final %f\n", start, final); + out->printf("done turning start %f final %f\r\n", start, final); Wheelchair::stop(); //delete me wait(5); float correction = final - curr_yaw; - out->printf("final pos %f actual pos %f\n", final, curr_yaw); + out->printf("final pos %f actual pos %f\r\n", final, curr_yaw); Wheelchair::turn_left(abs(correction)); Wheelchair::stop(); wait(5); - out->printf("curr_yaw %f\n", curr_yaw); + out->printf("curr_yaw %f\r\n", curr_yaw); return final; } double Wheelchair::turn_left(int deg) { bool overturn = false; - out->printf("turning left\n"); + out->printf("turning left\r\n"); double start = curr_yaw; double final = start - deg; @@ -211,13 +322,13 @@ overturn = true; } - out->printf("start %f, final %f\n", start, final); + out->printf("start %f, final %f\r\n", start, final); double curr = 361; while(curr >= final) { Wheelchair::left(); if( out->readable()) { - out->printf("stopped\n"); + out->printf("stopped\r\n"); Wheelchair::stop(); return; } @@ -228,14 +339,14 @@ } } - out->printf("done turning start %f final %f\n", start, final); + out->printf("done turning start %f final %f\r\n", start, final); Wheelchair::stop(); //delete me wait(2); /* float correction = final - curr_yaw; - out->printf("final pos %f actual pos %f\n", final, curr_yaw); + out->printf("final pos %f actual pos %f\r\n", final, curr_yaw); Wheelchair::turn_right(abs(correction)); Wheelchair::stop(); */ @@ -266,11 +377,11 @@ wait(2); float correction = finalpos - curr_yaw; - out->printf("final pos %f actual pos %f\n", finalpos, curr_yaw); + out->printf("final pos %f actual pos %f\r\n", finalpos, curr_yaw); //if(abs(correction) > turn_precision) { - out->printf("correcting %f\n", correction); + out->printf("correcting %f\r\n", correction); //ti->reset(); Wheelchair::turn_left(curr_yaw - finalpos); return;