for ros
Dependencies: QEI chair_BNO055 pid ros_lib_kinetic
Dependents: wheelchaircontrolrealtimeROS
Fork of wheelchaircontrol by
Diff: wheelchair.cpp
- Revision:
- 19:df4257c75ed0
- Parent:
- 18:3eadf01ec1b0
- Child:
- 21:69df88af7c46
--- a/wheelchair.cpp Mon Aug 27 04:48:16 2018 +0000 +++ b/wheelchair.cpp Wed Aug 29 21:01:51 2018 +0000 @@ -1,16 +1,38 @@ #include "wheelchair.h" +Serial qei(D1, D0); + +DigitalOut on(D12); +DigitalOut off(D11); +DigitalOut up(D2); +DigitalOut down(D3); bool manual_drive = false; -volatile float north; -//volatile double curr_yaw; +double encoder_distance; + double curr_yaw; -double Setpoint, Output; +double Setpoint, Output, Input; PID myPID(&curr_yaw, &Output, &Setpoint, 5.5, 0, .0036, P_ON_E, DIRECT); +PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); void Wheelchair::compass_thread() { curr_yaw = imu->yaw(); } +double Wheelchair::readEncoder() { + char input[64] = {0}; + for (int i=0; input[i-1] != '\n'; i++) { + while (true) { + //pc.printf("%f\r\n", ti.read()); + if (ti->read() > .02) break; + if (qei.readable()) { + input[i]= qei.getc(); + break; + } + } + } + return atoi(input); + } + Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time ) { x = new PwmOut(xPin); @@ -282,4 +304,61 @@ wheel->reset(); } +void Wheelchair::turn_on() { + on = 1; + wait(1); + on = 0; + } +void Wheelchair::turn_off() { + off = 1; + wait(1); + off = 0; + } + +void Wheelchair::pid_forward(double mm) +{ + y->write(def); + qei.putc('r'); + out->printf("pid foward\r\n"); + + double tempor; + Setpoint = mm; + + // Setpoint = wheel_right.getDistance(37.5)+mm; + myPIDDistance.SetTunings(5,0, 0.004); + myPIDDistance.SetOutputLimits(0,high-def); + 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); + + myPIDDistance.Compute(); + out->printf("distance %f\r\n", encoder_distance); + + x->write(Output+def); + } + +} + +void Wheelchair::kitchen() { + Wheelchair::pid_forward(5461); + Wheelchair::pid_right(90); + Wheelchair::pid_forward(3658); + Wheelchair::pid_left(90); + Wheelchair::pid_forward(3734); + } \ No newline at end of file