Updated with Statistics Library
Dependencies: QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic
wheelchair.cpp@28:e01253eb6c6f, 2019-06-28 (annotated)
- Committer:
- t1jain
- Date:
- Fri Jun 28 23:52:23 2019 +0000
- Revision:
- 28:e01253eb6c6f
- Parent:
- 27:da718b990837
Updated with Statistics Library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jvfausto | 21:3489cffad196 | 1 | |
ryanlin97 | 0:fc0c4a184482 | 2 | #include "wheelchair.h" |
jvfausto | 21:3489cffad196 | 3 | |
jvfausto | 21:3489cffad196 | 4 | bool manual_drive = false; // Variable changes between joystick and auto drive |
jvfausto | 21:3489cffad196 | 5 | double encoder_distance; // Keeps distanse due to original position |
jvfausto | 21:3489cffad196 | 6 | |
jvfausto | 21:3489cffad196 | 7 | volatile double Setpoint, Output, Input, Input2; // Variables for PID |
jvfausto | 21:3489cffad196 | 8 | volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID |
jvfausto | 21:3489cffad196 | 9 | volatile double vIn, vOut, vDesired; // Variables for PID Velosity |
jvfausto | 21:3489cffad196 | 10 | volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel |
jvfausto | 21:3489cffad196 | 11 | volatile double yIn, yOut, yDesired; // Variables for PID turn velosity |
ryanlin97 | 11:d14a1f7f1297 | 12 | |
jvfausto | 21:3489cffad196 | 13 | double dist_old, curr_pos; // Variables for odometry position |
jvfausto | 19:71a6621ee5c3 | 14 | |
jvfausto | 21:3489cffad196 | 15 | |
jvfausto | 21:3489cffad196 | 16 | PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor |
jvfausto | 21:3489cffad196 | 17 | PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor |
jvfausto | 21:3489cffad196 | 18 | PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Velosity PID Constructor |
jvfausto | 21:3489cffad196 | 19 | PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); // Slave Velosity PID Constructor |
jvfausto | 21:3489cffad196 | 20 | PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Angular Velosity PID Constructor |
jvfausto | 21:3489cffad196 | 21 | |
jvfausto | 19:71a6621ee5c3 | 22 | |
jvfausto | 21:3489cffad196 | 23 | /* Thread measures current angular position */ |
jvfausto | 21:3489cffad196 | 24 | void Wheelchair::compass_thread() |
jvfausto | 21:3489cffad196 | 25 | { |
ryanlin97 | 11:d14a1f7f1297 | 26 | curr_yaw = imu->yaw(); |
jvfausto | 21:3489cffad196 | 27 | z_angular = curr_yaw; |
jvfausto | 21:3489cffad196 | 28 | } |
jvfausto | 17:7f3b69300bb6 | 29 | |
jvfausto | 21:3489cffad196 | 30 | /* Thread measures velocity of wheels and distance traveled */ |
jvfausto | 21:3489cffad196 | 31 | void Wheelchair::velocity_thread() |
ryanlin97 | 1:c0beadca1617 | 32 | { |
jvfausto | 21:3489cffad196 | 33 | curr_vel = wheel->getVelocity(); |
jvfausto | 21:3489cffad196 | 34 | curr_velS = wheelS->getVelocity(); |
jvfausto | 21:3489cffad196 | 35 | curr_pos = wheel->getDistance(53.975); |
ryanlin97 | 1:c0beadca1617 | 36 | } |
ryanlin97 | 6:0cd57bdd8fbc | 37 | |
t1jain | 28:e01253eb6c6f | 38 | void Wheelchair::emergencyButton_thread () |
t1jain | 28:e01253eb6c6f | 39 | { |
t1jain | 28:e01253eb6c6f | 40 | while(1) { |
t1jain | 28:e01253eb6c6f | 41 | while(!e_button) { |
t1jain | 28:e01253eb6c6f | 42 | |
t1jain | 28:e01253eb6c6f | 43 | //Stop wheelchair |
t1jain | 28:e01253eb6c6f | 44 | Wheelchair::stop(); |
t1jain | 28:e01253eb6c6f | 45 | printf("E-button has been pressed\n\n\n"); |
t1jain | 28:e01253eb6c6f | 46 | off->write(high); //turn off PCB |
t1jain | 28:e01253eb6c6f | 47 | on->write(0); //make sure PCB not on |
t1jain | 28:e01253eb6c6f | 48 | //Reset Board |
t1jain | 28:e01253eb6c6f | 49 | NVIC_SystemReset(); |
t1jain | 28:e01253eb6c6f | 50 | |
t1jain | 28:e01253eb6c6f | 51 | } |
t1jain | 28:e01253eb6c6f | 52 | |
t1jain | 28:e01253eb6c6f | 53 | } |
t1jain | 28:e01253eb6c6f | 54 | } |
t1jain | 28:e01253eb6c6f | 55 | |
jvfausto | 21:3489cffad196 | 56 | void Wheelchair::assistSafe_thread() |
ryanlin97 | 1:c0beadca1617 | 57 | { |
jvfausto | 21:3489cffad196 | 58 | int ToFV[12]; |
jvfausto | 26:662693bd7f31 | 59 | for(int i = 0; i < 6; i++) // reads from the ToF Sensors |
jvfausto | 21:3489cffad196 | 60 | { |
jvfausto | 21:3489cffad196 | 61 | ToFV[i] = (*(ToF+i))->readFromOneSensor(); |
jvfausto | 21:3489cffad196 | 62 | //out->printf("%d ", ToFV[i]); |
jvfausto | 27:da718b990837 | 63 | } |
jvfausto | 27:da718b990837 | 64 | |
jvfausto | 26:662693bd7f31 | 65 | //out->printf("\r\n"); |
jvfausto | 27:da718b990837 | 66 | |
jvfausto | 27:da718b990837 | 67 | |
jvfausto | 27:da718b990837 | 68 | for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors |
jvfausto | 27:da718b990837 | 69 | runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); |
jvfausto | 27:da718b990837 | 70 | } |
jvfausto | 27:da718b990837 | 71 | |
jvfausto | 27:da718b990837 | 72 | int sensor1 = ToFV[0]; |
jvfausto | 27:da718b990837 | 73 | int sensor4 = ToFV[3]; |
jvfausto | 27:da718b990837 | 74 | out->printf("%d, %d\r\n", ToFV[1], runningAverage[0]); |
jvfausto | 27:da718b990837 | 75 | if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || |
jvfausto | 26:662693bd7f31 | 76 | 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && |
jvfausto | 26:662693bd7f31 | 77 | (sensor1 < 1500 || sensor4 < 1500)) || |
jvfausto | 26:662693bd7f31 | 78 | 550 > sensor1 || 550 > sensor4) |
jvfausto | 26:662693bd7f31 | 79 | { |
jvfausto | 26:662693bd7f31 | 80 | //out->printf("i am in danger\r\n"); |
jvfausto | 26:662693bd7f31 | 81 | if(x->read() > def) |
jvfausto | 26:662693bd7f31 | 82 | { |
jvfausto | 26:662693bd7f31 | 83 | x->write(def); |
jvfausto | 26:662693bd7f31 | 84 | forwardSafety = 1; |
jvfausto | 26:662693bd7f31 | 85 | } |
jvfausto | 26:662693bd7f31 | 86 | } |
jvfausto | 26:662693bd7f31 | 87 | else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || |
jvfausto | 26:662693bd7f31 | 88 | 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && |
jvfausto | 21:3489cffad196 | 89 | (sensor1 < 1500 || sensor4 < 1500)) || |
jvfausto | 21:3489cffad196 | 90 | 550 > sensor1 || 550 > sensor4) |
jvfausto | 21:3489cffad196 | 91 | { |
jvfausto | 21:3489cffad196 | 92 | //out->printf("i am in danger\r\n"); |
jvfausto | 21:3489cffad196 | 93 | if(x->read() > def) |
jvfausto | 21:3489cffad196 | 94 | { |
jvfausto | 21:3489cffad196 | 95 | x->write(def); |
jvfausto | 21:3489cffad196 | 96 | forwardSafety = 1; |
jvfausto | 21:3489cffad196 | 97 | } |
jvfausto | 21:3489cffad196 | 98 | } |
jvfausto | 21:3489cffad196 | 99 | else |
jvfausto | 21:3489cffad196 | 100 | forwardSafety = 0; |
jvfausto | 27:da718b990837 | 101 | |
jvfausto | 21:3489cffad196 | 102 | } |
ryanlin97 | 6:0cd57bdd8fbc | 103 | |
jvfausto | 21:3489cffad196 | 104 | /* Constructor for Wheelchair class */ |
jvfausto | 21:3489cffad196 | 105 | Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, |
jvfausto | 21:3489cffad196 | 106 | VL53L1X** ToFT) |
jvfausto | 21:3489cffad196 | 107 | { |
jvfausto | 21:3489cffad196 | 108 | x_position = 0; |
jvfausto | 21:3489cffad196 | 109 | y_position = 0; |
jvfausto | 21:3489cffad196 | 110 | forwardSafety = 0; |
jvfausto | 21:3489cffad196 | 111 | /* Initializes X and Y variables to Pins */ |
jvfausto | 21:3489cffad196 | 112 | x = new PwmOut(xPin); |
jvfausto | 21:3489cffad196 | 113 | y = new PwmOut(yPin); |
jvfausto | 21:3489cffad196 | 114 | /* Initializes IMU Library */ |
jvfausto | 26:662693bd7f31 | 115 | out = pc; // "out" is called for serial monitor |
jvfausto | 26:662693bd7f31 | 116 | out->printf("on\r\n"); |
jvfausto | 21:3489cffad196 | 117 | imu = new chair_BNO055(pc, time); |
jvfausto | 21:3489cffad196 | 118 | Wheelchair::stop(); // Wheelchair is initially stationary |
jvfausto | 21:3489cffad196 | 119 | imu->setup(); // turns on the IMU |
jvfausto | 21:3489cffad196 | 120 | wheelS = qeiS; // "wheel" is called for encoder |
jvfausto | 21:3489cffad196 | 121 | wheel = qei; |
jvfausto | 21:3489cffad196 | 122 | ToF = ToFT; // passes pointer with addresses of ToF sensors |
jvfausto | 21:3489cffad196 | 123 | |
t1jain | 28:e01253eb6c6f | 124 | for(int i = 0; i < 12; i++) // initializes the ToF Sensors |
jvfausto | 21:3489cffad196 | 125 | { |
jvfausto | 21:3489cffad196 | 126 | (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); |
jvfausto | 21:3489cffad196 | 127 | } |
jvfausto | 21:3489cffad196 | 128 | |
jvfausto | 21:3489cffad196 | 129 | out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor |
jvfausto | 21:3489cffad196 | 130 | ti = time; |
jvfausto | 27:da718b990837 | 131 | for(int i = 0; i < 100; i++) |
jvfausto | 27:da718b990837 | 132 | { |
jvfausto | 27:da718b990837 | 133 | ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); |
jvfausto | 27:da718b990837 | 134 | ledgeArrayRF[i] = (*(ToF+1))->readFromOneSensor(); |
jvfausto | 27:da718b990837 | 135 | } |
jvfausto | 27:da718b990837 | 136 | int* aaa = ledgeArrayLF; |
t1jain | 28:e01253eb6c6f | 137 | |
t1jain | 28:e01253eb6c6f | 138 | statistics LFTStats(aaa, 99, 1); |
t1jain | 28:e01253eb6c6f | 139 | out->printf("Statistics: Mean = %f, Standard Dev = %f\n", LFTStats.mean(), LFTStats.stdev()); |
jvfausto | 21:3489cffad196 | 140 | myPID.SetMode(AUTOMATIC); // PID mode: Automatic |
jvfausto | 21:3489cffad196 | 141 | } |
jvfausto | 21:3489cffad196 | 142 | |
jvfausto | 21:3489cffad196 | 143 | /* Move wheelchair with joystick on manual mode */ |
jvfausto | 21:3489cffad196 | 144 | void Wheelchair::move(float x_coor, float y_coor) |
jvfausto | 21:3489cffad196 | 145 | { |
jvfausto | 21:3489cffad196 | 146 | /* Scales one joystick measurement to the chair's joystick measurement */ |
ryanlin97 | 4:29a27953fe70 | 147 | float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; |
ryanlin97 | 4:29a27953fe70 | 148 | float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; |
jvfausto | 21:3489cffad196 | 149 | |
jvfausto | 21:3489cffad196 | 150 | /* Sends the scaled joystic values to the chair */ |
jvfausto | 21:3489cffad196 | 151 | x->write(scaled_x); |
ryanlin97 | 4:29a27953fe70 | 152 | y->write(scaled_y); |
ryanlin97 | 5:e0ccaab3959a | 153 | } |
jvfausto | 21:3489cffad196 | 154 | |
jvfausto | 21:3489cffad196 | 155 | /* Automatic mode: move forward and update x,y coordinate sent to chair */ |
jvfausto | 21:3489cffad196 | 156 | void Wheelchair::forward() |
ryanlin97 | 1:c0beadca1617 | 157 | { |
jvfausto | 26:662693bd7f31 | 158 | //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS); |
jvfausto | 21:3489cffad196 | 159 | if(forwardSafety == 0) |
jvfausto | 21:3489cffad196 | 160 | { |
ryanlin97 | 0:fc0c4a184482 | 161 | x->write(high); |
ryanlin97 | 3:a5e71bfdb492 | 162 | y->write(def+offset); |
jvfausto | 21:3489cffad196 | 163 | } |
jvfausto | 26:662693bd7f31 | 164 | out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975)); |
ryanlin97 | 0:fc0c4a184482 | 165 | } |
jvfausto | 21:3489cffad196 | 166 | |
jvfausto | 21:3489cffad196 | 167 | /* Automatic mode: move in reverse and update x,y coordinate sent to chair */ |
jvfausto | 21:3489cffad196 | 168 | void Wheelchair::backward() |
ryanlin97 | 1:c0beadca1617 | 169 | { |
ryanlin97 | 0:fc0c4a184482 | 170 | x->write(low); |
ryanlin97 | 0:fc0c4a184482 | 171 | y->write(def); |
ryanlin97 | 0:fc0c4a184482 | 172 | } |
jvfausto | 21:3489cffad196 | 173 | |
jvfausto | 21:3489cffad196 | 174 | /* Automatic mode: move right and update x,y coordinate sent to chair */ |
jvfausto | 21:3489cffad196 | 175 | void Wheelchair::right() |
ryanlin97 | 1:c0beadca1617 | 176 | { |
ryanlin97 | 0:fc0c4a184482 | 177 | x->write(def); |
ryanlin97 | 11:d14a1f7f1297 | 178 | y->write(low); |
ryanlin97 | 0:fc0c4a184482 | 179 | } |
ryanlin97 | 0:fc0c4a184482 | 180 | |
jvfausto | 21:3489cffad196 | 181 | /* Automatic mode: move left and update x,y coordinate sent to chair */ |
jvfausto | 21:3489cffad196 | 182 | void Wheelchair::left() |
ryanlin97 | 1:c0beadca1617 | 183 | { |
ryanlin97 | 0:fc0c4a184482 | 184 | x->write(def); |
ryanlin97 | 11:d14a1f7f1297 | 185 | y->write(high); |
ryanlin97 | 0:fc0c4a184482 | 186 | } |
jvfausto | 21:3489cffad196 | 187 | |
jvfausto | 21:3489cffad196 | 188 | /* Stop the wheelchair */ |
jvfausto | 21:3489cffad196 | 189 | void Wheelchair::stop() |
ryanlin97 | 1:c0beadca1617 | 190 | { |
ryanlin97 | 0:fc0c4a184482 | 191 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 192 | y->write(def); |
ryanlin97 | 6:0cd57bdd8fbc | 193 | } |
jvfausto | 21:3489cffad196 | 194 | |
jvfausto | 21:3489cffad196 | 195 | /* Counter-clockwise is - |
jvfausto | 21:3489cffad196 | 196 | * Clockwise is + |
jvfausto | 21:3489cffad196 | 197 | * Range of deg: 0 to 360 |
jvfausto | 21:3489cffad196 | 198 | * This constructor takes in an angle from user and adjusts for turning right |
jvfausto | 21:3489cffad196 | 199 | */ |
jvfausto | 21:3489cffad196 | 200 | void Wheelchair::pid_right(int deg) |
ryanlin97 | 12:921488918749 | 201 | { |
t1jain | 28:e01253eb6c6f | 202 | bool overturn = false; // Boolean if angle over 360˚ |
ryanlin97 | 12:921488918749 | 203 | |
jvfausto | 21:3489cffad196 | 204 | out->printf("pid right\r\r\n"); |
jvfausto | 21:3489cffad196 | 205 | x->write(def); // Update x sent to chair to be stationary |
jvfausto | 21:3489cffad196 | 206 | Setpoint = curr_yaw + deg; // Relative angle we want to turn |
jvfausto | 21:3489cffad196 | 207 | pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user |
jvfausto | 21:3489cffad196 | 208 | |
jvfausto | 21:3489cffad196 | 209 | /* Turns on overturn boolean if setpoint over 360˚ */ |
jvfausto | 21:3489cffad196 | 210 | if(Setpoint > 360) |
jvfausto | 21:3489cffad196 | 211 | { |
ryanlin97 | 12:921488918749 | 212 | overturn = true; |
ryanlin97 | 12:921488918749 | 213 | } |
jvfausto | 21:3489cffad196 | 214 | |
jvfausto | 21:3489cffad196 | 215 | myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 216 | myPID.SetOutputLimits(0, def-low-.15); // Limit is set to the differnce between def and low |
jvfausto | 21:3489cffad196 | 217 | myPID.SetControllerDirection(DIRECT); // PID mode: Direct |
jvfausto | 21:3489cffad196 | 218 | |
jvfausto | 21:3489cffad196 | 219 | /* PID stops when approaching a litte less than desired angle */ |
jvfausto | 21:3489cffad196 | 220 | while(pid_yaw < Setpoint - 3) |
jvfausto | 21:3489cffad196 | 221 | { |
jvfausto | 21:3489cffad196 | 222 | /* PID is set to correct angle range if angle greater than 360˚*/ |
jvfausto | 17:7f3b69300bb6 | 223 | if(overturn && curr_yaw < Setpoint-deg-1) |
jvfausto | 17:7f3b69300bb6 | 224 | { |
jvfausto | 21:3489cffad196 | 225 | pid_yaw = curr_yaw + 360; |
jvfausto | 21:3489cffad196 | 226 | } |
jvfausto | 21:3489cffad196 | 227 | else |
jvfausto | 21:3489cffad196 | 228 | { |
jvfausto | 17:7f3b69300bb6 | 229 | pid_yaw = curr_yaw; |
ryanlin97 | 12:921488918749 | 230 | } |
jvfausto | 21:3489cffad196 | 231 | |
jvfausto | 21:3489cffad196 | 232 | myPID.Compute(); // Does PID calculations |
jvfausto | 21:3489cffad196 | 233 | double tempor = -Output+def; // Temporary value with the voltage output |
jvfausto | 21:3489cffad196 | 234 | y->write(tempor); // Update y sent to chair |
jvfausto | 21:3489cffad196 | 235 | |
jvfausto | 21:3489cffad196 | 236 | /* Prints to serial monitor the current angle and setpoint */ |
jvfausto | 21:3489cffad196 | 237 | out->printf("curr_yaw %f\r\r\n", curr_yaw); |
jvfausto | 21:3489cffad196 | 238 | out->printf("Setpoint = %f \r\n", Setpoint); |
jvfausto | 21:3489cffad196 | 239 | |
jvfausto | 21:3489cffad196 | 240 | wait(.05); // Small delay (milliseconds) |
ryanlin97 | 12:921488918749 | 241 | } |
jvfausto | 21:3489cffad196 | 242 | |
jvfausto | 21:3489cffad196 | 243 | /* Saftey stop for wheelchair */ |
jvfausto | 21:3489cffad196 | 244 | Wheelchair::stop(); |
jvfausto | 21:3489cffad196 | 245 | out->printf("done \r\n"); |
jvfausto | 21:3489cffad196 | 246 | } |
jvfausto | 21:3489cffad196 | 247 | |
jvfausto | 21:3489cffad196 | 248 | /* Counter-clockwise is - |
jvfausto | 21:3489cffad196 | 249 | * Clockwise is + |
jvfausto | 21:3489cffad196 | 250 | * Range of deg: 0 to 360 |
jvfausto | 21:3489cffad196 | 251 | * This constructor takes in an angle from user and adjusts for turning left |
jvfausto | 21:3489cffad196 | 252 | */ |
jvfausto | 21:3489cffad196 | 253 | void Wheelchair::pid_left(int deg) |
ryanlin97 | 12:921488918749 | 254 | { |
jvfausto | 21:3489cffad196 | 255 | bool overturn = false; //Boolean if angle under 0˚ |
ryanlin97 | 12:921488918749 | 256 | |
jvfausto | 21:3489cffad196 | 257 | out->printf("pid Left\r\r\n"); |
jvfausto | 21:3489cffad196 | 258 | x->write(def); // Update x sent to chair to be stationary |
jvfausto | 21:3489cffad196 | 259 | Setpoint = curr_yaw - deg; // Relative angle we want to turn |
jvfausto | 21:3489cffad196 | 260 | pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user |
jvfausto | 21:3489cffad196 | 261 | |
jvfausto | 21:3489cffad196 | 262 | /* Turns on overturn boolean if setpoint less than 0˚ */ |
jvfausto | 21:3489cffad196 | 263 | if(Setpoint < 0) |
jvfausto | 21:3489cffad196 | 264 | { |
ryanlin97 | 12:921488918749 | 265 | overturn = true; |
ryanlin97 | 12:921488918749 | 266 | } |
jvfausto | 21:3489cffad196 | 267 | |
jvfausto | 21:3489cffad196 | 268 | myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 269 | myPID.SetOutputLimits(0,high-def-.12); //Limit is set to the differnce between def and low |
jvfausto | 21:3489cffad196 | 270 | myPID.SetControllerDirection(REVERSE); // PID mode: Reverse |
jvfausto | 21:3489cffad196 | 271 | |
jvfausto | 21:3489cffad196 | 272 | /* PID stops when approaching a litte more than desired angle */ |
jvfausto | 21:3489cffad196 | 273 | while(pid_yaw > Setpoint+3) |
jvfausto | 21:3489cffad196 | 274 | { |
jvfausto | 21:3489cffad196 | 275 | /* PID is set to correct angle range if angle less than 0˚ */ |
jvfausto | 21:3489cffad196 | 276 | if(overturn && curr_yaw > Setpoint+deg+1) |
jvfausto | 17:7f3b69300bb6 | 277 | { |
jvfausto | 17:7f3b69300bb6 | 278 | pid_yaw = curr_yaw - 360; |
jvfausto | 21:3489cffad196 | 279 | } |
jvfausto | 21:3489cffad196 | 280 | else |
jvfausto | 21:3489cffad196 | 281 | { |
jvfausto | 21:3489cffad196 | 282 | pid_yaw = curr_yaw; |
jvfausto | 21:3489cffad196 | 283 | } |
jvfausto | 21:3489cffad196 | 284 | |
jvfausto | 21:3489cffad196 | 285 | myPID.Compute(); // Does PID calculations |
jvfausto | 21:3489cffad196 | 286 | double tempor = Output+def; // Temporary value with the voltage output |
jvfausto | 21:3489cffad196 | 287 | y->write(tempor); // Update y sent to chair |
jvfausto | 21:3489cffad196 | 288 | |
jvfausto | 21:3489cffad196 | 289 | /* Prints to serial monitor the current angle and setpoint */ |
jvfausto | 17:7f3b69300bb6 | 290 | out->printf("curr_yaw %f\r\n", curr_yaw); |
jvfausto | 21:3489cffad196 | 291 | out->printf("Setpoint = %f \r\n", Setpoint); |
jvfausto | 21:3489cffad196 | 292 | |
jvfausto | 21:3489cffad196 | 293 | wait(.05); // Small delay (milliseconds) |
ryanlin97 | 12:921488918749 | 294 | } |
jvfausto | 21:3489cffad196 | 295 | |
jvfausto | 21:3489cffad196 | 296 | /* Saftey stop for wheelchair */ |
jvfausto | 21:3489cffad196 | 297 | Wheelchair::stop(); |
jvfausto | 21:3489cffad196 | 298 | out->printf("done \r\n"); |
ryanlin97 | 12:921488918749 | 299 | |
jvfausto | 21:3489cffad196 | 300 | } |
jvfausto | 21:3489cffad196 | 301 | |
jvfausto | 21:3489cffad196 | 302 | /* This constructor determines whether to turn left or right */ |
jvfausto | 21:3489cffad196 | 303 | void Wheelchair::pid_turn(int deg) |
jvfausto | 21:3489cffad196 | 304 | { |
jvfausto | 21:3489cffad196 | 305 | |
jvfausto | 21:3489cffad196 | 306 | /* Sets angle to coterminal angle for left turn if deg > 180 |
jvfausto | 21:3489cffad196 | 307 | * Sets angle to coterminal angle for right turn if deg < -180 |
jvfausto | 21:3489cffad196 | 308 | */ |
jvfausto | 21:3489cffad196 | 309 | if(deg > 180) |
jvfausto | 21:3489cffad196 | 310 | { |
ryanlin97 | 12:921488918749 | 311 | deg -= 360; |
ryanlin97 | 12:921488918749 | 312 | } |
jvfausto | 21:3489cffad196 | 313 | else if(deg < -180) |
jvfausto | 21:3489cffad196 | 314 | { |
jvfausto | 21:3489cffad196 | 315 | deg +=360; |
ryanlin97 | 12:921488918749 | 316 | } |
ryanlin97 | 12:921488918749 | 317 | |
jvfausto | 21:3489cffad196 | 318 | /* Makes sure angle inputted to function is positive */ |
ryanlin97 | 12:921488918749 | 319 | int turnAmt = abs(deg); |
jvfausto | 21:3489cffad196 | 320 | |
jvfausto | 21:3489cffad196 | 321 | /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */ |
jvfausto | 21:3489cffad196 | 322 | if(deg >= 0) |
jvfausto | 21:3489cffad196 | 323 | { |
jvfausto | 21:3489cffad196 | 324 | Wheelchair::pid_right(turnAmt); |
jvfausto | 21:3489cffad196 | 325 | } |
jvfausto | 21:3489cffad196 | 326 | else |
jvfausto | 21:3489cffad196 | 327 | { |
jvfausto | 21:3489cffad196 | 328 | Wheelchair::pid_left(turnAmt); |
jvfausto | 21:3489cffad196 | 329 | } |
ryanlin97 | 12:921488918749 | 330 | |
jvfausto | 21:3489cffad196 | 331 | } |
jvfausto | 21:3489cffad196 | 332 | |
jvfausto | 21:3489cffad196 | 333 | /* This constructor takes in distance to travel and adjust to move forward */ |
jvfausto | 19:71a6621ee5c3 | 334 | void Wheelchair::pid_forward(double mm) |
jvfausto | 17:7f3b69300bb6 | 335 | { |
jvfausto | 21:3489cffad196 | 336 | mm -= 20; // Makes sure distance does not overshoot |
jvfausto | 21:3489cffad196 | 337 | Input = 0; // Initializes input to zero: Test latter w/o |
jvfausto | 21:3489cffad196 | 338 | wheel->reset(); // Resets encoders so that they start at 0 |
jvfausto | 21:3489cffad196 | 339 | |
jvfausto | 17:7f3b69300bb6 | 340 | out->printf("pid foward\r\n"); |
jvfausto | 21:3489cffad196 | 341 | |
jvfausto | 21:3489cffad196 | 342 | double tempor; // Initializes Temporary variable for x input |
jvfausto | 21:3489cffad196 | 343 | Setpoint = mm; // Initializes the setpoint to desired value |
jvfausto | 21:3489cffad196 | 344 | |
jvfausto | 21:3489cffad196 | 345 | myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D |
jvfausto | 21:3489cffad196 | 346 | myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def |
jvfausto | 21:3489cffad196 | 347 | myPIDDistance.SetControllerDirection(DIRECT); // PID mode: Direct |
jvfausto | 21:3489cffad196 | 348 | |
jvfausto | 21:3489cffad196 | 349 | y->write(def+offset); // Update y to make chair stationary |
jvfausto | 21:3489cffad196 | 350 | |
jvfausto | 21:3489cffad196 | 351 | /* Chair stops moving when Setpoint is reached */ |
jvfausto | 21:3489cffad196 | 352 | while(Input < Setpoint){ |
jvfausto | 21:3489cffad196 | 353 | |
jvfausto | 21:3489cffad196 | 354 | if(out->readable()) // Emergency Break |
jvfausto | 21:3489cffad196 | 355 | { |
jvfausto | 21:3489cffad196 | 356 | break; |
jvfausto | 21:3489cffad196 | 357 | } |
jvfausto | 17:7f3b69300bb6 | 358 | |
jvfausto | 21:3489cffad196 | 359 | Input = wheel->getDistance(53.975); // Gets distance from Encoder into PID |
jvfausto | 21:3489cffad196 | 360 | wait(.05); // Slight Delay: *****Test without |
jvfausto | 21:3489cffad196 | 361 | myPIDDistance.Compute(); // Compute distance traveled by chair |
jvfausto | 21:3489cffad196 | 362 | |
jvfausto | 21:3489cffad196 | 363 | tempor = Output + def; // Temporary output variable |
jvfausto | 21:3489cffad196 | 364 | x->write(tempor); // Update x sent to chair |
jvfausto | 17:7f3b69300bb6 | 365 | |
jvfausto | 21:3489cffad196 | 366 | /* Prints to serial monitor the distance traveled by chair */ |
jvfausto | 19:71a6621ee5c3 | 367 | out->printf("distance %f\r\n", Input); |
jvfausto | 17:7f3b69300bb6 | 368 | } |
ryanlin97 | 12:921488918749 | 369 | |
jvfausto | 17:7f3b69300bb6 | 370 | } |
jvfausto | 21:3489cffad196 | 371 | |
jvfausto | 21:3489cffad196 | 372 | /* This constructor returns the relative angular position of chair */ |
jvfausto | 21:3489cffad196 | 373 | double Wheelchair::getTwistZ() |
jvfausto | 17:7f3b69300bb6 | 374 | { |
jvfausto | 21:3489cffad196 | 375 | return imu->gyro_z(); |
jvfausto | 21:3489cffad196 | 376 | } |
jvfausto | 18:663b6d693252 | 377 | |
jvfausto | 21:3489cffad196 | 378 | /* This constructor computes the relative angle for Twist message in ROS */ |
jvfausto | 21:3489cffad196 | 379 | void Wheelchair::pid_twistA() |
jvfausto | 21:3489cffad196 | 380 | { |
jvfausto | 21:3489cffad196 | 381 | /* Initialize variables for angle and update x,y sent to chair */ |
jvfausto | 21:3489cffad196 | 382 | char c; |
jvfausto | 21:3489cffad196 | 383 | double temporA = def; |
jvfausto | 21:3489cffad196 | 384 | y->write(def); |
jvfausto | 21:3489cffad196 | 385 | x->write(def); |
jvfausto | 21:3489cffad196 | 386 | |
jvfausto | 21:3489cffad196 | 387 | PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 388 | PIDAngularV.SetOutputLimits(-.1, .1); // Limit set to be in range specified |
jvfausto | 21:3489cffad196 | 389 | PIDAngularV.SetControllerDirection(DIRECT); // PID mode: Direct |
jvfausto | 21:3489cffad196 | 390 | |
jvfausto | 21:3489cffad196 | 391 | /* Computes angular position of wheelchair while turning */ |
jvfausto | 21:3489cffad196 | 392 | while(1) |
jvfausto | 21:3489cffad196 | 393 | { |
jvfausto | 21:3489cffad196 | 394 | yDesired = angularV; |
jvfausto | 21:3489cffad196 | 395 | |
jvfausto | 21:3489cffad196 | 396 | /* Update and set all variable so that the chair is stationary |
jvfausto | 21:3489cffad196 | 397 | * if the desired angle is zero |
jvfausto | 21:3489cffad196 | 398 | */ |
jvfausto | 21:3489cffad196 | 399 | if(yDesired == 0) |
jvfausto | 18:663b6d693252 | 400 | { |
jvfausto | 21:3489cffad196 | 401 | x->write(def); |
jvfausto | 21:3489cffad196 | 402 | y->write(def); |
jvfausto | 21:3489cffad196 | 403 | yDesired = 0; |
ryanlin97 | 8:381a4ec3fef8 | 404 | return; |
ryanlin97 | 7:5e38d43fbce3 | 405 | } |
jvfausto | 21:3489cffad196 | 406 | |
jvfausto | 21:3489cffad196 | 407 | /* Continuously updates with current angle measured by IMU */ |
jvfausto | 21:3489cffad196 | 408 | yIn = imu->gyro_z(); |
jvfausto | 21:3489cffad196 | 409 | PIDAngularV.Compute(); |
jvfausto | 21:3489cffad196 | 410 | temporA += yOut; // Temporary value with the voltage output |
jvfausto | 21:3489cffad196 | 411 | y->write(temporA); // Update y sent to chair |
jvfausto | 21:3489cffad196 | 412 | |
jvfausto | 21:3489cffad196 | 413 | //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); |
jvfausto | 21:3489cffad196 | 414 | wait(.05); // Small delay (milliseconds) |
ryanlin97 | 6:0cd57bdd8fbc | 415 | } |
jvfausto | 21:3489cffad196 | 416 | |
jvfausto | 21:3489cffad196 | 417 | } |
ryanlin97 | 6:0cd57bdd8fbc | 418 | |
jvfausto | 21:3489cffad196 | 419 | /* This constructor computes the relative velocity for Twist message in ROS */ |
jvfausto | 21:3489cffad196 | 420 | void Wheelchair::pid_twistV() |
ryanlin97 | 6:0cd57bdd8fbc | 421 | { |
jvfausto | 21:3489cffad196 | 422 | /* Initializes variables as default */ |
jvfausto | 21:3489cffad196 | 423 | double temporV = def; |
jvfausto | 26:662693bd7f31 | 424 | double temporS = def+offset; |
jvfausto | 21:3489cffad196 | 425 | vDesiredS = 0; |
jvfausto | 21:3489cffad196 | 426 | x->write(def); |
jvfausto | 21:3489cffad196 | 427 | y->write(def); |
jvfausto | 21:3489cffad196 | 428 | wheel->reset(); // Resets the encoders |
jvfausto | 21:3489cffad196 | 429 | /* Sets the constants for P and D */ |
jvfausto | 21:3489cffad196 | 430 | PIDVelosity.SetTunings(.0005,0, 0.00); |
jvfausto | 26:662693bd7f31 | 431 | PIDSlaveV.SetTunings(.005,0.000001, 0.000001); |
jvfausto | 21:3489cffad196 | 432 | |
jvfausto | 21:3489cffad196 | 433 | /* Limits to the range specified */ |
jvfausto | 21:3489cffad196 | 434 | PIDVelosity.SetOutputLimits(-.005, .005); |
jvfausto | 21:3489cffad196 | 435 | PIDSlaveV.SetOutputLimits(-.002, .002); |
jvfausto | 21:3489cffad196 | 436 | |
jvfausto | 21:3489cffad196 | 437 | /* PID mode: Direct */ |
jvfausto | 21:3489cffad196 | 438 | PIDVelosity.SetControllerDirection(DIRECT); |
jvfausto | 21:3489cffad196 | 439 | PIDSlaveV.SetControllerDirection(DIRECT); |
jvfausto | 21:3489cffad196 | 440 | |
jvfausto | 21:3489cffad196 | 441 | while(1) |
jvfausto | 21:3489cffad196 | 442 | { |
jvfausto | 21:3489cffad196 | 443 | linearV = .7; |
jvfausto | 21:3489cffad196 | 444 | test1 = linearV*100; |
jvfausto | 21:3489cffad196 | 445 | vel = curr_vel; |
jvfausto | 21:3489cffad196 | 446 | vDesired = linearV*100; |
jvfausto | 21:3489cffad196 | 447 | if(out->readable()) |
jvfausto | 21:3489cffad196 | 448 | return; |
jvfausto | 21:3489cffad196 | 449 | /* Update and set all variable so that the chair is stationary |
jvfausto | 21:3489cffad196 | 450 | * if the velocity is zero |
jvfausto | 21:3489cffad196 | 451 | */ |
jvfausto | 21:3489cffad196 | 452 | if(linearV == 0) |
jvfausto | 21:3489cffad196 | 453 | { |
jvfausto | 21:3489cffad196 | 454 | x->write(def); |
jvfausto | 21:3489cffad196 | 455 | y->write(def); |
ryanlin97 | 8:381a4ec3fef8 | 456 | |
jvfausto | 21:3489cffad196 | 457 | vel = 0; |
jvfausto | 21:3489cffad196 | 458 | vDesired = 0; |
jvfausto | 21:3489cffad196 | 459 | dist_old = 0; |
ryanlin97 | 8:381a4ec3fef8 | 460 | return; |
ryanlin97 | 8:381a4ec3fef8 | 461 | } |
jvfausto | 21:3489cffad196 | 462 | |
jvfausto | 21:3489cffad196 | 463 | if(vDesired >= 0) |
jvfausto | 21:3489cffad196 | 464 | { |
jvfausto | 21:3489cffad196 | 465 | PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 466 | PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the range specified |
jvfausto | 21:3489cffad196 | 467 | } |
jvfausto | 21:3489cffad196 | 468 | else |
jvfausto | 21:3489cffad196 | 469 | { |
jvfausto | 21:3489cffad196 | 470 | PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D |
jvfausto | 21:3489cffad196 | 471 | PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to range specified |
jvfausto | 21:3489cffad196 | 472 | } |
jvfausto | 21:3489cffad196 | 473 | |
jvfausto | 21:3489cffad196 | 474 | /* Sets maximum value of variable to 1 */ |
jvfausto | 21:3489cffad196 | 475 | if(temporV >= 1.5) |
jvfausto | 21:3489cffad196 | 476 | { |
jvfausto | 21:3489cffad196 | 477 | temporV = 1.5; |
ryanlin97 | 8:381a4ec3fef8 | 478 | } |
jvfausto | 21:3489cffad196 | 479 | /* Scales and makes some adjustments to velocity */ |
jvfausto | 21:3489cffad196 | 480 | vIn = curr_vel*100; |
jvfausto | 21:3489cffad196 | 481 | vInS = curr_vel-curr_velS; |
jvfausto | 21:3489cffad196 | 482 | PIDVelosity.Compute(); |
jvfausto | 21:3489cffad196 | 483 | PIDSlaveV.Compute(); |
jvfausto | 21:3489cffad196 | 484 | if(forwardSafety == 0) |
jvfausto | 21:3489cffad196 | 485 | { |
jvfausto | 21:3489cffad196 | 486 | temporV += vOut; |
jvfausto | 21:3489cffad196 | 487 | temporS += vOutS; |
jvfausto | 21:3489cffad196 | 488 | |
jvfausto | 21:3489cffad196 | 489 | /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ |
jvfausto | 21:3489cffad196 | 490 | x->write(temporV); |
jvfausto | 21:3489cffad196 | 491 | test2 = temporV; |
jvfausto | 21:3489cffad196 | 492 | y->write(temporS); |
jvfausto | 21:3489cffad196 | 493 | } |
jvfausto | 21:3489cffad196 | 494 | else |
jvfausto | 21:3489cffad196 | 495 | { |
jvfausto | 21:3489cffad196 | 496 | x->write(def); |
jvfausto | 21:3489cffad196 | 497 | y->write(def); |
jvfausto | 21:3489cffad196 | 498 | } |
jvfausto | 21:3489cffad196 | 499 | //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS); |
jvfausto | 21:3489cffad196 | 500 | Wheelchair::odomMsg(); |
jvfausto | 21:3489cffad196 | 501 | wait(.01); // Small delay (milliseconds) |
ryanlin97 | 6:0cd57bdd8fbc | 502 | } |
ryanlin97 | 11:d14a1f7f1297 | 503 | } |
ryanlin97 | 11:d14a1f7f1297 | 504 | |
jvfausto | 21:3489cffad196 | 505 | /* This constructor calculates the relative position of the chair everytime the encoders reset |
jvfausto | 21:3489cffad196 | 506 | * by setting its old position as the origin to calculate the new position |
jvfausto | 21:3489cffad196 | 507 | */ |
jvfausto | 21:3489cffad196 | 508 | void Wheelchair::odomMsg() |
ryanlin97 | 11:d14a1f7f1297 | 509 | { |
jvfausto | 21:3489cffad196 | 510 | double dist_new = curr_pos; |
jvfausto | 21:3489cffad196 | 511 | double dist = dist_new-dist_old; |
jvfausto | 21:3489cffad196 | 512 | double temp_x = dist*sin(z_angular*3.14159/180); |
jvfausto | 21:3489cffad196 | 513 | double temp_y = dist*cos(z_angular*3.14159/180); |
jvfausto | 21:3489cffad196 | 514 | |
jvfausto | 21:3489cffad196 | 515 | x_position += temp_x; |
jvfausto | 21:3489cffad196 | 516 | y_position += temp_y; |
ryanlin97 | 11:d14a1f7f1297 | 517 | |
jvfausto | 21:3489cffad196 | 518 | dist_old = dist_new; |
jvfausto | 21:3489cffad196 | 519 | } |
jvfausto | 21:3489cffad196 | 520 | |
jvfausto | 21:3489cffad196 | 521 | /* This constructor prints the Odometry message to the serial monitor */ |
jvfausto | 21:3489cffad196 | 522 | void Wheelchair::showOdom() |
jvfausto | 21:3489cffad196 | 523 | { |
jvfausto | 21:3489cffad196 | 524 | out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); |
jvfausto | 21:3489cffad196 | 525 | } |
jvfausto | 21:3489cffad196 | 526 | |
jvfausto | 21:3489cffad196 | 527 | /* This constructor returns the approximate distance based on the wheel diameter */ |
jvfausto | 21:3489cffad196 | 528 | float Wheelchair::getDistance() |
jvfausto | 21:3489cffad196 | 529 | { |
jvfausto | 21:3489cffad196 | 530 | return wheel->getDistance(Diameter); |
ryanlin97 | 6:0cd57bdd8fbc | 531 | } |
ryanlin97 | 8:381a4ec3fef8 | 532 | |
jvfausto | 21:3489cffad196 | 533 | /* This constructor resets the wheel encoder's */ |
jvfausto | 21:3489cffad196 | 534 | void Wheelchair::resetDistance() |
jvfausto | 21:3489cffad196 | 535 | { |
ryanlin97 | 12:921488918749 | 536 | wheel->reset(); |
jvfausto | 21:3489cffad196 | 537 | } |
jvfausto | 21:3489cffad196 | 538 | |
jvfausto | 21:3489cffad196 | 539 | |
jvfausto | 21:3489cffad196 | 540 | /*Predetermined paths For Demmo*/ |
jvfausto | 21:3489cffad196 | 541 | void Wheelchair::desk() |
jvfausto | 21:3489cffad196 | 542 | { |
jvfausto | 19:71a6621ee5c3 | 543 | Wheelchair::pid_forward(5461); |
jvfausto | 19:71a6621ee5c3 | 544 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 545 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 546 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 547 | Wheelchair::pid_forward(3658); |
jvfausto | 21:3489cffad196 | 548 | } |
jvfausto | 21:3489cffad196 | 549 | |
jvfausto | 21:3489cffad196 | 550 | void Wheelchair::kitchen() |
jvfausto | 21:3489cffad196 | 551 | { |
jvfausto | 19:71a6621ee5c3 | 552 | Wheelchair::pid_forward(5461); |
jvfausto | 20:f42db4ae16f0 | 553 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 554 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 555 | Wheelchair::pid_left(90); |
jvfausto | 19:71a6621ee5c3 | 556 | Wheelchair::pid_forward(305); |
jvfausto | 21:3489cffad196 | 557 | } |
jvfausto | 21:3489cffad196 | 558 | |
jvfausto | 21:3489cffad196 | 559 | void Wheelchair::desk_to_kitchen() |
jvfausto | 21:3489cffad196 | 560 | { |
jvfausto | 19:71a6621ee5c3 | 561 | Wheelchair::pid_right(180); |
jvfausto | 19:71a6621ee5c3 | 562 | Wheelchair::pid_forward(3700); |
jvfausto | 21:3489cffad196 | 563 | } |