Added more code for side and angled sensor
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
Diff: wheelchair.cpp
- Revision:
- 21:3489cffad196
- Parent:
- 20:f42db4ae16f0
- Child:
- 26:662693bd7f31
diff -r f42db4ae16f0 -r 3489cffad196 wheelchair.cpp --- a/wheelchair.cpp Fri Aug 31 20:00:01 2018 +0000 +++ b/wheelchair.cpp Fri Apr 19 23:04:01 2019 +0000 @@ -1,440 +1,516 @@ + #include "wheelchair.h" + +bool manual_drive = false; // Variable changes between joystick and auto drive +double encoder_distance; // Keeps distanse due to original position + +volatile double Setpoint, Output, Input, Input2; // Variables for PID +volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID +volatile double vIn, vOut, vDesired; // Variables for PID Velosity +volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel +volatile double yIn, yOut, yDesired; // Variables for PID turn velosity -bool manual_drive = false; -volatile float north; -//volatile double curr_yaw; -double curr_yaw; -double encoder_distance; -char myString[64]; -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.0025, P_ON_E, DIRECT); +double dist_old, curr_pos; // Variables for odometry position + +PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor +PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor +PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Velosity PID Constructor +PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); // Slave Velosity PID Constructor +PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Angular Velosity PID Constructor + -//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() { +/* Thread measures current angular position */ +void Wheelchair::compass_thread() +{ curr_yaw = imu->yaw(); - north = boxcar(imu->angle_north()); + z_angular = curr_yaw; +} - } - -Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei) +/* Thread measures velocity of wheels and distance traveled */ +void Wheelchair::velocity_thread() { - 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; - wheel = qei; - out->printf("wheelchair setup done \r\n"); - ti = time; - //wheel = new QEI(Encoder1, Encoder2, NC, EncoderReadRate); - myPID.SetMode(AUTOMATIC); + curr_vel = wheel->getVelocity(); + curr_velS = wheelS->getVelocity(); + curr_pos = wheel->getDistance(53.975); } -/* -* joystick has analog out of 200-700, scale values between 1.3 and 3.3 -*/ -void Wheelchair::move(float x_coor, float y_coor) +void Wheelchair::assistSafe_thread() { + int ToFV[12]; + for(int i = 0; i < 9; i++) // reads from the ToF Sensors + { + ToFV[i] = (*(ToF+i))->readFromOneSensor(); + //out->printf("%d ", ToFV[i]); + } + out->printf("\r\n"); + int sensor1 = ToFV[2]; + int sensor4 = ToFV[5]; + out->printf("%d, %d\r\n", sensor1, sensor4); + if(((2 * maxDeceleration*sensor1 < curr_vel*curr_vel*1000*1000 || + 2 * maxDeceleration*sensor4 < curr_vel*curr_vel*1000*1000) && + (sensor1 < 1500 || sensor4 < 1500)) || + 550 > sensor1 || 550 > sensor4) + { + //out->printf("i am in danger\r\n"); + if(x->read() > def) + { + x->write(def); + forwardSafety = 1; + } + } + else + forwardSafety = 0; + +} +/* Constructor for Wheelchair class */ +Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, +VL53L1X** ToFT) +{ + x_position = 0; + y_position = 0; + forwardSafety = 0; + /* Initializes X and Y variables to Pins */ + x = new PwmOut(xPin); + y = new PwmOut(yPin); + /* Initializes IMU Library */ + imu = new chair_BNO055(pc, time); + Wheelchair::stop(); // Wheelchair is initially stationary + imu->setup(); // turns on the IMU + out = pc; // "out" is called for serial monitor + wheelS = qeiS; // "wheel" is called for encoder + wheel = qei; + ToF = ToFT; // passes pointer with addresses of ToF sensors + + for(int i = 0; i < 12; i++) // initializes the ToF Sensors + { + (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); + } + + out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor + ti = time; + + + myPID.SetMode(AUTOMATIC); // PID mode: Automatic +} + +/* Move wheelchair with joystick on manual mode */ +void Wheelchair::move(float x_coor, float y_coor) +{ + /* Scales one joystick measurement to the chair's joystick measurement */ float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; - - // lowPass(scaled_x); - //lowPass(scaled_y); - - x->write(scaled_x); + + /* Sends the scaled joystic values to the chair */ + x->write(scaled_x); y->write(scaled_y); - - //out->printf("yaw %f\r\r\n", imu->yaw()); - } - -void Wheelchair::forward() + +/* Automatic mode: move forward and update x,y coordinate sent to chair */ +void Wheelchair::forward() { + if(forwardSafety == 0) + { x->write(high); y->write(def+offset); - // out->printf("distance %f\r\n", wheel_right.getDistance(37.5)); + } } - -void Wheelchair::backward() + +/* Automatic mode: move in reverse and update x,y coordinate sent to chair */ +void Wheelchair::backward() { x->write(low); y->write(def); } - -void Wheelchair::right() + +/* Automatic mode: move right and update x,y coordinate sent to chair */ +void Wheelchair::right() { x->write(def); y->write(low); } -void Wheelchair::left() + /* Automatic mode: move left and update x,y coordinate sent to chair */ +void Wheelchair::left() { x->write(def); y->write(high); } - -void Wheelchair::stop() + +/* Stop the wheelchair */ +void Wheelchair::stop() { x->write(def); y->write(def); } -// counter clockwise is - -// clockwise is + -void Wheelchair::pid_right(int deg) + +/* Counter-clockwise is - + * Clockwise is + + * Range of deg: 0 to 360 + * This constructor takes in an angle from user and adjusts for turning right + */ +void Wheelchair::pid_right(int deg) { - bool overturn = false; + bool overturn = false; //Boolean if angle over 360˚ - out->printf("pid right\r\r\n"); - x->write(def); - Setpoint = curr_yaw + deg; - pid_yaw = curr_yaw; - if(Setpoint > 360) { - // Setpoint -= 360; + out->printf("pid right\r\r\n"); + x->write(def); // Update x sent to chair to be stationary + Setpoint = curr_yaw + deg; // Relative angle we want to turn + pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user + + /* Turns on overturn boolean if setpoint over 360˚ */ + if(Setpoint > 360) + { overturn = true; } - myPID.SetTunings(5.5,0, 0.0035); - myPID.SetOutputLimits(0, def-low-.15); - myPID.SetControllerDirection(DIRECT); - while(pid_yaw < Setpoint - 3){//curr_yaw <= Setpoint) { + + myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D + myPID.SetOutputLimits(0, def-low-.15); // Limit is set to the differnce between def and low + myPID.SetControllerDirection(DIRECT); // PID mode: Direct + + /* PID stops when approaching a litte less than desired angle */ + while(pid_yaw < Setpoint - 3) + { + /* PID is set to correct angle range if angle greater than 360˚*/ if(overturn && curr_yaw < Setpoint-deg-1) { - pid_yaw = curr_yaw + 360; - } - else + pid_yaw = curr_yaw + 360; + } + else + { pid_yaw = curr_yaw; - myPID.Compute(); - double tempor = -Output+def; - 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 \r\n"); + + myPID.Compute(); // Does PID calculations + double tempor = -Output+def; // Temporary value with the voltage output + y->write(tempor); // Update y sent to chair + + /* Prints to serial monitor the current angle and setpoint */ + out->printf("curr_yaw %f\r\r\n", curr_yaw); + out->printf("Setpoint = %f \r\n", Setpoint); + + wait(.05); // Small delay (milliseconds) } - -void Wheelchair::pid_left(int deg) + + /* Saftey stop for wheelchair */ + Wheelchair::stop(); + out->printf("done \r\n"); +} + +/* Counter-clockwise is - + * Clockwise is + + * Range of deg: 0 to 360 + * This constructor takes in an angle from user and adjusts for turning left + */ +void Wheelchair::pid_left(int deg) { - bool overturn = false; + bool overturn = false; //Boolean if angle under 0˚ - out->printf("pid Left\r\r\n"); - x->write(def); - Setpoint = curr_yaw - deg; - pid_yaw = curr_yaw; - if(Setpoint < 0) { - // Setpoint += 360; + out->printf("pid Left\r\r\n"); + x->write(def); // Update x sent to chair to be stationary + Setpoint = curr_yaw - deg; // Relative angle we want to turn + pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user + + /* Turns on overturn boolean if setpoint less than 0˚ */ + if(Setpoint < 0) + { overturn = true; } - myPID.SetTunings(5,0, 0.004); - myPID.SetOutputLimits(0,high-def-.12); - myPID.SetControllerDirection(REVERSE); - while(pid_yaw > Setpoint+3){//pid_yaw < Setpoint + 2) { - myPID.Compute(); - if(overturn && curr_yaw > Setpoint+deg+1) + + myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D + myPID.SetOutputLimits(0,high-def-.12); //Limit is set to the differnce between def and low + myPID.SetControllerDirection(REVERSE); // PID mode: Reverse + + /* PID stops when approaching a litte more than desired angle */ + while(pid_yaw > Setpoint+3) + { + /* PID is set to correct angle range if angle less than 0˚ */ + if(overturn && curr_yaw > Setpoint+deg+1) { pid_yaw = curr_yaw - 360; - } - else - pid_yaw = curr_yaw; - double tempor = Output+def; - - y->write(tempor); + } + else + { + pid_yaw = curr_yaw; + } + + myPID.Compute(); // Does PID calculations + double tempor = Output+def; // Temporary value with the voltage output + y->write(tempor); // Update y sent to chair + + /* Prints to serial monitor the current angle and setpoint */ out->printf("curr_yaw %f\r\n", curr_yaw); - wait(.05); - } - Wheelchair::stop(); + out->printf("Setpoint = %f \r\n", Setpoint); + + wait(.05); // Small delay (milliseconds) } + + /* Saftey stop for wheelchair */ + Wheelchair::stop(); + out->printf("done \r\n"); -void Wheelchair::pid_turn(int deg) { - if(deg > 180) { +} + +/* This constructor determines whether to turn left or right */ +void Wheelchair::pid_turn(int deg) +{ + + /* Sets angle to coterminal angle for left turn if deg > 180 + * Sets angle to coterminal angle for right turn if deg < -180 + */ + if(deg > 180) + { deg -= 360; } - - else if(deg < -180) { - deg+=360; + else if(deg < -180) + { + deg +=360; } + /* Makes sure angle inputted to function is positive */ int turnAmt = abs(deg); - ti->reset(); + + /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */ + if(deg >= 0) + { + Wheelchair::pid_right(turnAmt); + } + else + { + Wheelchair::pid_left(turnAmt); + } - if(deg >= 0){ - Wheelchair::pid_right(turnAmt); - } - else { - Wheelchair::pid_left(turnAmt); - } - } +} + +/* This constructor takes in distance to travel and adjust to move forward */ void Wheelchair::pid_forward(double mm) { - mm -= 20; - Input = 0; - wheel->reset(); + mm -= 20; // Makes sure distance does not overshoot + Input = 0; // Initializes input to zero: Test latter w/o + wheel->reset(); // Resets encoders so that they start at 0 + out->printf("pid foward\r\n"); - - double tempor; - Setpoint = mm; + + double tempor; // Initializes Temporary variable for x input + Setpoint = mm; // Initializes the setpoint to desired value + + myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D + myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def + myPIDDistance.SetControllerDirection(DIRECT); // PID mode: Direct + + y->write(def+offset); // Update y to make chair stationary + + /* Chair stops moving when Setpoint is reached */ + while(Input < Setpoint){ + + if(out->readable()) // Emergency Break + { + break; + } - // Setpoint = wheel_right.getDistance(37.5)+mm; - myPIDDistance.SetTunings(5,0, 0.004); - myPIDDistance.SetOutputLimits(0,high-def-.15); - myPIDDistance.SetControllerDirection(DIRECT); - 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(); + Input = wheel->getDistance(53.975); // Gets distance from Encoder into PID + wait(.05); // Slight Delay: *****Test without + myPIDDistance.Compute(); // Compute distance traveled by chair + + tempor = Output + def; // Temporary output variable + x->write(tempor); // Update x sent to chair - tempor = Output + def; - x->write(tempor); + /* Prints to serial monitor the distance traveled by chair */ out->printf("distance %f\r\n", Input); } - } -void Wheelchair::pid_reverse(double mm) + +/* This constructor returns the relative angular position of chair */ +double Wheelchair::getTwistZ() { - /* qei.putc('r'); - out->printf("pid reverse\r\n"); - - double tempor; - Setpoint2 = mm; - - // Setpoint = wheel_right.getDistance(37.5)+mm; - myPIDDistance.SetTunings(5,0, 0.004); - myPIDDistance.SetOutputLimits(0,def); - myPIDDistance.SetControllerDirection(REVERSE); - 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(); - - // get value from encoder2 - qei.putc('k'); - ti->reset(); + return imu->gyro_z(); +} - 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(tempor2 - encoder_distance2) < 500) - { - encoder_distance2 = tempor2; - out->printf("this is fine\r\n"); - } - for(i = 0; i < 64; i++) +/* This constructor computes the relative angle for Twist message in ROS */ +void Wheelchair::pid_twistA() +{ + /* Initialize variables for angle and update x,y sent to chair */ + char c; + double temporA = def; + y->write(def); + x->write(def); + + PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D + PIDAngularV.SetOutputLimits(-.1, .1); // Limit set to be in range specified + PIDAngularV.SetControllerDirection(DIRECT); // PID mode: Direct + + /* Computes angular position of wheelchair while turning */ + while(1) + { + yDesired = angularV; + + /* Update and set all variable so that the chair is stationary + * if the desired angle is zero + */ + if(yDesired == 0) { - myString[i] = 0; - } - tempor = Output; - x->write(tempor); - out->printf("distance %f\r\n", encoder_distance); - }*/ -} -double Wheelchair::turn_right(int deg) -{ - bool overturn = false; - out->printf("turning right\r\n"); - - double start = curr_yaw; - double final = start + deg; - - if(final > 360) { - final -= 360; - overturn = true; - } - - 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\r\n"); - Wheelchair::stop(); + x->write(def); + y->write(def); + yDesired = 0; return; } - curr = curr_yaw; - if(overturn && curr > (360 - deg) ) { - curr = 0; - } + + /* Continuously updates with current angle measured by IMU */ + yIn = imu->gyro_z(); + PIDAngularV.Compute(); + temporA += yOut; // Temporary value with the voltage output + y->write(temporA); // Update y sent to chair + + //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); + wait(.05); // Small delay (milliseconds) } - - 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\r\n", final, curr_yaw); - Wheelchair::turn_left(abs(correction)); - Wheelchair::stop(); - - wait(5); - out->printf("curr_yaw %f\r\n", curr_yaw); - return final; -} + +} -double Wheelchair::turn_left(int deg) +/* This constructor computes the relative velocity for Twist message in ROS */ +void Wheelchair::pid_twistV() { - bool overturn = false; - out->printf("turning left\r\n"); - - double start = curr_yaw; - double final = start - deg; + /* Initializes variables as default */ + double temporV = def; + double temporS = def; + vDesiredS = 0; + x->write(def); + y->write(def); + wheel->reset(); // Resets the encoders + /* Sets the constants for P and D */ + PIDVelosity.SetTunings(.0005,0, 0.00); + PIDSlaveV.SetTunings(.01,0.000001, 0.000001); + + /* Limits to the range specified */ + PIDVelosity.SetOutputLimits(-.005, .005); + PIDSlaveV.SetOutputLimits(-.002, .002); + + /* PID mode: Direct */ + PIDVelosity.SetControllerDirection(DIRECT); + PIDSlaveV.SetControllerDirection(DIRECT); + + while(1) + { + linearV = .7; + test1 = linearV*100; + vel = curr_vel; + vDesired = linearV*100; + if(out->readable()) + return; + /* Update and set all variable so that the chair is stationary + * if the velocity is zero + */ + if(linearV == 0) + { + x->write(def); + y->write(def); - if(final < 0) { - final += 360; - overturn = true; - } - - out->printf("start %f, final %f\r\n", start, final); - - double curr = 361; - while(curr >= final) { - Wheelchair::left(); - if( out->readable()) { - out->printf("stopped\r\n"); - Wheelchair::stop(); + vel = 0; + vDesired = 0; + dist_old = 0; return; } - curr = curr_yaw; - - if(overturn && curr >= 0 && curr <= start ) { - curr = 361; + + if(vDesired >= 0) + { + PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D + PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the range specified + } + else + { + PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D + PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to range specified + } + + /* Sets maximum value of variable to 1 */ + if(temporV >= 1.5) + { + temporV = 1.5; } + /* Scales and makes some adjustments to velocity */ + vIn = curr_vel*100; + vInS = curr_vel-curr_velS; + PIDVelosity.Compute(); + PIDSlaveV.Compute(); + if(forwardSafety == 0) + { + temporV += vOut; + temporS += vOutS; + + /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ + x->write(temporV); + test2 = temporV; + y->write(temporS); + } + else + { + x->write(def); + y->write(def); + } + //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS); + Wheelchair::odomMsg(); + wait(.01); // Small delay (milliseconds) } - - 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\r\n", final, curr_yaw); - Wheelchair::turn_right(abs(correction)); - Wheelchair::stop(); -*/ - return final; } -void Wheelchair::turn(int deg) +/* This constructor calculates the relative position of the chair everytime the encoders reset + * by setting its old position as the origin to calculate the new position + */ +void Wheelchair::odomMsg() { - if(deg > 180) { - deg -= 360; - } - - else if(deg < -180) { - deg+=360; - } + double dist_new = curr_pos; + double dist = dist_new-dist_old; + double temp_x = dist*sin(z_angular*3.14159/180); + double temp_y = dist*cos(z_angular*3.14159/180); + + x_position += temp_x; + y_position += temp_y; - double finalpos; - int turnAmt = abs(deg); - //ti->reset(); - /* - if(deg >= 0){ - finalpos = Wheelchair::turn_right(turnAmt); - } - else { - finalpos = Wheelchair::turn_left(turnAmt); - } - */ - wait(2); - - float correction = 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\r\n", correction); - //ti->reset(); - Wheelchair::turn_left(curr_yaw - finalpos); - return; - //} - + dist_old = dist_new; + } + +/* This constructor prints the Odometry message to the serial monitor */ + void Wheelchair::showOdom() + { + out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); + } + +/* This constructor returns the approximate distance based on the wheel diameter */ +float Wheelchair::getDistance() +{ + return wheel->getDistance(Diameter); } -float Wheelchair::getDistance() { - return wheel->getDistance(Diameter); - } - -void Wheelchair::resetDistance(){ +/* This constructor resets the wheel encoder's */ +void Wheelchair::resetDistance() +{ wheel->reset(); - } -void Wheelchair::desk() { +} + + +/*Predetermined paths For Demmo*/ +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() { +} + +void Wheelchair::kitchen() +{ Wheelchair::pid_forward(5461); Wheelchair::pid_right(87); Wheelchair::pid_forward(3658); Wheelchair::pid_left(90); Wheelchair::pid_forward(305); - } - -void Wheelchair::desk_to_kitchen(){ +} + +void Wheelchair::desk_to_kitchen() +{ Wheelchair::pid_right(180); Wheelchair::pid_forward(3700); - } - +} \ No newline at end of file