Added more code for side and angled sensor
Dependencies: QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic
wheelchair.cpp@43:9858a580b3d7, 2019-08-13 (annotated)
- Committer:
- sepham
- Date:
- Tue Aug 13 19:22:08 2019 +0000
- Revision:
- 43:9858a580b3d7
- Parent:
- 42:62c49ad06707
Angled side sensor, side sensor more code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JesiMiranda | 35:5a2fed4c2e9f | 1 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 2 | * Importing header into wheelchair.cpp * |
JesiMiranda | 35:5a2fed4c2e9f | 3 | **************************************************************************/ |
ryanlin97 | 0:fc0c4a184482 | 4 | #include "wheelchair.h" |
t1jain | 30:c25b2556e84d | 5 | |
JesiMiranda | 35:5a2fed4c2e9f | 6 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 7 | * Defining global variables * |
JesiMiranda | 35:5a2fed4c2e9f | 8 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 9 | bool manual_drive = false; // Variable changes between joystick and auto drive |
jvfausto | 21:3489cffad196 | 10 | double encoder_distance; // Keeps distanse due to original position |
JesiMiranda | 35:5a2fed4c2e9f | 11 | |
jvfausto | 21:3489cffad196 | 12 | volatile double Setpoint, Output, Input, Input2; // Variables for PID |
jvfausto | 21:3489cffad196 | 13 | volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID |
jvfausto | 21:3489cffad196 | 14 | volatile double vIn, vOut, vDesired; // Variables for PID Velosity |
jvfausto | 21:3489cffad196 | 15 | volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel |
jvfausto | 21:3489cffad196 | 16 | volatile double yIn, yOut, yDesired; // Variables for PID turn velosity |
t1jain | 30:c25b2556e84d | 17 | // int* ToFDataPointer1; |
t1jain | 30:c25b2556e84d | 18 | // int* ToFDataPointer2; |
t1jain | 30:c25b2556e84d | 19 | |
JesiMiranda | 35:5a2fed4c2e9f | 20 | int ledgeArrayLF[150]; |
JesiMiranda | 35:5a2fed4c2e9f | 21 | int ledgeArrayRF[150]; |
t1jain | 30:c25b2556e84d | 22 | int* ToFDataPointer1 = ledgeArrayLF; |
t1jain | 30:c25b2556e84d | 23 | int* ToFDataPointer2 = ledgeArrayRF; |
JesiMiranda | 35:5a2fed4c2e9f | 24 | Statistics LFTStats(ToFDataPointer1, 149, 1); |
JesiMiranda | 35:5a2fed4c2e9f | 25 | Statistics RFTStats(ToFDataPointer2, 149, 1); |
t1jain | 40:11b8567c507b | 26 | int k = 0; |
t1jain | 38:cb87f4e353b9 | 27 | |
t1jain | 38:cb87f4e353b9 | 28 | int ledgeArrayLB[150]; |
t1jain | 38:cb87f4e353b9 | 29 | int ledgeArrayRB[150]; |
t1jain | 38:cb87f4e353b9 | 30 | int* ToFDataPointer3 = ledgeArrayLB; |
t1jain | 38:cb87f4e353b9 | 31 | int* ToFDataPointer4 = ledgeArrayRB; |
t1jain | 38:cb87f4e353b9 | 32 | Statistics LBTStats(ToFDataPointer3, 149, 1); |
t1jain | 38:cb87f4e353b9 | 33 | Statistics RBTStats(ToFDataPointer4, 149, 1); |
ryanlin97 | 11:d14a1f7f1297 | 34 | |
jvfausto | 21:3489cffad196 | 35 | double dist_old, curr_pos; // Variables for odometry position |
t1jain | 30:c25b2556e84d | 36 | double outlierToF[4]; |
jvfausto | 19:71a6621ee5c3 | 37 | |
JesiMiranda | 35:5a2fed4c2e9f | 38 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 39 | * Creating PID objects * |
JesiMiranda | 35:5a2fed4c2e9f | 40 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 41 | PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor |
jvfausto | 21:3489cffad196 | 42 | PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor |
jvfausto | 21:3489cffad196 | 43 | PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Velosity PID Constructor |
jvfausto | 21:3489cffad196 | 44 | PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); // Slave Velosity PID Constructor |
jvfausto | 21:3489cffad196 | 45 | PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Angular Velosity PID Constructor |
JesiMiranda | 35:5a2fed4c2e9f | 46 | |
jvfausto | 19:71a6621ee5c3 | 47 | |
JesiMiranda | 35:5a2fed4c2e9f | 48 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 49 | * Thread measures current angular position * |
JesiMiranda | 35:5a2fed4c2e9f | 50 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 51 | void Wheelchair::compass_thread() |
JesiMiranda | 35:5a2fed4c2e9f | 52 | { |
JesiMiranda | 35:5a2fed4c2e9f | 53 | curr_yaw = imu->yaw(); |
JesiMiranda | 35:5a2fed4c2e9f | 54 | z_angular = curr_yaw; |
jvfausto | 21:3489cffad196 | 55 | } |
jvfausto | 17:7f3b69300bb6 | 56 | |
JesiMiranda | 35:5a2fed4c2e9f | 57 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 58 | * Thread measures velocity of wheels and distance traveled * |
JesiMiranda | 35:5a2fed4c2e9f | 59 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 60 | void Wheelchair::velocity_thread() |
ryanlin97 | 1:c0beadca1617 | 61 | { |
jvfausto | 21:3489cffad196 | 62 | curr_vel = wheel->getVelocity(); |
jvfausto | 21:3489cffad196 | 63 | curr_velS = wheelS->getVelocity(); |
jvfausto | 21:3489cffad196 | 64 | curr_pos = wheel->getDistance(53.975); |
ryanlin97 | 1:c0beadca1617 | 65 | } |
ryanlin97 | 6:0cd57bdd8fbc | 66 | |
t1jain | 34:b89967adc86c | 67 | void Wheelchair::emergencyButton_thread () |
t1jain | 34:b89967adc86c | 68 | { |
t1jain | 34:b89967adc86c | 69 | while(1) { |
t1jain | 34:b89967adc86c | 70 | while(!e_button) { |
t1jain | 34:b89967adc86c | 71 | |
t1jain | 34:b89967adc86c | 72 | //Stop wheelchair |
t1jain | 34:b89967adc86c | 73 | Wheelchair::stop(); |
t1jain | 34:b89967adc86c | 74 | printf("E-button has been pressed\r\n"); |
t1jain | 34:b89967adc86c | 75 | off->write(high); // Turn off PCB |
t1jain | 34:b89967adc86c | 76 | on->write(0); // Make sure PCB not on |
t1jain | 34:b89967adc86c | 77 | //Reset Board |
t1jain | 34:b89967adc86c | 78 | NVIC_SystemReset(); |
t1jain | 34:b89967adc86c | 79 | |
t1jain | 34:b89967adc86c | 80 | } |
t1jain | 34:b89967adc86c | 81 | |
t1jain | 34:b89967adc86c | 82 | } |
t1jain | 34:b89967adc86c | 83 | } |
t1jain | 34:b89967adc86c | 84 | |
JesiMiranda | 35:5a2fed4c2e9f | 85 | /************************************************************************* |
JesiMiranda | 37:e0e6d3fe06a2 | 86 | * Thread checks ToF sensors for safety of wheelchair movement * |
JesiMiranda | 35:5a2fed4c2e9f | 87 | **************************************************************************/ |
t1jain | 30:c25b2556e84d | 88 | void Wheelchair::ToFSafe_thread() |
ryanlin97 | 1:c0beadca1617 | 89 | { |
jvfausto | 21:3489cffad196 | 90 | int ToFV[12]; |
JesiMiranda | 35:5a2fed4c2e9f | 91 | for(int i = 0; i < 6; i++) { // Reads from the ToF Sensors |
jvfausto | 21:3489cffad196 | 92 | ToFV[i] = (*(ToF+i))->readFromOneSensor(); |
t1jain | 32:fb26baa75d44 | 93 | //out->printf("%d ", ToFV[i]); |
JesiMiranda | 35:5a2fed4c2e9f | 94 | } |
JesiMiranda | 35:5a2fed4c2e9f | 95 | |
JesiMiranda | 35:5a2fed4c2e9f | 96 | //out->printf("\r\n"); |
JesiMiranda | 35:5a2fed4c2e9f | 97 | |
t1jain | 40:11b8567c507b | 98 | k++; |
JesiMiranda | 35:5a2fed4c2e9f | 99 | |
t1jain | 40:11b8567c507b | 100 | if (k == 150) { |
t1jain | 40:11b8567c507b | 101 | k = 1; |
t1jain | 30:c25b2556e84d | 102 | } |
JesiMiranda | 35:5a2fed4c2e9f | 103 | |
t1jain | 40:11b8567c507b | 104 | ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); |
t1jain | 40:11b8567c507b | 105 | ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); |
JesiMiranda | 35:5a2fed4c2e9f | 106 | |
t1jain | 41:1a687bcf4b0a | 107 | /* for(int i = 0; i < 100; i++) |
t1jain | 31:06f2362caf12 | 108 | { |
t1jain | 31:06f2362caf12 | 109 | out->printf("%d, ",ledgeArrayRF[i]); |
t1jain | 31:06f2362caf12 | 110 | } |
t1jain | 41:1a687bcf4b0a | 111 | out->printf("\r\n"); */ |
t1jain | 34:b89967adc86c | 112 | |
t1jain | 30:c25b2556e84d | 113 | outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); |
JesiMiranda | 35:5a2fed4c2e9f | 114 | outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); |
JesiMiranda | 35:5a2fed4c2e9f | 115 | |
t1jain | 42:62c49ad06707 | 116 | for(int i = 0; i < 2; i++) { // Reads from the ToF Sensors |
jvfausto | 27:da718b990837 | 117 | runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); |
t1jain | 30:c25b2556e84d | 118 | } |
JesiMiranda | 35:5a2fed4c2e9f | 119 | |
jvfausto | 27:da718b990837 | 120 | int sensor1 = ToFV[0]; |
jvfausto | 27:da718b990837 | 121 | int sensor4 = ToFV[3]; |
JesiMiranda | 35:5a2fed4c2e9f | 122 | if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || |
JesiMiranda | 35:5a2fed4c2e9f | 123 | 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && |
JesiMiranda | 35:5a2fed4c2e9f | 124 | (sensor1 < 1500 || sensor4 < 1500)) || |
JesiMiranda | 35:5a2fed4c2e9f | 125 | 550 > sensor1 || 550 > sensor4) { |
JesiMiranda | 35:5a2fed4c2e9f | 126 | if(x->read() > def) { |
jvfausto | 26:662693bd7f31 | 127 | x->write(def); |
t1jain | 30:c25b2556e84d | 128 | forwardSafety = 1; // You cannot move forward |
jvfausto | 26:662693bd7f31 | 129 | } |
jvfausto | 26:662693bd7f31 | 130 | } |
JesiMiranda | 35:5a2fed4c2e9f | 131 | |
JesiMiranda | 35:5a2fed4c2e9f | 132 | else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || |
JesiMiranda | 35:5a2fed4c2e9f | 133 | 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && |
JesiMiranda | 35:5a2fed4c2e9f | 134 | (sensor1 < 1500 || sensor4 < 1500)) || |
JesiMiranda | 35:5a2fed4c2e9f | 135 | 550 > sensor1 || 550 > sensor4) { |
JesiMiranda | 35:5a2fed4c2e9f | 136 | if(x->read() > def) { |
jvfausto | 21:3489cffad196 | 137 | x->write(def); |
t1jain | 34:b89967adc86c | 138 | forwardSafety = 1; // You cannot move forward |
jvfausto | 21:3489cffad196 | 139 | } |
jvfausto | 21:3489cffad196 | 140 | } |
JesiMiranda | 35:5a2fed4c2e9f | 141 | |
t1jain | 30:c25b2556e84d | 142 | else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) { |
t1jain | 30:c25b2556e84d | 143 | forwardSafety = 1; |
t1jain | 38:cb87f4e353b9 | 144 | out->printf("I'M STOPPING BECAUSE OF A FRONT LEDGE\r\n"); |
JesiMiranda | 35:5a2fed4c2e9f | 145 | } |
JesiMiranda | 35:5a2fed4c2e9f | 146 | |
jvfausto | 21:3489cffad196 | 147 | else |
jvfausto | 21:3489cffad196 | 148 | forwardSafety = 0; |
t1jain | 38:cb87f4e353b9 | 149 | |
t1jain | 38:cb87f4e353b9 | 150 | //////////////////////////////////////////////////////////////////////////// |
t1jain | 38:cb87f4e353b9 | 151 | |
t1jain | 40:11b8567c507b | 152 | ledgeArrayLB[k] = (*(ToF+1))->readFromOneSensor(); |
t1jain | 40:11b8567c507b | 153 | ledgeArrayRB[k] = (*(ToF+4))->readFromOneSensor(); |
t1jain | 38:cb87f4e353b9 | 154 | |
t1jain | 38:cb87f4e353b9 | 155 | outlierToF[2] = LBTStats.mean() + 2*LBTStats.stdev(); |
t1jain | 38:cb87f4e353b9 | 156 | outlierToF[3] = RBTStats.mean() + 2*RBTStats.stdev(); |
t1jain | 38:cb87f4e353b9 | 157 | |
t1jain | 42:62c49ad06707 | 158 | for(int i = 2; i < 4; i++) { // Reads from the ToF Sensors |
t1jain | 38:cb87f4e353b9 | 159 | runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); |
t1jain | 38:cb87f4e353b9 | 160 | } |
t1jain | 38:cb87f4e353b9 | 161 | |
t1jain | 38:cb87f4e353b9 | 162 | int sensor7 = ToFV[6]; |
t1jain | 38:cb87f4e353b9 | 163 | int sensor10 = ToFV[9]; |
t1jain | 38:cb87f4e353b9 | 164 | if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor7 < curr_vel*curr_vel*1000*1000 || |
t1jain | 38:cb87f4e353b9 | 165 | 2 * maxDecelerationSlow*sensor10 < curr_vel*curr_vel*1000*1000) && |
t1jain | 38:cb87f4e353b9 | 166 | (sensor7 < 1500 || sensor10 < 1500)) || |
t1jain | 38:cb87f4e353b9 | 167 | 550 > sensor7 || 550 > sensor10) { |
t1jain | 38:cb87f4e353b9 | 168 | if(x->read() > def) { |
t1jain | 38:cb87f4e353b9 | 169 | x->write(def); |
t1jain | 39:5438557a8b4e | 170 | backwardSafety = 1; // You cannot move backwards |
t1jain | 38:cb87f4e353b9 | 171 | } |
t1jain | 38:cb87f4e353b9 | 172 | } |
t1jain | 38:cb87f4e353b9 | 173 | |
t1jain | 38:cb87f4e353b9 | 174 | else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || |
t1jain | 38:cb87f4e353b9 | 175 | 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && |
t1jain | 38:cb87f4e353b9 | 176 | (sensor7 < 1500 || sensor10 < 1500)) || |
t1jain | 38:cb87f4e353b9 | 177 | 550 > sensor7 || 550 > sensor10) { |
t1jain | 38:cb87f4e353b9 | 178 | if(x->read() > def) { |
t1jain | 38:cb87f4e353b9 | 179 | x->write(def); |
t1jain | 39:5438557a8b4e | 180 | backwardSafety = 1; // You cannot move backwards |
t1jain | 42:62c49ad06707 | 181 | // Stop moving backwards |
t1jain | 38:cb87f4e353b9 | 182 | } |
t1jain | 38:cb87f4e353b9 | 183 | } |
t1jain | 38:cb87f4e353b9 | 184 | |
t1jain | 38:cb87f4e353b9 | 185 | else if ((runningAverage[2] > outlierToF[2]) || (runningAverage[3] > outlierToF[3])) { |
t1jain | 38:cb87f4e353b9 | 186 | backwardSafety = 1; |
t1jain | 38:cb87f4e353b9 | 187 | out->printf("I'M STOPPING BECAUSE OF A BACK LEDGE\r\n"); |
t1jain | 38:cb87f4e353b9 | 188 | } |
t1jain | 38:cb87f4e353b9 | 189 | |
t1jain | 38:cb87f4e353b9 | 190 | else |
t1jain | 38:cb87f4e353b9 | 191 | backwardSafety = 0; |
t1jain | 41:1a687bcf4b0a | 192 | |
t1jain | 38:cb87f4e353b9 | 193 | //////////////////////////////////////////////////////////////////////////// |
JesiMiranda | 35:5a2fed4c2e9f | 194 | |
JesiMiranda | 37:e0e6d3fe06a2 | 195 | /*Side Tof begin*/ |
t1jain | 42:62c49ad06707 | 196 | int sensor3 = ToFV[2]; //front left |
t1jain | 42:62c49ad06707 | 197 | int sensor6 = ToFV[5]; //front right |
sepham | 43:9858a580b3d7 | 198 | int sensor27 = ToFV[98]; //left side on back |
sepham | 43:9858a580b3d7 | 199 | int sensor28 = ToFV[99]; //right side on back |
sepham | 43:9858a580b3d7 | 200 | |
sepham | 43:9858a580b3d7 | 201 | int angleRight = ToFV[100]; //The angled sensor for blindspots |
sepham | 43:9858a580b3d7 | 202 | int angleLeft = ToFV[101]; |
t1jain | 34:b89967adc86c | 203 | |
JesiMiranda | 37:e0e6d3fe06a2 | 204 | double currAngularVelocity = imu->gyro_x(); //Current angular velocity from IMU |
JesiMiranda | 37:e0e6d3fe06a2 | 205 | double angle = imu->yaw() * 3.14159 / 180; //from IMU, in rads |
JesiMiranda | 37:e0e6d3fe06a2 | 206 | double arcLength = WheelchairRadius * currAngularVelocity * |
JesiMiranda | 37:e0e6d3fe06a2 | 207 | currAngularVelocity / (2 * maxAngularDeceleration); //S = r*Ө, in cm |
JesiMiranda | 37:e0e6d3fe06a2 | 208 | |
JesiMiranda | 37:e0e6d3fe06a2 | 209 | /* Clear the front side first, else continue going straight or can't turn |
JesiMiranda | 37:e0e6d3fe06a2 | 210 | After clearing the front sideand movinf forward, check if can clear the back |
JesiMiranda | 37:e0e6d3fe06a2 | 211 | when turning */ |
sepham | 43:9858a580b3d7 | 212 | |
sepham | 43:9858a580b3d7 | 213 | //When either sensors too close to the wall, can't turn |
JesiMiranda | 37:e0e6d3fe06a2 | 214 | if(sensor3 <= minWallLength) { |
JesiMiranda | 37:e0e6d3fe06a2 | 215 | leftSafety = 1; |
JesiMiranda | 37:e0e6d3fe06a2 | 216 | out-> printf("Detecting wall to the left!\n"); |
JesiMiranda | 37:e0e6d3fe06a2 | 217 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 218 | else{ |
JesiMiranda | 37:e0e6d3fe06a2 | 219 | leftSafety = 0; |
JesiMiranda | 37:e0e6d3fe06a2 | 220 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 221 | |
JesiMiranda | 37:e0e6d3fe06a2 | 222 | if(sensor6 <= minWallLength) { |
JesiMiranda | 37:e0e6d3fe06a2 | 223 | rightSafety = 1; |
JesiMiranda | 37:e0e6d3fe06a2 | 224 | out-> printf("Detecting wall to the right!\n"); |
JesiMiranda | 37:e0e6d3fe06a2 | 225 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 226 | else { |
JesiMiranda | 37:e0e6d3fe06a2 | 227 | rightSafety = 0; |
t1jain | 34:b89967adc86c | 228 | } |
t1jain | 34:b89967adc86c | 229 | |
sepham | 43:9858a580b3d7 | 230 | //For the back side ToF. |
sepham | 43:9858a580b3d7 | 231 | |
sepham | 43:9858a580b3d7 | 232 | //Scenario 1: Wheelchair close to right side wall and you want to turn left. |
sepham | 43:9858a580b3d7 | 233 | //The back right side sensor say too close and corner get bumped |
sepham | 43:9858a580b3d7 | 234 | if(sensor28 <= minWallLength) { |
sepham | 43:9858a580b3d7 | 235 | leftSafety = 1; |
sepham | 43:9858a580b3d7 | 236 | out-> printf("Detecting wall to the right at back! Don't turn left. \n"); |
sepham | 43:9858a580b3d7 | 237 | } |
sepham | 43:9858a580b3d7 | 238 | else { |
sepham | 43:9858a580b3d7 | 239 | leftSafety = 0; |
sepham | 43:9858a580b3d7 | 240 | } |
sepham | 43:9858a580b3d7 | 241 | |
sepham | 43:9858a580b3d7 | 242 | //Scenario 2: Wheelchair close to left side wall and you want to turn right. |
sepham | 43:9858a580b3d7 | 243 | //The back left side sensor say too close and corner get bumped |
sepham | 43:9858a580b3d7 | 244 | if(sensor27 <= minWallLength) { |
sepham | 43:9858a580b3d7 | 245 | rightSafety = 1; |
sepham | 43:9858a580b3d7 | 246 | out-> printf("Detecting wall to the left at back! Don't turn right. \n"); |
sepham | 43:9858a580b3d7 | 247 | } |
sepham | 43:9858a580b3d7 | 248 | else { |
sepham | 43:9858a580b3d7 | 249 | rightSafety = 0; |
sepham | 43:9858a580b3d7 | 250 | } |
sepham | 43:9858a580b3d7 | 251 | |
sepham | 43:9858a580b3d7 | 252 | //Below for the angled sensor when detect something in the blindspot between |
sepham | 43:9858a580b3d7 | 253 | //the side sensors |
sepham | 43:9858a580b3d7 | 254 | if(angleLeft <= 100000) { |
sepham | 43:9858a580b3d7 | 255 | leftSafety = 1; |
sepham | 43:9858a580b3d7 | 256 | out-> printf("Blind spot on the left side\n"); |
sepham | 43:9858a580b3d7 | 257 | } |
sepham | 43:9858a580b3d7 | 258 | else{ |
sepham | 43:9858a580b3d7 | 259 | leftSafety = 0; |
sepham | 43:9858a580b3d7 | 260 | } |
sepham | 43:9858a580b3d7 | 261 | |
sepham | 43:9858a580b3d7 | 262 | if(angleRight <= 100000) { |
sepham | 43:9858a580b3d7 | 263 | rightSafety = 1; |
sepham | 43:9858a580b3d7 | 264 | out-> printf("Blindspot on the right side\n"); |
sepham | 43:9858a580b3d7 | 265 | } |
sepham | 43:9858a580b3d7 | 266 | else{ |
sepham | 43:9858a580b3d7 | 267 | rightSafety = 0; |
sepham | 43:9858a580b3d7 | 268 | } |
sepham | 43:9858a580b3d7 | 269 | |
JesiMiranda | 37:e0e6d3fe06a2 | 270 | /*Check whether safe to keep turning |
JesiMiranda | 37:e0e6d3fe06a2 | 271 | Know the exact moment you can stop the chair going at a certain speed before |
JesiMiranda | 37:e0e6d3fe06a2 | 272 | its too late*/ |
JesiMiranda | 37:e0e6d3fe06a2 | 273 | if((currAngularVelocity * currAngularVelocity > 2 * |
JesiMiranda | 37:e0e6d3fe06a2 | 274 | maxAngularDeceleration * angle) && (sensor3/10 <= arcLength + 10)) { |
JesiMiranda | 37:e0e6d3fe06a2 | 275 | leftSafety = 1; //Not safe to turn left |
JesiMiranda | 37:e0e6d3fe06a2 | 276 | out-> printf("Too fast to the left!\n"); |
JesiMiranda | 37:e0e6d3fe06a2 | 277 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 278 | else{ |
JesiMiranda | 37:e0e6d3fe06a2 | 279 | leftSafety = 0; |
sepham | 43:9858a580b3d7 | 280 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 281 | if((currAngularVelocity * currAngularVelocity > 2 * |
JesiMiranda | 37:e0e6d3fe06a2 | 282 | maxAngularDeceleration * angle) && (sensor6/10 <= arcLength + 10)) { |
JesiMiranda | 37:e0e6d3fe06a2 | 283 | rightSafety = 1; //Not safe to turn right |
JesiMiranda | 37:e0e6d3fe06a2 | 284 | out-> printf("Too fast to the right!\n"); |
t1jain | 34:b89967adc86c | 285 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 286 | else{ |
JesiMiranda | 37:e0e6d3fe06a2 | 287 | rightSafety = 0; |
sepham | 43:9858a580b3d7 | 288 | } |
JesiMiranda | 37:e0e6d3fe06a2 | 289 | |
sepham | 43:9858a580b3d7 | 290 | //Deal with the speed of turning when moving forward, stop in time when |
sepham | 43:9858a580b3d7 | 291 | //Scenario 1: Turning to the right forward, but the left back side sensor pick |
sepham | 43:9858a580b3d7 | 292 | //something up. Can't turn right |
sepham | 43:9858a580b3d7 | 293 | /* if((currAngularVelocity * currAngularVelocity > 2 * |
sepham | 43:9858a580b3d7 | 294 | maxAngularDeceleration * angle) && (sensor27/10 <= arcLength + 10)) { |
sepham | 43:9858a580b3d7 | 295 | rightSafety = 1; //Not safe to turn left |
sepham | 43:9858a580b3d7 | 296 | out-> printf("Too fast to the left! Back left area danger \n"); |
sepham | 43:9858a580b3d7 | 297 | } |
sepham | 43:9858a580b3d7 | 298 | else{ |
sepham | 43:9858a580b3d7 | 299 | rightSafety = 0; |
sepham | 43:9858a580b3d7 | 300 | } |
sepham | 43:9858a580b3d7 | 301 | //Scenario 2: Turning to the left forward, but the right back side sensor pick |
sepham | 43:9858a580b3d7 | 302 | //something up. CAn't turn left |
sepham | 43:9858a580b3d7 | 303 | if((currAngularVelocity * currAngularVelocity > 2 * |
sepham | 43:9858a580b3d7 | 304 | maxAngularDeceleration * angle) && (sensor28/10 <= arcLength + 10)) { |
sepham | 43:9858a580b3d7 | 305 | leftSafety = 1; //Not safe to turn left |
sepham | 43:9858a580b3d7 | 306 | out-> printf("Too fast to the left! Back right area danger\n"); |
JesiMiranda | 37:e0e6d3fe06a2 | 307 | } |
sepham | 43:9858a580b3d7 | 308 | else{ |
sepham | 43:9858a580b3d7 | 309 | leftSafety = 0; |
sepham | 43:9858a580b3d7 | 310 | } */ |
JesiMiranda | 37:e0e6d3fe06a2 | 311 | |
sepham | 43:9858a580b3d7 | 312 | //In the blindspot of the side sensor, keep wheelchair from turning if detect in |
sepham | 43:9858a580b3d7 | 313 | //that blind area. Account from certain distance from the angled sensor and how |
sepham | 43:9858a580b3d7 | 314 | //fast turning/moving toward obstacle. |
sepham | 43:9858a580b3d7 | 315 | if((currAngularVelocity * currAngularVelocity > 2 * |
sepham | 43:9858a580b3d7 | 316 | maxAngularDeceleration * angle) && (angleLeft/10 <= arcLength + 10)) { |
sepham | 43:9858a580b3d7 | 317 | leftSafety = 1; //Not safe to turn left |
sepham | 43:9858a580b3d7 | 318 | out-> printf("Too fast to the left!, blindspot \n"); |
sepham | 43:9858a580b3d7 | 319 | } |
sepham | 43:9858a580b3d7 | 320 | else{ |
sepham | 43:9858a580b3d7 | 321 | leftSafety = 0; |
sepham | 43:9858a580b3d7 | 322 | } |
sepham | 43:9858a580b3d7 | 323 | if((currAngularVelocity * currAngularVelocity > 2 * |
sepham | 43:9858a580b3d7 | 324 | maxAngularDeceleration * angle) && (angleRight/10 <= arcLength + 10)) { |
sepham | 43:9858a580b3d7 | 325 | rightSafety = 1; //Not safe to turn right |
sepham | 43:9858a580b3d7 | 326 | out-> printf("Too fast to the right!, blindspot \n"); |
sepham | 43:9858a580b3d7 | 327 | } |
sepham | 43:9858a580b3d7 | 328 | else{ |
sepham | 43:9858a580b3d7 | 329 | rightSafety = 0; |
sepham | 43:9858a580b3d7 | 330 | } |
sepham | 43:9858a580b3d7 | 331 | |
sepham | 43:9858a580b3d7 | 332 | /*Side Tof end*/ |
JesiMiranda | 37:e0e6d3fe06a2 | 333 | |
jvfausto | 21:3489cffad196 | 334 | } |
ryanlin97 | 6:0cd57bdd8fbc | 335 | |
JesiMiranda | 35:5a2fed4c2e9f | 336 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 337 | * Constructor for Wheelchair class * |
JesiMiranda | 35:5a2fed4c2e9f | 338 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 339 | Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, |
JesiMiranda | 35:5a2fed4c2e9f | 340 | VL53L1X** ToFT) |
jvfausto | 21:3489cffad196 | 341 | { |
jvfausto | 21:3489cffad196 | 342 | x_position = 0; |
jvfausto | 21:3489cffad196 | 343 | y_position = 0; |
jvfausto | 21:3489cffad196 | 344 | forwardSafety = 0; |
t1jain | 39:5438557a8b4e | 345 | backwardSafety = 0; |
JesiMiranda | 35:5a2fed4c2e9f | 346 | |
jvfausto | 21:3489cffad196 | 347 | /* Initializes X and Y variables to Pins */ |
JesiMiranda | 35:5a2fed4c2e9f | 348 | x = new PwmOut(xPin); |
jvfausto | 21:3489cffad196 | 349 | y = new PwmOut(yPin); |
JesiMiranda | 35:5a2fed4c2e9f | 350 | |
jvfausto | 21:3489cffad196 | 351 | /* Initializes IMU Library */ |
jvfausto | 26:662693bd7f31 | 352 | out = pc; // "out" is called for serial monitor |
jvfausto | 26:662693bd7f31 | 353 | out->printf("on\r\n"); |
t1jain | 42:62c49ad06707 | 354 | imu = new BNO080Wheelchair (pc, D2, D4, D13, D15, 0x4b, 100000); // THIS NEEDS TO BE REVISED SO THAT PIN NAMES ARE CALLED |
jvfausto | 21:3489cffad196 | 355 | Wheelchair::stop(); // Wheelchair is initially stationary |
jvfausto | 21:3489cffad196 | 356 | imu->setup(); // turns on the IMU |
jvfausto | 21:3489cffad196 | 357 | wheelS = qeiS; // "wheel" is called for encoder |
JesiMiranda | 35:5a2fed4c2e9f | 358 | wheel = qei; |
jvfausto | 21:3489cffad196 | 359 | ToF = ToFT; // passes pointer with addresses of ToF sensors |
JesiMiranda | 35:5a2fed4c2e9f | 360 | |
JesiMiranda | 35:5a2fed4c2e9f | 361 | for(int i = 0; i < 12; i++) { // initializes the ToF Sensors |
jvfausto | 21:3489cffad196 | 362 | (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); |
jvfausto | 21:3489cffad196 | 363 | } |
JesiMiranda | 35:5a2fed4c2e9f | 364 | |
jvfausto | 21:3489cffad196 | 365 | out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor |
jvfausto | 21:3489cffad196 | 366 | ti = time; |
JesiMiranda | 35:5a2fed4c2e9f | 367 | for(int i = 0; i < 10; i++) { |
JesiMiranda | 35:5a2fed4c2e9f | 368 | (*(ToF+1))->readFromOneSensor(); |
JesiMiranda | 35:5a2fed4c2e9f | 369 | (*(ToF+1))->readFromOneSensor(); |
t1jain | 31:06f2362caf12 | 370 | } |
JesiMiranda | 35:5a2fed4c2e9f | 371 | for(int i = 0; i < 150; i++) { |
jvfausto | 27:da718b990837 | 372 | ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); |
t1jain | 30:c25b2556e84d | 373 | ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor(); |
jvfausto | 27:da718b990837 | 374 | } |
JesiMiranda | 35:5a2fed4c2e9f | 375 | |
t1jain | 30:c25b2556e84d | 376 | outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); |
t1jain | 30:c25b2556e84d | 377 | outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); |
t1jain | 34:b89967adc86c | 378 | |
jvfausto | 21:3489cffad196 | 379 | myPID.SetMode(AUTOMATIC); // PID mode: Automatic |
jvfausto | 21:3489cffad196 | 380 | } |
jvfausto | 21:3489cffad196 | 381 | |
JesiMiranda | 35:5a2fed4c2e9f | 382 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 383 | * Move wheelchair with joystick on manual mode * |
JesiMiranda | 35:5a2fed4c2e9f | 384 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 385 | void Wheelchair::move(float x_coor, float y_coor) |
jvfausto | 21:3489cffad196 | 386 | { |
JesiMiranda | 35:5a2fed4c2e9f | 387 | /* Scales one joystick measurement to the chair's joystick measurement */ |
ryanlin97 | 4:29a27953fe70 | 388 | float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; |
ryanlin97 | 4:29a27953fe70 | 389 | float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; |
jvfausto | 21:3489cffad196 | 390 | |
JesiMiranda | 35:5a2fed4c2e9f | 391 | /* Sends the scaled joystic values to the chair */ |
JesiMiranda | 35:5a2fed4c2e9f | 392 | x->write(scaled_x); |
ryanlin97 | 4:29a27953fe70 | 393 | y->write(scaled_y); |
ryanlin97 | 5:e0ccaab3959a | 394 | } |
JesiMiranda | 35:5a2fed4c2e9f | 395 | |
JesiMiranda | 35:5a2fed4c2e9f | 396 | |
JesiMiranda | 35:5a2fed4c2e9f | 397 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 398 | * Automatic mode: move forward and update x,y coordinate sent to chair * |
JesiMiranda | 35:5a2fed4c2e9f | 399 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 400 | void Wheelchair::forward() |
ryanlin97 | 1:c0beadca1617 | 401 | { |
jvfausto | 26:662693bd7f31 | 402 | //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS); |
JesiMiranda | 35:5a2fed4c2e9f | 403 | if(forwardSafety == 0) { |
JesiMiranda | 35:5a2fed4c2e9f | 404 | x->write(high); |
JesiMiranda | 35:5a2fed4c2e9f | 405 | y->write(def+offset); |
jvfausto | 21:3489cffad196 | 406 | } |
jvfausto | 26:662693bd7f31 | 407 | out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975)); |
ryanlin97 | 0:fc0c4a184482 | 408 | } |
JesiMiranda | 35:5a2fed4c2e9f | 409 | |
JesiMiranda | 35:5a2fed4c2e9f | 410 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 411 | * Automatic mode: move in reverse and update x,y coordinate sent to chair* |
JesiMiranda | 35:5a2fed4c2e9f | 412 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 413 | void Wheelchair::backward() |
ryanlin97 | 1:c0beadca1617 | 414 | { |
t1jain | 39:5438557a8b4e | 415 | if (backwardSafety == 0) { |
t1jain | 39:5438557a8b4e | 416 | x->write(low); |
t1jain | 39:5438557a8b4e | 417 | y->write(def); |
t1jain | 39:5438557a8b4e | 418 | } |
ryanlin97 | 0:fc0c4a184482 | 419 | } |
JesiMiranda | 35:5a2fed4c2e9f | 420 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 421 | * Automatic mode: move right and update x,y coordinate sent to chair * |
JesiMiranda | 35:5a2fed4c2e9f | 422 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 423 | void Wheelchair::right() |
ryanlin97 | 1:c0beadca1617 | 424 | { |
t1jain | 34:b89967adc86c | 425 | //if safe to move, from ToFSafety |
JesiMiranda | 37:e0e6d3fe06a2 | 426 | if(rightSafety == 0) { |
JesiMiranda | 35:5a2fed4c2e9f | 427 | x->write(def); |
JesiMiranda | 35:5a2fed4c2e9f | 428 | y->write(low); |
t1jain | 34:b89967adc86c | 429 | } |
ryanlin97 | 0:fc0c4a184482 | 430 | } |
JesiMiranda | 35:5a2fed4c2e9f | 431 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 432 | * Automatic mode: move left and update x,y coordinate sent to chair * |
JesiMiranda | 35:5a2fed4c2e9f | 433 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 434 | void Wheelchair::left() |
ryanlin97 | 1:c0beadca1617 | 435 | { |
t1jain | 34:b89967adc86c | 436 | //if safe to move, from ToFSafety |
JesiMiranda | 37:e0e6d3fe06a2 | 437 | if(leftSafety == 0) { |
t1jain | 34:b89967adc86c | 438 | x->write(def); |
t1jain | 34:b89967adc86c | 439 | y->write(high); |
t1jain | 34:b89967adc86c | 440 | } |
ryanlin97 | 0:fc0c4a184482 | 441 | } |
JesiMiranda | 35:5a2fed4c2e9f | 442 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 443 | * Stop the wheelchair * |
JesiMiranda | 35:5a2fed4c2e9f | 444 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 445 | void Wheelchair::stop() |
ryanlin97 | 1:c0beadca1617 | 446 | { |
ryanlin97 | 0:fc0c4a184482 | 447 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 448 | y->write(def); |
ryanlin97 | 6:0cd57bdd8fbc | 449 | } |
JesiMiranda | 35:5a2fed4c2e9f | 450 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 451 | * Counter-clockwise is ( - ) * |
JesiMiranda | 35:5a2fed4c2e9f | 452 | * Clockwise is ( + ) * |
JesiMiranda | 35:5a2fed4c2e9f | 453 | * Range of deg: 0 to 360 * |
JesiMiranda | 35:5a2fed4c2e9f | 454 | * This method takes in an angle from user and adjusts for turning right * |
JesiMiranda | 35:5a2fed4c2e9f | 455 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 456 | void Wheelchair::pid_right(int deg) |
ryanlin97 | 12:921488918749 | 457 | { |
t1jain | 33:f3585571f11e | 458 | bool overturn = false; // Boolean if angle over 360˚ |
JesiMiranda | 35:5a2fed4c2e9f | 459 | |
JesiMiranda | 35:5a2fed4c2e9f | 460 | out->printf("pid right\r\r\n"); |
jvfausto | 21:3489cffad196 | 461 | x->write(def); // Update x sent to chair to be stationary |
jvfausto | 21:3489cffad196 | 462 | Setpoint = curr_yaw + deg; // Relative angle we want to turn |
jvfausto | 21:3489cffad196 | 463 | pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user |
JesiMiranda | 35:5a2fed4c2e9f | 464 | |
jvfausto | 21:3489cffad196 | 465 | /* Turns on overturn boolean if setpoint over 360˚ */ |
JesiMiranda | 35:5a2fed4c2e9f | 466 | if(Setpoint > 360) { |
ryanlin97 | 12:921488918749 | 467 | overturn = true; |
ryanlin97 | 12:921488918749 | 468 | } |
JesiMiranda | 35:5a2fed4c2e9f | 469 | |
jvfausto | 21:3489cffad196 | 470 | myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 471 | myPID.SetOutputLimits(0, def-low-.15); // Limit is set to the differnce between def and low |
jvfausto | 21:3489cffad196 | 472 | myPID.SetControllerDirection(DIRECT); // PID mode: Direct |
JesiMiranda | 35:5a2fed4c2e9f | 473 | |
JesiMiranda | 35:5a2fed4c2e9f | 474 | /* PID stops when approaching a litte less than desired angle */ |
JesiMiranda | 35:5a2fed4c2e9f | 475 | while(pid_yaw < Setpoint - 3) { |
jvfausto | 21:3489cffad196 | 476 | /* PID is set to correct angle range if angle greater than 360˚*/ |
JesiMiranda | 35:5a2fed4c2e9f | 477 | if(overturn && curr_yaw < Setpoint-deg-1) { |
JesiMiranda | 35:5a2fed4c2e9f | 478 | pid_yaw = curr_yaw + 360; |
JesiMiranda | 35:5a2fed4c2e9f | 479 | } else { |
jvfausto | 17:7f3b69300bb6 | 480 | pid_yaw = curr_yaw; |
ryanlin97 | 12:921488918749 | 481 | } |
JesiMiranda | 35:5a2fed4c2e9f | 482 | |
jvfausto | 21:3489cffad196 | 483 | myPID.Compute(); // Does PID calculations |
jvfausto | 21:3489cffad196 | 484 | double tempor = -Output+def; // Temporary value with the voltage output |
JesiMiranda | 35:5a2fed4c2e9f | 485 | y->write(tempor); // Update y sent to chair |
JesiMiranda | 35:5a2fed4c2e9f | 486 | |
jvfausto | 21:3489cffad196 | 487 | /* Prints to serial monitor the current angle and setpoint */ |
JesiMiranda | 35:5a2fed4c2e9f | 488 | out->printf("curr_yaw %f\r\r\n", curr_yaw); |
jvfausto | 21:3489cffad196 | 489 | out->printf("Setpoint = %f \r\n", Setpoint); |
JesiMiranda | 35:5a2fed4c2e9f | 490 | |
jvfausto | 21:3489cffad196 | 491 | wait(.05); // Small delay (milliseconds) |
ryanlin97 | 12:921488918749 | 492 | } |
JesiMiranda | 35:5a2fed4c2e9f | 493 | |
jvfausto | 21:3489cffad196 | 494 | /* Saftey stop for wheelchair */ |
JesiMiranda | 35:5a2fed4c2e9f | 495 | Wheelchair::stop(); |
jvfausto | 21:3489cffad196 | 496 | out->printf("done \r\n"); |
jvfausto | 21:3489cffad196 | 497 | } |
JesiMiranda | 35:5a2fed4c2e9f | 498 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 499 | * Counter-clockwise is ( - ) * |
JesiMiranda | 35:5a2fed4c2e9f | 500 | * Clockwise is ( + ) * |
JesiMiranda | 35:5a2fed4c2e9f | 501 | * Range of deg: 0 to 360 * |
JesiMiranda | 35:5a2fed4c2e9f | 502 | * This method takes in an angle from user and adjusts for turning left * |
JesiMiranda | 35:5a2fed4c2e9f | 503 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 504 | void Wheelchair::pid_left(int deg) |
ryanlin97 | 12:921488918749 | 505 | { |
jvfausto | 21:3489cffad196 | 506 | bool overturn = false; //Boolean if angle under 0˚ |
JesiMiranda | 35:5a2fed4c2e9f | 507 | |
JesiMiranda | 35:5a2fed4c2e9f | 508 | out->printf("pid Left\r\r\n"); |
jvfausto | 21:3489cffad196 | 509 | x->write(def); // Update x sent to chair to be stationary |
jvfausto | 21:3489cffad196 | 510 | Setpoint = curr_yaw - deg; // Relative angle we want to turn |
jvfausto | 21:3489cffad196 | 511 | pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user |
JesiMiranda | 35:5a2fed4c2e9f | 512 | |
jvfausto | 21:3489cffad196 | 513 | /* Turns on overturn boolean if setpoint less than 0˚ */ |
JesiMiranda | 35:5a2fed4c2e9f | 514 | if(Setpoint < 0) { |
ryanlin97 | 12:921488918749 | 515 | overturn = true; |
ryanlin97 | 12:921488918749 | 516 | } |
JesiMiranda | 35:5a2fed4c2e9f | 517 | |
jvfausto | 21:3489cffad196 | 518 | myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D |
t1jain | 33:f3585571f11e | 519 | myPID.SetOutputLimits(0,high-def-.12); // Limit is set to the differnce between def and low |
jvfausto | 21:3489cffad196 | 520 | myPID.SetControllerDirection(REVERSE); // PID mode: Reverse |
JesiMiranda | 35:5a2fed4c2e9f | 521 | |
jvfausto | 21:3489cffad196 | 522 | /* PID stops when approaching a litte more than desired angle */ |
JesiMiranda | 35:5a2fed4c2e9f | 523 | while(pid_yaw > Setpoint+3) { |
JesiMiranda | 35:5a2fed4c2e9f | 524 | /* PID is set to correct angle range if angle less than 0˚ */ |
JesiMiranda | 35:5a2fed4c2e9f | 525 | if(overturn && curr_yaw > Setpoint+deg+1) { |
JesiMiranda | 35:5a2fed4c2e9f | 526 | pid_yaw = curr_yaw - 360; |
JesiMiranda | 35:5a2fed4c2e9f | 527 | } else { |
JesiMiranda | 35:5a2fed4c2e9f | 528 | pid_yaw = curr_yaw; |
JesiMiranda | 35:5a2fed4c2e9f | 529 | } |
JesiMiranda | 35:5a2fed4c2e9f | 530 | |
jvfausto | 21:3489cffad196 | 531 | myPID.Compute(); // Does PID calculations |
jvfausto | 21:3489cffad196 | 532 | double tempor = Output+def; // Temporary value with the voltage output |
jvfausto | 21:3489cffad196 | 533 | y->write(tempor); // Update y sent to chair |
JesiMiranda | 35:5a2fed4c2e9f | 534 | |
jvfausto | 21:3489cffad196 | 535 | /* Prints to serial monitor the current angle and setpoint */ |
jvfausto | 17:7f3b69300bb6 | 536 | out->printf("curr_yaw %f\r\n", curr_yaw); |
jvfausto | 21:3489cffad196 | 537 | out->printf("Setpoint = %f \r\n", Setpoint); |
JesiMiranda | 35:5a2fed4c2e9f | 538 | |
jvfausto | 21:3489cffad196 | 539 | wait(.05); // Small delay (milliseconds) |
ryanlin97 | 12:921488918749 | 540 | } |
JesiMiranda | 35:5a2fed4c2e9f | 541 | |
JesiMiranda | 35:5a2fed4c2e9f | 542 | /* Saftey stop for wheelchair */ |
JesiMiranda | 35:5a2fed4c2e9f | 543 | Wheelchair::stop(); |
jvfausto | 21:3489cffad196 | 544 | out->printf("done \r\n"); |
ryanlin97 | 12:921488918749 | 545 | |
jvfausto | 21:3489cffad196 | 546 | } |
JesiMiranda | 35:5a2fed4c2e9f | 547 | |
JesiMiranda | 35:5a2fed4c2e9f | 548 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 549 | * This method determines whether to turn left or right * |
JesiMiranda | 35:5a2fed4c2e9f | 550 | **************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 551 | void Wheelchair::pid_turn(int deg) |
JesiMiranda | 35:5a2fed4c2e9f | 552 | { |
JesiMiranda | 35:5a2fed4c2e9f | 553 | |
JesiMiranda | 35:5a2fed4c2e9f | 554 | /***************************************************************** |
JesiMiranda | 35:5a2fed4c2e9f | 555 | * Sets angle to coterminal angle for left turn if deg > 180 * |
JesiMiranda | 35:5a2fed4c2e9f | 556 | * Sets angle to coterminal angle for right turn if deg < -180 * |
JesiMiranda | 35:5a2fed4c2e9f | 557 | *****************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 558 | if(deg > 180) { |
ryanlin97 | 12:921488918749 | 559 | deg -= 360; |
JesiMiranda | 35:5a2fed4c2e9f | 560 | } else if(deg < -180) { |
JesiMiranda | 35:5a2fed4c2e9f | 561 | deg +=360; |
ryanlin97 | 12:921488918749 | 562 | } |
JesiMiranda | 35:5a2fed4c2e9f | 563 | |
jvfausto | 21:3489cffad196 | 564 | /* Makes sure angle inputted to function is positive */ |
ryanlin97 | 12:921488918749 | 565 | int turnAmt = abs(deg); |
JesiMiranda | 35:5a2fed4c2e9f | 566 | |
jvfausto | 21:3489cffad196 | 567 | /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */ |
JesiMiranda | 35:5a2fed4c2e9f | 568 | if(deg >= 0) { |
JesiMiranda | 35:5a2fed4c2e9f | 569 | Wheelchair::pid_right(turnAmt); |
JesiMiranda | 35:5a2fed4c2e9f | 570 | } else { |
JesiMiranda | 35:5a2fed4c2e9f | 571 | Wheelchair::pid_left(turnAmt); |
jvfausto | 21:3489cffad196 | 572 | } |
ryanlin97 | 12:921488918749 | 573 | |
jvfausto | 21:3489cffad196 | 574 | } |
jvfausto | 21:3489cffad196 | 575 | |
JesiMiranda | 35:5a2fed4c2e9f | 576 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 577 | * This method takes in distance to travel and adjust to move forward * |
JesiMiranda | 35:5a2fed4c2e9f | 578 | **************************************************************************/ |
jvfausto | 19:71a6621ee5c3 | 579 | void Wheelchair::pid_forward(double mm) |
jvfausto | 17:7f3b69300bb6 | 580 | { |
jvfausto | 21:3489cffad196 | 581 | mm -= 20; // Makes sure distance does not overshoot |
jvfausto | 21:3489cffad196 | 582 | Input = 0; // Initializes input to zero: Test latter w/o |
jvfausto | 21:3489cffad196 | 583 | wheel->reset(); // Resets encoders so that they start at 0 |
JesiMiranda | 35:5a2fed4c2e9f | 584 | |
jvfausto | 17:7f3b69300bb6 | 585 | out->printf("pid foward\r\n"); |
JesiMiranda | 35:5a2fed4c2e9f | 586 | |
jvfausto | 21:3489cffad196 | 587 | double tempor; // Initializes Temporary variable for x input |
jvfausto | 21:3489cffad196 | 588 | Setpoint = mm; // Initializes the setpoint to desired value |
JesiMiranda | 35:5a2fed4c2e9f | 589 | |
jvfausto | 21:3489cffad196 | 590 | myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D |
JesiMiranda | 35:5a2fed4c2e9f | 591 | myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def |
jvfausto | 21:3489cffad196 | 592 | myPIDDistance.SetControllerDirection(DIRECT); // PID mode: Direct |
JesiMiranda | 35:5a2fed4c2e9f | 593 | |
jvfausto | 21:3489cffad196 | 594 | y->write(def+offset); // Update y to make chair stationary |
JesiMiranda | 35:5a2fed4c2e9f | 595 | |
jvfausto | 21:3489cffad196 | 596 | /* Chair stops moving when Setpoint is reached */ |
JesiMiranda | 35:5a2fed4c2e9f | 597 | while(Input < Setpoint) { |
JesiMiranda | 35:5a2fed4c2e9f | 598 | |
JesiMiranda | 35:5a2fed4c2e9f | 599 | if(out->readable()) { // Emergency Break |
jvfausto | 21:3489cffad196 | 600 | break; |
jvfausto | 21:3489cffad196 | 601 | } |
jvfausto | 17:7f3b69300bb6 | 602 | |
jvfausto | 21:3489cffad196 | 603 | Input = wheel->getDistance(53.975); // Gets distance from Encoder into PID |
jvfausto | 21:3489cffad196 | 604 | wait(.05); // Slight Delay: *****Test without |
jvfausto | 21:3489cffad196 | 605 | myPIDDistance.Compute(); // Compute distance traveled by chair |
JesiMiranda | 35:5a2fed4c2e9f | 606 | |
JesiMiranda | 35:5a2fed4c2e9f | 607 | tempor = Output + def; // Temporary output variable |
jvfausto | 21:3489cffad196 | 608 | x->write(tempor); // Update x sent to chair |
jvfausto | 17:7f3b69300bb6 | 609 | |
jvfausto | 21:3489cffad196 | 610 | /* Prints to serial monitor the distance traveled by chair */ |
jvfausto | 19:71a6621ee5c3 | 611 | out->printf("distance %f\r\n", Input); |
JesiMiranda | 35:5a2fed4c2e9f | 612 | } |
JesiMiranda | 35:5a2fed4c2e9f | 613 | |
JesiMiranda | 35:5a2fed4c2e9f | 614 | } |
jvfausto | 21:3489cffad196 | 615 | |
JesiMiranda | 35:5a2fed4c2e9f | 616 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 617 | * This method returns the relative angular position of chair * |
JesiMiranda | 35:5a2fed4c2e9f | 618 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 619 | double Wheelchair::getTwistZ() |
jvfausto | 17:7f3b69300bb6 | 620 | { |
jvfausto | 21:3489cffad196 | 621 | return imu->gyro_z(); |
JesiMiranda | 35:5a2fed4c2e9f | 622 | } |
jvfausto | 18:663b6d693252 | 623 | |
JesiMiranda | 35:5a2fed4c2e9f | 624 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 625 | * This method computes the relative angle for Twist message in ROS * |
JesiMiranda | 35:5a2fed4c2e9f | 626 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 627 | void Wheelchair::pid_twistA() |
jvfausto | 21:3489cffad196 | 628 | { |
jvfausto | 21:3489cffad196 | 629 | /* Initialize variables for angle and update x,y sent to chair */ |
jvfausto | 21:3489cffad196 | 630 | char c; |
jvfausto | 21:3489cffad196 | 631 | double temporA = def; |
JesiMiranda | 35:5a2fed4c2e9f | 632 | y->write(def); |
JesiMiranda | 35:5a2fed4c2e9f | 633 | x->write(def); |
JesiMiranda | 35:5a2fed4c2e9f | 634 | |
jvfausto | 21:3489cffad196 | 635 | PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 636 | PIDAngularV.SetOutputLimits(-.1, .1); // Limit set to be in range specified |
jvfausto | 21:3489cffad196 | 637 | PIDAngularV.SetControllerDirection(DIRECT); // PID mode: Direct |
JesiMiranda | 35:5a2fed4c2e9f | 638 | |
jvfausto | 21:3489cffad196 | 639 | /* Computes angular position of wheelchair while turning */ |
JesiMiranda | 35:5a2fed4c2e9f | 640 | while(1) { |
jvfausto | 21:3489cffad196 | 641 | yDesired = angularV; |
JesiMiranda | 35:5a2fed4c2e9f | 642 | |
jvfausto | 21:3489cffad196 | 643 | /* Update and set all variable so that the chair is stationary |
jvfausto | 21:3489cffad196 | 644 | * if the desired angle is zero |
jvfausto | 21:3489cffad196 | 645 | */ |
JesiMiranda | 35:5a2fed4c2e9f | 646 | if(yDesired == 0) { |
jvfausto | 21:3489cffad196 | 647 | x->write(def); |
jvfausto | 21:3489cffad196 | 648 | y->write(def); |
jvfausto | 21:3489cffad196 | 649 | yDesired = 0; |
ryanlin97 | 8:381a4ec3fef8 | 650 | return; |
ryanlin97 | 7:5e38d43fbce3 | 651 | } |
JesiMiranda | 35:5a2fed4c2e9f | 652 | |
jvfausto | 21:3489cffad196 | 653 | /* Continuously updates with current angle measured by IMU */ |
JesiMiranda | 35:5a2fed4c2e9f | 654 | yIn = imu->gyro_z(); |
jvfausto | 21:3489cffad196 | 655 | PIDAngularV.Compute(); |
jvfausto | 21:3489cffad196 | 656 | temporA += yOut; // Temporary value with the voltage output |
jvfausto | 21:3489cffad196 | 657 | y->write(temporA); // Update y sent to chair |
JesiMiranda | 35:5a2fed4c2e9f | 658 | |
jvfausto | 21:3489cffad196 | 659 | //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); |
jvfausto | 21:3489cffad196 | 660 | wait(.05); // Small delay (milliseconds) |
ryanlin97 | 6:0cd57bdd8fbc | 661 | } |
JesiMiranda | 35:5a2fed4c2e9f | 662 | |
JesiMiranda | 35:5a2fed4c2e9f | 663 | } |
ryanlin97 | 6:0cd57bdd8fbc | 664 | |
JesiMiranda | 35:5a2fed4c2e9f | 665 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 666 | * This method computes the relative velocity for Twist message in ROS * |
JesiMiranda | 35:5a2fed4c2e9f | 667 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 668 | void Wheelchair::pid_twistV() |
ryanlin97 | 6:0cd57bdd8fbc | 669 | { |
jvfausto | 21:3489cffad196 | 670 | /* Initializes variables as default */ |
jvfausto | 21:3489cffad196 | 671 | double temporV = def; |
jvfausto | 26:662693bd7f31 | 672 | double temporS = def+offset; |
jvfausto | 21:3489cffad196 | 673 | vDesiredS = 0; |
jvfausto | 21:3489cffad196 | 674 | x->write(def); |
jvfausto | 21:3489cffad196 | 675 | y->write(def); |
jvfausto | 21:3489cffad196 | 676 | wheel->reset(); // Resets the encoders |
jvfausto | 21:3489cffad196 | 677 | /* Sets the constants for P and D */ |
JesiMiranda | 35:5a2fed4c2e9f | 678 | PIDVelosity.SetTunings(.0005,0, 0.00); |
JesiMiranda | 35:5a2fed4c2e9f | 679 | PIDSlaveV.SetTunings(.005,0.000001, 0.000001); |
JesiMiranda | 35:5a2fed4c2e9f | 680 | |
jvfausto | 21:3489cffad196 | 681 | /* Limits to the range specified */ |
JesiMiranda | 35:5a2fed4c2e9f | 682 | PIDVelosity.SetOutputLimits(-.005, .005); |
JesiMiranda | 35:5a2fed4c2e9f | 683 | PIDSlaveV.SetOutputLimits(-.002, .002); |
JesiMiranda | 35:5a2fed4c2e9f | 684 | |
jvfausto | 21:3489cffad196 | 685 | /* PID mode: Direct */ |
JesiMiranda | 35:5a2fed4c2e9f | 686 | PIDVelosity.SetControllerDirection(DIRECT); |
JesiMiranda | 35:5a2fed4c2e9f | 687 | PIDSlaveV.SetControllerDirection(DIRECT); |
JesiMiranda | 35:5a2fed4c2e9f | 688 | |
JesiMiranda | 35:5a2fed4c2e9f | 689 | while(1) { |
jvfausto | 21:3489cffad196 | 690 | linearV = .7; |
jvfausto | 21:3489cffad196 | 691 | test1 = linearV*100; |
jvfausto | 21:3489cffad196 | 692 | vel = curr_vel; |
jvfausto | 21:3489cffad196 | 693 | vDesired = linearV*100; |
jvfausto | 21:3489cffad196 | 694 | if(out->readable()) |
jvfausto | 21:3489cffad196 | 695 | return; |
JesiMiranda | 35:5a2fed4c2e9f | 696 | /* Update and set all variable so that the chair is stationary |
JesiMiranda | 35:5a2fed4c2e9f | 697 | * if the velocity is zero |
JesiMiranda | 35:5a2fed4c2e9f | 698 | */ |
JesiMiranda | 35:5a2fed4c2e9f | 699 | if(linearV == 0) { |
jvfausto | 21:3489cffad196 | 700 | x->write(def); |
jvfausto | 21:3489cffad196 | 701 | y->write(def); |
ryanlin97 | 8:381a4ec3fef8 | 702 | |
jvfausto | 21:3489cffad196 | 703 | vel = 0; |
jvfausto | 21:3489cffad196 | 704 | vDesired = 0; |
jvfausto | 21:3489cffad196 | 705 | dist_old = 0; |
ryanlin97 | 8:381a4ec3fef8 | 706 | return; |
ryanlin97 | 8:381a4ec3fef8 | 707 | } |
JesiMiranda | 35:5a2fed4c2e9f | 708 | |
JesiMiranda | 35:5a2fed4c2e9f | 709 | if(vDesired >= 0) { |
jvfausto | 21:3489cffad196 | 710 | PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 711 | PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the range specified |
JesiMiranda | 35:5a2fed4c2e9f | 712 | } else { |
jvfausto | 21:3489cffad196 | 713 | PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 714 | PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to range specified |
JesiMiranda | 35:5a2fed4c2e9f | 715 | } |
JesiMiranda | 35:5a2fed4c2e9f | 716 | |
jvfausto | 21:3489cffad196 | 717 | /* Sets maximum value of variable to 1 */ |
JesiMiranda | 35:5a2fed4c2e9f | 718 | if(temporV >= 1.5) { |
jvfausto | 21:3489cffad196 | 719 | temporV = 1.5; |
ryanlin97 | 8:381a4ec3fef8 | 720 | } |
jvfausto | 21:3489cffad196 | 721 | /* Scales and makes some adjustments to velocity */ |
jvfausto | 21:3489cffad196 | 722 | vIn = curr_vel*100; |
jvfausto | 21:3489cffad196 | 723 | vInS = curr_vel-curr_velS; |
jvfausto | 21:3489cffad196 | 724 | PIDVelosity.Compute(); |
jvfausto | 21:3489cffad196 | 725 | PIDSlaveV.Compute(); |
JesiMiranda | 35:5a2fed4c2e9f | 726 | if(forwardSafety == 0) { |
JesiMiranda | 35:5a2fed4c2e9f | 727 | temporV += vOut; |
JesiMiranda | 35:5a2fed4c2e9f | 728 | temporS += vOutS; |
JesiMiranda | 35:5a2fed4c2e9f | 729 | |
JesiMiranda | 35:5a2fed4c2e9f | 730 | /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ |
JesiMiranda | 35:5a2fed4c2e9f | 731 | x->write(temporV); |
JesiMiranda | 35:5a2fed4c2e9f | 732 | test2 = temporV; |
JesiMiranda | 35:5a2fed4c2e9f | 733 | y->write(temporS); |
JesiMiranda | 35:5a2fed4c2e9f | 734 | } else { |
jvfausto | 21:3489cffad196 | 735 | x->write(def); |
jvfausto | 21:3489cffad196 | 736 | y->write(def); |
jvfausto | 21:3489cffad196 | 737 | } |
jvfausto | 21:3489cffad196 | 738 | //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS); |
jvfausto | 21:3489cffad196 | 739 | Wheelchair::odomMsg(); |
jvfausto | 21:3489cffad196 | 740 | wait(.01); // Small delay (milliseconds) |
ryanlin97 | 6:0cd57bdd8fbc | 741 | } |
ryanlin97 | 11:d14a1f7f1297 | 742 | } |
ryanlin97 | 11:d14a1f7f1297 | 743 | |
JesiMiranda | 35:5a2fed4c2e9f | 744 | /************************************************************************* |
JesiMiranda | 35:5a2fed4c2e9f | 745 | * This method calculates the relative position of the chair everytime the* |
JesiMiranda | 35:5a2fed4c2e9f | 746 | * encoders reset by setting its old position as the origin to calculate * |
JesiMiranda | 35:5a2fed4c2e9f | 747 | * the new position * |
JesiMiranda | 35:5a2fed4c2e9f | 748 | **************************************************************************/ |
jvfausto | 21:3489cffad196 | 749 | void Wheelchair::odomMsg() |
ryanlin97 | 11:d14a1f7f1297 | 750 | { |
jvfausto | 21:3489cffad196 | 751 | double dist_new = curr_pos; |
jvfausto | 21:3489cffad196 | 752 | double dist = dist_new-dist_old; |
jvfausto | 21:3489cffad196 | 753 | double temp_x = dist*sin(z_angular*3.14159/180); |
jvfausto | 21:3489cffad196 | 754 | double temp_y = dist*cos(z_angular*3.14159/180); |
JesiMiranda | 35:5a2fed4c2e9f | 755 | |
jvfausto | 21:3489cffad196 | 756 | x_position += temp_x; |
jvfausto | 21:3489cffad196 | 757 | y_position += temp_y; |
JesiMiranda | 35:5a2fed4c2e9f | 758 | |
jvfausto | 21:3489cffad196 | 759 | dist_old = dist_new; |
JesiMiranda | 35:5a2fed4c2e9f | 760 | } |
jvfausto | 21:3489cffad196 | 761 | |
JesiMiranda | 35:5a2fed4c2e9f | 762 | /************************************************************************** |
JesiMiranda | 35:5a2fed4c2e9f | 763 | * This method prints the Odometry message to the serial monitor * |
JesiMiranda | 35:5a2fed4c2e9f | 764 | ***************************************************************************/ |
JesiMiranda | 35:5a2fed4c2e9f | 765 | void Wheelchair::showOdom() |
JesiMiranda | 35:5a2fed4c2e9f | 766 | { |
JesiMiranda | 35:5a2fed4c2e9f | 767 | out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); |
JesiMiranda | 35:5a2fed4c2e9f | 768 | } |
jvfausto | 21:3489cffad196 | 769 | |
JesiMiranda | 35:5a2fed4c2e9f | 770 | /************************************************************************** |
JesiMiranda | 35:5a2fed4c2e9f | 771 | * This method returns the approximate distance based on the wheel diameter* |
JesiMiranda | 35:5a2fed4c2e9f | 772 | ***************************************************************************/ |
jvfausto | 21:3489cffad196 | 773 | float Wheelchair::getDistance() |
jvfausto | 21:3489cffad196 | 774 | { |
jvfausto | 21:3489cffad196 | 775 | return wheel->getDistance(Diameter); |
ryanlin97 | 6:0cd57bdd8fbc | 776 | } |
ryanlin97 | 8:381a4ec3fef8 | 777 | |
JesiMiranda | 35:5a2fed4c2e9f | 778 | /************************************************************************** |
JesiMiranda | 35:5a2fed4c2e9f | 779 | * This method resets the Encoder's * |
JesiMiranda | 35:5a2fed4c2e9f | 780 | ***************************************************************************/ |
jvfausto | 21:3489cffad196 | 781 | void Wheelchair::resetDistance() |
jvfausto | 21:3489cffad196 | 782 | { |
ryanlin97 | 12:921488918749 | 783 | wheel->reset(); |
jvfausto | 21:3489cffad196 | 784 | } |
jvfausto | 21:3489cffad196 | 785 | |
JesiMiranda | 35:5a2fed4c2e9f | 786 | /*---------------------------------------------------------------------------------------------------*/ |
JesiMiranda | 35:5a2fed4c2e9f | 787 | /*---------------------------------------------------------------------------------------------------*/ |
JesiMiranda | 35:5a2fed4c2e9f | 788 | /*---------------------------------------------------------------------------------------------------*/ |
JesiMiranda | 35:5a2fed4c2e9f | 789 | /*---------------------------------------------------------------------------------------------------*/ |
JesiMiranda | 35:5a2fed4c2e9f | 790 | /*---------------------------------------------------------------------------------------------------*/ |
jvfausto | 21:3489cffad196 | 791 | |
JesiMiranda | 35:5a2fed4c2e9f | 792 | /*Predetermined paths For Demmo*/ |
JesiMiranda | 35:5a2fed4c2e9f | 793 | void Wheelchair::desk() |
jvfausto | 21:3489cffad196 | 794 | { |
jvfausto | 19:71a6621ee5c3 | 795 | Wheelchair::pid_forward(5461); |
jvfausto | 19:71a6621ee5c3 | 796 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 797 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 798 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 799 | Wheelchair::pid_forward(3658); |
jvfausto | 21:3489cffad196 | 800 | } |
JesiMiranda | 35:5a2fed4c2e9f | 801 | |
JesiMiranda | 35:5a2fed4c2e9f | 802 | void Wheelchair::kitchen() |
jvfausto | 21:3489cffad196 | 803 | { |
jvfausto | 19:71a6621ee5c3 | 804 | Wheelchair::pid_forward(5461); |
jvfausto | 20:f42db4ae16f0 | 805 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 806 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 807 | Wheelchair::pid_left(90); |
jvfausto | 19:71a6621ee5c3 | 808 | Wheelchair::pid_forward(305); |
jvfausto | 21:3489cffad196 | 809 | } |
JesiMiranda | 35:5a2fed4c2e9f | 810 | |
jvfausto | 21:3489cffad196 | 811 | void Wheelchair::desk_to_kitchen() |
jvfausto | 21:3489cffad196 | 812 | { |
jvfausto | 19:71a6621ee5c3 | 813 | Wheelchair::pid_right(180); |
jvfausto | 19:71a6621ee5c3 | 814 | Wheelchair::pid_forward(3700); |
jvfausto | 21:3489cffad196 | 815 | } |