Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@24:d2f234fbc20d, 2018-10-27 (annotated)

Committer:
jvfausto
Date:
Sat Oct 27 10:10:14 2018 +0000
Revision:
24:d2f234fbc20d
Parent:
23:8d11d953ceeb
Child:
25:58ec657a44f2
PID values work for the chair

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 11:d14a1f7f1297 2
jvfausto 23:8d11d953ceeb 3 bool manual_drive = false; // Variable Changes between joystick and auto drive
jvfausto 24:d2f234fbc20d 4 double curr_yaw, curr_vel, curr_velS; // Variable that contains current relative angle
jvfausto 23:8d11d953ceeb 5 double encoder_distance; // Keeps distanse due to original position
jvfausto 19:71a6621ee5c3 6
jvfausto 23:8d11d953ceeb 7 volatile double Setpoint, Output, Input, Input2; // Variables for PID
jvfausto 23:8d11d953ceeb 8 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID
jvfausto 24:d2f234fbc20d 9 volatile double vIn, vOut, vDesired;
jvfausto 24:d2f234fbc20d 10 volatile double vInS, vOutS, vDesiredS;
jvfausto 24:d2f234fbc20d 11 volatile double yIn, yOut, yDesired;
jvfausto 19:71a6621ee5c3 12
jvfausto 23:8d11d953ceeb 13 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor
jvfausto 23:8d11d953ceeb 14 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor
jvfausto 24:d2f234fbc20d 15 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT);
jvfausto 24:d2f234fbc20d 16 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);
jvfausto 24:d2f234fbc20d 17 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);
jvfausto 23:8d11d953ceeb 18
jvfausto 23:8d11d953ceeb 19 void Wheelchair::compass_thread() { // Thread that measures which angle we are at
ryanlin97 11:d14a1f7f1297 20 curr_yaw = imu->yaw();
ryanlin97 11:d14a1f7f1297 21 }
jvfausto 24:d2f234fbc20d 22 void Wheelchair::velosity_thread() {
jvfausto 24:d2f234fbc20d 23 curr_vel = wheel->getVelosity();
jvfausto 24:d2f234fbc20d 24 curr_velS = wheelS->getVelosity();
jvfausto 24:d2f234fbc20d 25 }
ryanlin97 11:d14a1f7f1297 26
jvfausto 24:d2f234fbc20d 27 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS) // Function Constructor for Wheelchair class
ryanlin97 1:c0beadca1617 28 {
jvfausto 23:8d11d953ceeb 29 //Initializes X and Y variables to Pins
jvfausto 23:8d11d953ceeb 30 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 31 y = new PwmOut(yPin);
jvfausto 23:8d11d953ceeb 32
jvfausto 23:8d11d953ceeb 33 // Initializes IMU Library
ryanlin97 11:d14a1f7f1297 34 imu = new chair_BNO055(pc, time);
jvfausto 23:8d11d953ceeb 35 Wheelchair::stop(); // Wheelchair is not moving when initializing
jvfausto 23:8d11d953ceeb 36 imu->setup(); // turns on the IMU
jvfausto 23:8d11d953ceeb 37 out = pc; // "out" is called for serial monitor
jvfausto 24:d2f234fbc20d 38 wheelS = qeiS; // "wheel" is called for encoder
jvfausto 24:d2f234fbc20d 39 wheel = qei;
jvfausto 23:8d11d953ceeb 40 out->printf("wheelchair setup done \r\n"); // make sure it initialized
ryanlin97 11:d14a1f7f1297 41 ti = time;
jvfausto 23:8d11d953ceeb 42 myPID.SetMode(AUTOMATIC); // set PID to automatic
ryanlin97 1:c0beadca1617 43 }
ryanlin97 6:0cd57bdd8fbc 44
jvfausto 23:8d11d953ceeb 45 void Wheelchair::move(float x_coor, float y_coor) // moves the chair with joystick on manual
ryanlin97 1:c0beadca1617 46 {
ryanlin97 6:0cd57bdd8fbc 47
jvfausto 23:8d11d953ceeb 48 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; // Scales one joystic measurement to the
jvfausto 23:8d11d953ceeb 49 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; // chair's joystic measurement
ryanlin97 11:d14a1f7f1297 50
jvfausto 23:8d11d953ceeb 51 x->write(scaled_x); // Sends the scaled joystic values to the chair
ryanlin97 4:29a27953fe70 52 y->write(scaled_y);
ryanlin97 5:e0ccaab3959a 53 }
ryanlin97 11:d14a1f7f1297 54
jvfausto 23:8d11d953ceeb 55 void Wheelchair::forward() // In auto to move foward
ryanlin97 1:c0beadca1617 56 {
ryanlin97 0:fc0c4a184482 57 x->write(high);
ryanlin97 3:a5e71bfdb492 58 y->write(def+offset);
ryanlin97 0:fc0c4a184482 59 }
ryanlin97 0:fc0c4a184482 60
jvfausto 23:8d11d953ceeb 61 void Wheelchair::backward() // In auto to move reverse
ryanlin97 1:c0beadca1617 62 {
ryanlin97 0:fc0c4a184482 63 x->write(low);
ryanlin97 0:fc0c4a184482 64 y->write(def);
ryanlin97 0:fc0c4a184482 65 }
ryanlin97 0:fc0c4a184482 66
jvfausto 23:8d11d953ceeb 67 void Wheelchair::right() // In auto to move right
ryanlin97 1:c0beadca1617 68 {
ryanlin97 0:fc0c4a184482 69 x->write(def);
ryanlin97 11:d14a1f7f1297 70 y->write(low);
ryanlin97 0:fc0c4a184482 71 }
ryanlin97 0:fc0c4a184482 72
jvfausto 23:8d11d953ceeb 73 void Wheelchair::left() // In auto to move left
ryanlin97 1:c0beadca1617 74 {
ryanlin97 0:fc0c4a184482 75 x->write(def);
ryanlin97 11:d14a1f7f1297 76 y->write(high);
ryanlin97 0:fc0c4a184482 77 }
ryanlin97 0:fc0c4a184482 78
jvfausto 23:8d11d953ceeb 79 void Wheelchair::stop() // Stops the chair
ryanlin97 1:c0beadca1617 80 {
ryanlin97 0:fc0c4a184482 81 x->write(def);
ryanlin97 0:fc0c4a184482 82 y->write(def);
ryanlin97 6:0cd57bdd8fbc 83 }
ryanlin97 11:d14a1f7f1297 84 // counter clockwise is -
ryanlin97 11:d14a1f7f1297 85 // clockwise is +
jvfausto 23:8d11d953ceeb 86 void Wheelchair::pid_right(int deg) // Takes in degree and turns right
ryanlin97 12:921488918749 87 {
jvfausto 23:8d11d953ceeb 88 bool overturn = false; //Boolean if we have to turn over relative 360˚
ryanlin97 12:921488918749 89
jvfausto 23:8d11d953ceeb 90 out->printf("pid right\r\r\n");
jvfausto 23:8d11d953ceeb 91 x->write(def); // Not moving fowards or reverse
jvfausto 23:8d11d953ceeb 92 Setpoint = curr_yaw + deg; // Relative angle we want to turn
jvfausto 23:8d11d953ceeb 93 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
jvfausto 23:8d11d953ceeb 94
jvfausto 23:8d11d953ceeb 95 if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚
ryanlin97 12:921488918749 96 overturn = true;
ryanlin97 12:921488918749 97 }
jvfausto 23:8d11d953ceeb 98
jvfausto 23:8d11d953ceeb 99 myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D
jvfausto 23:8d11d953ceeb 100 myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low
jvfausto 23:8d11d953ceeb 101 myPID.SetControllerDirection(DIRECT); // PID mode Direct
jvfausto 23:8d11d953ceeb 102
jvfausto 23:8d11d953ceeb 103 while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching
jvfausto 23:8d11d953ceeb 104 // a little less than desired angle
jvfausto 23:8d11d953ceeb 105 if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary
jvfausto 17:7f3b69300bb6 106 {
jvfausto 17:7f3b69300bb6 107 pid_yaw = curr_yaw + 360;
jvfausto 17:7f3b69300bb6 108 }
jvfausto 17:7f3b69300bb6 109 else
jvfausto 17:7f3b69300bb6 110 pid_yaw = curr_yaw;
jvfausto 23:8d11d953ceeb 111
jvfausto 23:8d11d953ceeb 112 myPID.Compute(); // Does PID calculations
jvfausto 23:8d11d953ceeb 113 double tempor = -Output+def; // Temporary value with the voltage output
jvfausto 23:8d11d953ceeb 114 y->write(tempor); // Sends to chair y output command
jvfausto 23:8d11d953ceeb 115
jvfausto 23:8d11d953ceeb 116 out->printf("curr_yaw %f\r\r\n", curr_yaw);
jvfausto 17:7f3b69300bb6 117 out->printf("Setpoint = %f \r\n", Setpoint);
jvfausto 17:7f3b69300bb6 118
jvfausto 23:8d11d953ceeb 119 wait(.05); // Small delay
ryanlin97 12:921488918749 120 }
jvfausto 23:8d11d953ceeb 121 Wheelchair::stop(); // Safety Stop
jvfausto 17:7f3b69300bb6 122 out->printf("done \r\n");
jvfausto 23:8d11d953ceeb 123 }
ryanlin97 6:0cd57bdd8fbc 124
jvfausto 23:8d11d953ceeb 125 void Wheelchair::pid_left(int deg) // Takes in degree and turns left
ryanlin97 12:921488918749 126 {
jvfausto 23:8d11d953ceeb 127 bool overturn = false; //Boolean if we have to turn under relative 0˚
ryanlin97 12:921488918749 128
jvfausto 23:8d11d953ceeb 129 out->printf("pid Left\r\r\n");
jvfausto 23:8d11d953ceeb 130 x->write(def); // Not moving fowards or reverse
jvfausto 23:8d11d953ceeb 131 Setpoint = curr_yaw - deg; // Relative angle we want to turn
jvfausto 23:8d11d953ceeb 132 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
jvfausto 23:8d11d953ceeb 133 if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚
ryanlin97 12:921488918749 134 overturn = true;
ryanlin97 12:921488918749 135 }
jvfausto 23:8d11d953ceeb 136 myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D
jvfausto 23:8d11d953ceeb 137 myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def
jvfausto 23:8d11d953ceeb 138 myPID.SetControllerDirection(REVERSE); // PID mode Reverse
jvfausto 23:8d11d953ceeb 139 while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching
jvfausto 23:8d11d953ceeb 140 // a little more than desired angle
jvfausto 23:8d11d953ceeb 141 if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary
jvfausto 17:7f3b69300bb6 142 {
jvfausto 17:7f3b69300bb6 143 pid_yaw = curr_yaw - 360;
jvfausto 17:7f3b69300bb6 144 }
jvfausto 17:7f3b69300bb6 145 else
jvfausto 17:7f3b69300bb6 146 pid_yaw = curr_yaw;
jvfausto 23:8d11d953ceeb 147
jvfausto 23:8d11d953ceeb 148 myPID.Compute(); // Does PID calculations
jvfausto 23:8d11d953ceeb 149 double tempor = Output+def; // Temporary value with the voltage output
jvfausto 23:8d11d953ceeb 150 y->write(tempor); // Sends to chair y output command
jvfausto 23:8d11d953ceeb 151
jvfausto 17:7f3b69300bb6 152 out->printf("curr_yaw %f\r\n", curr_yaw);
jvfausto 23:8d11d953ceeb 153 wait(.05); // Small Delay
ryanlin97 12:921488918749 154 }
jvfausto 23:8d11d953ceeb 155 Wheelchair::stop(); // Safety Stop
jvfausto 23:8d11d953ceeb 156 }
ryanlin97 12:921488918749 157
jvfausto 23:8d11d953ceeb 158 void Wheelchair::pid_turn(int deg) { // Determine wether we are turn right or left
jvfausto 23:8d11d953ceeb 159
jvfausto 23:8d11d953ceeb 160 if(deg > 180) { // If deg > 180 turn left: coterminal angle
ryanlin97 12:921488918749 161 deg -= 360;
ryanlin97 12:921488918749 162 }
ryanlin97 12:921488918749 163
jvfausto 23:8d11d953ceeb 164 else if(deg < -180) { // If deg < -180 turn right: coterminal angle
ryanlin97 12:921488918749 165 deg+=360;
ryanlin97 12:921488918749 166 }
ryanlin97 12:921488918749 167
jvfausto 23:8d11d953ceeb 168 int turnAmt = abs(deg); // Makes sure input angle is positive
ryanlin97 12:921488918749 169
ryanlin97 12:921488918749 170 if(deg >= 0){
jvfausto 23:8d11d953ceeb 171 Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree
jvfausto 23:8d11d953ceeb 172 }
ryanlin97 12:921488918749 173 else {
jvfausto 23:8d11d953ceeb 174 Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree
ryanlin97 12:921488918749 175 }
jvfausto 23:8d11d953ceeb 176 }
jvfausto 19:71a6621ee5c3 177 void Wheelchair::pid_forward(double mm)
jvfausto 17:7f3b69300bb6 178 {
jvfausto 23:8d11d953ceeb 179 mm -= 20; // Makes sure distance does not overshoot
jvfausto 23:8d11d953ceeb 180 Input = 0; // Initializes imput to cero: Test latter w/o
jvfausto 23:8d11d953ceeb 181 wheel->reset(); // Resets encoders so that they start at 0
jvfausto 17:7f3b69300bb6 182 out->printf("pid foward\r\n");
jvfausto 17:7f3b69300bb6 183
jvfausto 23:8d11d953ceeb 184 double tempor; // Initializes Temporary variable for x input
jvfausto 23:8d11d953ceeb 185 Setpoint = mm; // Initializes the setpoint to desired value
jvfausto 17:7f3b69300bb6 186
jvfausto 23:8d11d953ceeb 187 myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D
jvfausto 23:8d11d953ceeb 188 myPIDDistance.SetOutputLimits(0,high-def-.15); // Limits to the differnce between High and Def
jvfausto 23:8d11d953ceeb 189 myPIDDistance.SetControllerDirection(DIRECT); // PID to Direct
jvfausto 23:8d11d953ceeb 190 y->write(def+offset); // Sets chair to not turn
jvfausto 23:8d11d953ceeb 191
jvfausto 23:8d11d953ceeb 192 while(Input < Setpoint){ // Stop moving when reaching setpoint
jvfausto 23:8d11d953ceeb 193
jvfausto 23:8d11d953ceeb 194 if(out->readable()) // Emergency Break
jvfausto 19:71a6621ee5c3 195 break;
jvfausto 23:8d11d953ceeb 196
jvfausto 23:8d11d953ceeb 197 Input = wheel->getDistance(53.975); // Gets Distance from Encoder onto PID
jvfausto 23:8d11d953ceeb 198 wait(.05); // Slight Delay: *****Test without
jvfausto 23:8d11d953ceeb 199 myPIDDistance.Compute(); // Compute Output for chair
jvfausto 17:7f3b69300bb6 200
jvfausto 23:8d11d953ceeb 201 tempor = Output + def; // Temporary output variable
jvfausto 23:8d11d953ceeb 202 x->write(tempor); // Sends to chair x output
jvfausto 19:71a6621ee5c3 203 out->printf("distance %f\r\n", Input);
jvfausto 17:7f3b69300bb6 204 }
ryanlin97 12:921488918749 205
jvfausto 17:7f3b69300bb6 206 }
jvfausto 17:7f3b69300bb6 207 void Wheelchair::pid_reverse(double mm)
jvfausto 17:7f3b69300bb6 208 {
jvfausto 17:7f3b69300bb6 209
jvfausto 17:7f3b69300bb6 210 }
jvfausto 24:d2f234fbc20d 211 void Wheelchair::pid_twistA()
jvfausto 24:d2f234fbc20d 212 {
jvfausto 24:d2f234fbc20d 213 char c;
jvfausto 24:d2f234fbc20d 214 double temporA = def;
jvfausto 24:d2f234fbc20d 215 y->write(def);
jvfausto 24:d2f234fbc20d 216 x->write(def);
ryanlin97 8:381a4ec3fef8 217
jvfausto 24:d2f234fbc20d 218 PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D
jvfausto 24:d2f234fbc20d 219 PIDAngularV.SetOutputLimits(-.1, .1); // Limits to the differnce between def and low
jvfausto 24:d2f234fbc20d 220 PIDAngularV.SetControllerDirection(DIRECT); // PID mode Direct
jvfausto 24:d2f234fbc20d 221 while(1)
jvfausto 24:d2f234fbc20d 222 {
jvfausto 24:d2f234fbc20d 223 if(out->readable())
jvfausto 24:d2f234fbc20d 224 c = out->getc();
jvfausto 24:d2f234fbc20d 225 if(c == '0')
jvfausto 24:d2f234fbc20d 226 {
jvfausto 24:d2f234fbc20d 227 yDesired = 0;
jvfausto 24:d2f234fbc20d 228 }
jvfausto 24:d2f234fbc20d 229 if(c == '1')
jvfausto 24:d2f234fbc20d 230 yDesired = 30;
jvfausto 24:d2f234fbc20d 231 if(c == '2')
jvfausto 24:d2f234fbc20d 232 yDesired = 90;
jvfausto 24:d2f234fbc20d 233 if(c == '9')
jvfausto 24:d2f234fbc20d 234 yDesired = -30;
jvfausto 24:d2f234fbc20d 235 if(c == '8')
jvfausto 24:d2f234fbc20d 236 yDesired = -90;
jvfausto 24:d2f234fbc20d 237 if(c == 'r')
jvfausto 24:d2f234fbc20d 238 {
jvfausto 24:d2f234fbc20d 239 yDesired = 0;
jvfausto 24:d2f234fbc20d 240 return;
jvfausto 24:d2f234fbc20d 241 }
jvfausto 24:d2f234fbc20d 242
jvfausto 24:d2f234fbc20d 243 yIn = imu->gyro_z();
jvfausto 24:d2f234fbc20d 244 PIDAngularV.Compute();
jvfausto 24:d2f234fbc20d 245 temporA += yOut; // Temporary value with the voltage output
jvfausto 24:d2f234fbc20d 246 y->write(temporA);
jvfausto 24:d2f234fbc20d 247 out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
jvfausto 24:d2f234fbc20d 248 wait(.05);
jvfausto 24:d2f234fbc20d 249 }
jvfausto 24:d2f234fbc20d 250 }
jvfausto 24:d2f234fbc20d 251 void Wheelchair::pid_twistV()
jvfausto 24:d2f234fbc20d 252 {
jvfausto 24:d2f234fbc20d 253 double temporV = def;
jvfausto 24:d2f234fbc20d 254 double temporS = def;
jvfausto 24:d2f234fbc20d 255 vDesiredS = 0;
jvfausto 24:d2f234fbc20d 256 char c;
jvfausto 24:d2f234fbc20d 257 x->write(def);
jvfausto 24:d2f234fbc20d 258 y->write(def);
jvfausto 24:d2f234fbc20d 259
jvfausto 24:d2f234fbc20d 260 PIDVelosity.SetTunings(.00005,0, 0.00); // Sets the constants for P and D
jvfausto 24:d2f234fbc20d 261 PIDSlaveV.SetTunings(.007,0.000001, 0.000001); // Sets the constants for P and D
jvfausto 24:d2f234fbc20d 262 PIDVelosity.SetOutputLimits(-.005, .005); // Limits to the differnce between def and low
jvfausto 24:d2f234fbc20d 263 PIDSlaveV.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low
jvfausto 24:d2f234fbc20d 264 PIDVelosity.SetControllerDirection(DIRECT);
jvfausto 24:d2f234fbc20d 265 PIDSlaveV.SetControllerDirection(DIRECT);
jvfausto 24:d2f234fbc20d 266 while(1)
jvfausto 24:d2f234fbc20d 267 {
jvfausto 24:d2f234fbc20d 268 if(out->readable())
jvfausto 24:d2f234fbc20d 269 c = out->getc();
jvfausto 24:d2f234fbc20d 270 if(c == '0')
jvfausto 24:d2f234fbc20d 271 vDesired = 0;
jvfausto 24:d2f234fbc20d 272 if(c == '3')
jvfausto 24:d2f234fbc20d 273 vDesired = 100;
jvfausto 24:d2f234fbc20d 274 if(c == '4')
jvfausto 24:d2f234fbc20d 275 vDesired = 150;
jvfausto 24:d2f234fbc20d 276 if(c == '7')
jvfausto 24:d2f234fbc20d 277 vDesired = -50;
jvfausto 24:d2f234fbc20d 278 if(c == '6')
jvfausto 24:d2f234fbc20d 279 vDesired = -100;
jvfausto 24:d2f234fbc20d 280 if(c == 'r')
jvfausto 24:d2f234fbc20d 281 {
jvfausto 24:d2f234fbc20d 282 yDesired = 0;
jvfausto 24:d2f234fbc20d 283 return;
jvfausto 24:d2f234fbc20d 284 }
jvfausto 24:d2f234fbc20d 285 if(vDesired >= 0)
jvfausto 24:d2f234fbc20d 286 {
jvfausto 24:d2f234fbc20d 287 PIDVelosity.SetTunings(.00001,0, 0.00); // Sets the constants for P and D
jvfausto 24:d2f234fbc20d 288 PIDVelosity.SetOutputLimits(-.003, .003); // Limits to the differnce between def and low
jvfausto 24:d2f234fbc20d 289 }
jvfausto 24:d2f234fbc20d 290 else
jvfausto 24:d2f234fbc20d 291 {
jvfausto 24:d2f234fbc20d 292 PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D
jvfausto 24:d2f234fbc20d 293 PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to the differnce between def and low
jvfausto 24:d2f234fbc20d 294 }
jvfausto 24:d2f234fbc20d 295 if(temporV >= 1)
jvfausto 24:d2f234fbc20d 296 temporV = 1;
jvfausto 24:d2f234fbc20d 297 vIn = curr_vel*100;
jvfausto 24:d2f234fbc20d 298 vInS = curr_vel-curr_velS;
jvfausto 24:d2f234fbc20d 299 PIDVelosity.Compute();
jvfausto 24:d2f234fbc20d 300 PIDSlaveV.Compute();
jvfausto 24:d2f234fbc20d 301 temporV += vOut;
jvfausto 24:d2f234fbc20d 302 temporS += vOutS;
jvfausto 24:d2f234fbc20d 303 x->write(temporV);
jvfausto 24:d2f234fbc20d 304 y->write(temporS);
jvfausto 24:d2f234fbc20d 305 out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
jvfausto 24:d2f234fbc20d 306 wait(.01);
jvfausto 24:d2f234fbc20d 307 }
jvfausto 24:d2f234fbc20d 308 }
ryanlin97 12:921488918749 309 float Wheelchair::getDistance() {
ryanlin97 12:921488918749 310 return wheel->getDistance(Diameter);
ryanlin97 12:921488918749 311 }
ryanlin97 12:921488918749 312
ryanlin97 12:921488918749 313 void Wheelchair::resetDistance(){
ryanlin97 12:921488918749 314 wheel->reset();
ryanlin97 12:921488918749 315 }
jvfausto 23:8d11d953ceeb 316 /*Predetermined paths For Demmo*/
jvfausto 19:71a6621ee5c3 317 void Wheelchair::desk() {
jvfausto 19:71a6621ee5c3 318 Wheelchair::pid_forward(5461);
jvfausto 19:71a6621ee5c3 319 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 320 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 321 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 322 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 323 }
ryanlin97 10:e5463c11e0a0 324
jvfausto 19:71a6621ee5c3 325 void Wheelchair::kitchen() {
jvfausto 19:71a6621ee5c3 326 Wheelchair::pid_forward(5461);
jvfausto 20:f42db4ae16f0 327 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 328 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 329 Wheelchair::pid_left(90);
jvfausto 19:71a6621ee5c3 330 Wheelchair::pid_forward(305);
jvfausto 19:71a6621ee5c3 331 }
ryanlin97 12:921488918749 332
jvfausto 19:71a6621ee5c3 333 void Wheelchair::desk_to_kitchen(){
jvfausto 19:71a6621ee5c3 334 Wheelchair::pid_right(180);
jvfausto 19:71a6621ee5c3 335 Wheelchair::pid_forward(3700);
jvfausto 19:71a6621ee5c3 336 }
jvfausto 19:71a6621ee5c3 337