Added Back ToF code
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-6 Version1-7
wheelchair.cpp@23:8d11d953ceeb, 2018-10-16 (annotated)
- Committer:
- jvfausto
- Date:
- Tue Oct 16 23:03:40 2018 +0000
- Revision:
- 23:8d11d953ceeb
- Parent:
- 20:f42db4ae16f0
- Child:
- 24:d2f234fbc20d
revision
Who changed what in which revision?
User | Revision | Line number | New 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 | 23:8d11d953ceeb | 4 | double curr_yaw; // 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 | 19:71a6621ee5c3 | 9 | |
jvfausto | 23:8d11d953ceeb | 10 | PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor |
jvfausto | 23:8d11d953ceeb | 11 | PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor |
jvfausto | 23:8d11d953ceeb | 12 | |
jvfausto | 23:8d11d953ceeb | 13 | void Wheelchair::compass_thread() { // Thread that measures which angle we are at |
ryanlin97 | 11:d14a1f7f1297 | 14 | curr_yaw = imu->yaw(); |
ryanlin97 | 11:d14a1f7f1297 | 15 | } |
ryanlin97 | 11:d14a1f7f1297 | 16 | |
jvfausto | 23:8d11d953ceeb | 17 | Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei) // Function Constructor for Wheelchair class |
ryanlin97 | 1:c0beadca1617 | 18 | { |
jvfausto | 23:8d11d953ceeb | 19 | //Initializes X and Y variables to Pins |
jvfausto | 23:8d11d953ceeb | 20 | x = new PwmOut(xPin); |
ryanlin97 | 3:a5e71bfdb492 | 21 | y = new PwmOut(yPin); |
jvfausto | 23:8d11d953ceeb | 22 | |
jvfausto | 23:8d11d953ceeb | 23 | // Initializes IMU Library |
ryanlin97 | 11:d14a1f7f1297 | 24 | imu = new chair_BNO055(pc, time); |
jvfausto | 23:8d11d953ceeb | 25 | Wheelchair::stop(); // Wheelchair is not moving when initializing |
jvfausto | 23:8d11d953ceeb | 26 | imu->setup(); // turns on the IMU |
jvfausto | 23:8d11d953ceeb | 27 | out = pc; // "out" is called for serial monitor |
jvfausto | 23:8d11d953ceeb | 28 | wheel = qei; // "wheel" is called for encoder |
jvfausto | 23:8d11d953ceeb | 29 | out->printf("wheelchair setup done \r\n"); // make sure it initialized |
ryanlin97 | 11:d14a1f7f1297 | 30 | ti = time; |
jvfausto | 23:8d11d953ceeb | 31 | myPID.SetMode(AUTOMATIC); // set PID to automatic |
ryanlin97 | 1:c0beadca1617 | 32 | } |
ryanlin97 | 6:0cd57bdd8fbc | 33 | |
jvfausto | 23:8d11d953ceeb | 34 | void Wheelchair::move(float x_coor, float y_coor) // moves the chair with joystick on manual |
ryanlin97 | 1:c0beadca1617 | 35 | { |
ryanlin97 | 6:0cd57bdd8fbc | 36 | |
jvfausto | 23:8d11d953ceeb | 37 | float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; // Scales one joystic measurement to the |
jvfausto | 23:8d11d953ceeb | 38 | float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; // chair's joystic measurement |
ryanlin97 | 11:d14a1f7f1297 | 39 | |
jvfausto | 23:8d11d953ceeb | 40 | x->write(scaled_x); // Sends the scaled joystic values to the chair |
ryanlin97 | 4:29a27953fe70 | 41 | y->write(scaled_y); |
ryanlin97 | 5:e0ccaab3959a | 42 | } |
ryanlin97 | 11:d14a1f7f1297 | 43 | |
jvfausto | 23:8d11d953ceeb | 44 | void Wheelchair::forward() // In auto to move foward |
ryanlin97 | 1:c0beadca1617 | 45 | { |
ryanlin97 | 0:fc0c4a184482 | 46 | x->write(high); |
ryanlin97 | 3:a5e71bfdb492 | 47 | y->write(def+offset); |
ryanlin97 | 0:fc0c4a184482 | 48 | } |
ryanlin97 | 0:fc0c4a184482 | 49 | |
jvfausto | 23:8d11d953ceeb | 50 | void Wheelchair::backward() // In auto to move reverse |
ryanlin97 | 1:c0beadca1617 | 51 | { |
ryanlin97 | 0:fc0c4a184482 | 52 | x->write(low); |
ryanlin97 | 0:fc0c4a184482 | 53 | y->write(def); |
ryanlin97 | 0:fc0c4a184482 | 54 | } |
ryanlin97 | 0:fc0c4a184482 | 55 | |
jvfausto | 23:8d11d953ceeb | 56 | void Wheelchair::right() // In auto to move right |
ryanlin97 | 1:c0beadca1617 | 57 | { |
ryanlin97 | 0:fc0c4a184482 | 58 | x->write(def); |
ryanlin97 | 11:d14a1f7f1297 | 59 | y->write(low); |
ryanlin97 | 0:fc0c4a184482 | 60 | } |
ryanlin97 | 0:fc0c4a184482 | 61 | |
jvfausto | 23:8d11d953ceeb | 62 | void Wheelchair::left() // In auto to move left |
ryanlin97 | 1:c0beadca1617 | 63 | { |
ryanlin97 | 0:fc0c4a184482 | 64 | x->write(def); |
ryanlin97 | 11:d14a1f7f1297 | 65 | y->write(high); |
ryanlin97 | 0:fc0c4a184482 | 66 | } |
ryanlin97 | 0:fc0c4a184482 | 67 | |
jvfausto | 23:8d11d953ceeb | 68 | void Wheelchair::stop() // Stops the chair |
ryanlin97 | 1:c0beadca1617 | 69 | { |
ryanlin97 | 0:fc0c4a184482 | 70 | x->write(def); |
ryanlin97 | 0:fc0c4a184482 | 71 | y->write(def); |
ryanlin97 | 6:0cd57bdd8fbc | 72 | } |
ryanlin97 | 11:d14a1f7f1297 | 73 | // counter clockwise is - |
ryanlin97 | 11:d14a1f7f1297 | 74 | // clockwise is + |
jvfausto | 23:8d11d953ceeb | 75 | void Wheelchair::pid_right(int deg) // Takes in degree and turns right |
ryanlin97 | 12:921488918749 | 76 | { |
jvfausto | 23:8d11d953ceeb | 77 | bool overturn = false; //Boolean if we have to turn over relative 360˚ |
ryanlin97 | 12:921488918749 | 78 | |
jvfausto | 23:8d11d953ceeb | 79 | out->printf("pid right\r\r\n"); |
jvfausto | 23:8d11d953ceeb | 80 | x->write(def); // Not moving fowards or reverse |
jvfausto | 23:8d11d953ceeb | 81 | Setpoint = curr_yaw + deg; // Relative angle we want to turn |
jvfausto | 23:8d11d953ceeb | 82 | pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input) |
jvfausto | 23:8d11d953ceeb | 83 | |
jvfausto | 23:8d11d953ceeb | 84 | if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚ |
ryanlin97 | 12:921488918749 | 85 | overturn = true; |
ryanlin97 | 12:921488918749 | 86 | } |
jvfausto | 23:8d11d953ceeb | 87 | |
jvfausto | 23:8d11d953ceeb | 88 | myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D |
jvfausto | 23:8d11d953ceeb | 89 | myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low |
jvfausto | 23:8d11d953ceeb | 90 | myPID.SetControllerDirection(DIRECT); // PID mode Direct |
jvfausto | 23:8d11d953ceeb | 91 | |
jvfausto | 23:8d11d953ceeb | 92 | while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching |
jvfausto | 23:8d11d953ceeb | 93 | // a little less than desired angle |
jvfausto | 23:8d11d953ceeb | 94 | if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary |
jvfausto | 17:7f3b69300bb6 | 95 | { |
jvfausto | 17:7f3b69300bb6 | 96 | pid_yaw = curr_yaw + 360; |
jvfausto | 17:7f3b69300bb6 | 97 | } |
jvfausto | 17:7f3b69300bb6 | 98 | else |
jvfausto | 17:7f3b69300bb6 | 99 | pid_yaw = curr_yaw; |
jvfausto | 23:8d11d953ceeb | 100 | |
jvfausto | 23:8d11d953ceeb | 101 | myPID.Compute(); // Does PID calculations |
jvfausto | 23:8d11d953ceeb | 102 | double tempor = -Output+def; // Temporary value with the voltage output |
jvfausto | 23:8d11d953ceeb | 103 | y->write(tempor); // Sends to chair y output command |
jvfausto | 23:8d11d953ceeb | 104 | |
jvfausto | 23:8d11d953ceeb | 105 | out->printf("curr_yaw %f\r\r\n", curr_yaw); |
jvfausto | 17:7f3b69300bb6 | 106 | out->printf("Setpoint = %f \r\n", Setpoint); |
jvfausto | 17:7f3b69300bb6 | 107 | |
jvfausto | 23:8d11d953ceeb | 108 | wait(.05); // Small delay |
ryanlin97 | 12:921488918749 | 109 | } |
jvfausto | 23:8d11d953ceeb | 110 | Wheelchair::stop(); // Safety Stop |
jvfausto | 17:7f3b69300bb6 | 111 | out->printf("done \r\n"); |
jvfausto | 23:8d11d953ceeb | 112 | } |
ryanlin97 | 6:0cd57bdd8fbc | 113 | |
jvfausto | 23:8d11d953ceeb | 114 | void Wheelchair::pid_left(int deg) // Takes in degree and turns left |
ryanlin97 | 12:921488918749 | 115 | { |
jvfausto | 23:8d11d953ceeb | 116 | bool overturn = false; //Boolean if we have to turn under relative 0˚ |
ryanlin97 | 12:921488918749 | 117 | |
jvfausto | 23:8d11d953ceeb | 118 | out->printf("pid Left\r\r\n"); |
jvfausto | 23:8d11d953ceeb | 119 | x->write(def); // Not moving fowards or reverse |
jvfausto | 23:8d11d953ceeb | 120 | Setpoint = curr_yaw - deg; // Relative angle we want to turn |
jvfausto | 23:8d11d953ceeb | 121 | pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input) |
jvfausto | 23:8d11d953ceeb | 122 | if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚ |
ryanlin97 | 12:921488918749 | 123 | overturn = true; |
ryanlin97 | 12:921488918749 | 124 | } |
jvfausto | 23:8d11d953ceeb | 125 | myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D |
jvfausto | 23:8d11d953ceeb | 126 | myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def |
jvfausto | 23:8d11d953ceeb | 127 | myPID.SetControllerDirection(REVERSE); // PID mode Reverse |
jvfausto | 23:8d11d953ceeb | 128 | while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching |
jvfausto | 23:8d11d953ceeb | 129 | // a little more than desired angle |
jvfausto | 23:8d11d953ceeb | 130 | if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary |
jvfausto | 17:7f3b69300bb6 | 131 | { |
jvfausto | 17:7f3b69300bb6 | 132 | pid_yaw = curr_yaw - 360; |
jvfausto | 17:7f3b69300bb6 | 133 | } |
jvfausto | 17:7f3b69300bb6 | 134 | else |
jvfausto | 17:7f3b69300bb6 | 135 | pid_yaw = curr_yaw; |
jvfausto | 23:8d11d953ceeb | 136 | |
jvfausto | 23:8d11d953ceeb | 137 | myPID.Compute(); // Does PID calculations |
jvfausto | 23:8d11d953ceeb | 138 | double tempor = Output+def; // Temporary value with the voltage output |
jvfausto | 23:8d11d953ceeb | 139 | y->write(tempor); // Sends to chair y output command |
jvfausto | 23:8d11d953ceeb | 140 | |
jvfausto | 17:7f3b69300bb6 | 141 | out->printf("curr_yaw %f\r\n", curr_yaw); |
jvfausto | 23:8d11d953ceeb | 142 | wait(.05); // Small Delay |
ryanlin97 | 12:921488918749 | 143 | } |
jvfausto | 23:8d11d953ceeb | 144 | Wheelchair::stop(); // Safety Stop |
jvfausto | 23:8d11d953ceeb | 145 | } |
ryanlin97 | 12:921488918749 | 146 | |
jvfausto | 23:8d11d953ceeb | 147 | void Wheelchair::pid_turn(int deg) { // Determine wether we are turn right or left |
jvfausto | 23:8d11d953ceeb | 148 | |
jvfausto | 23:8d11d953ceeb | 149 | if(deg > 180) { // If deg > 180 turn left: coterminal angle |
ryanlin97 | 12:921488918749 | 150 | deg -= 360; |
ryanlin97 | 12:921488918749 | 151 | } |
ryanlin97 | 12:921488918749 | 152 | |
jvfausto | 23:8d11d953ceeb | 153 | else if(deg < -180) { // If deg < -180 turn right: coterminal angle |
ryanlin97 | 12:921488918749 | 154 | deg+=360; |
ryanlin97 | 12:921488918749 | 155 | } |
ryanlin97 | 12:921488918749 | 156 | |
jvfausto | 23:8d11d953ceeb | 157 | int turnAmt = abs(deg); // Makes sure input angle is positive |
ryanlin97 | 12:921488918749 | 158 | |
ryanlin97 | 12:921488918749 | 159 | if(deg >= 0){ |
jvfausto | 23:8d11d953ceeb | 160 | Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree |
jvfausto | 23:8d11d953ceeb | 161 | } |
ryanlin97 | 12:921488918749 | 162 | else { |
jvfausto | 23:8d11d953ceeb | 163 | Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree |
ryanlin97 | 12:921488918749 | 164 | } |
jvfausto | 23:8d11d953ceeb | 165 | } |
jvfausto | 19:71a6621ee5c3 | 166 | void Wheelchair::pid_forward(double mm) |
jvfausto | 17:7f3b69300bb6 | 167 | { |
jvfausto | 23:8d11d953ceeb | 168 | mm -= 20; // Makes sure distance does not overshoot |
jvfausto | 23:8d11d953ceeb | 169 | Input = 0; // Initializes imput to cero: Test latter w/o |
jvfausto | 23:8d11d953ceeb | 170 | wheel->reset(); // Resets encoders so that they start at 0 |
jvfausto | 17:7f3b69300bb6 | 171 | out->printf("pid foward\r\n"); |
jvfausto | 17:7f3b69300bb6 | 172 | |
jvfausto | 23:8d11d953ceeb | 173 | double tempor; // Initializes Temporary variable for x input |
jvfausto | 23:8d11d953ceeb | 174 | Setpoint = mm; // Initializes the setpoint to desired value |
jvfausto | 17:7f3b69300bb6 | 175 | |
jvfausto | 23:8d11d953ceeb | 176 | myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D |
jvfausto | 23:8d11d953ceeb | 177 | myPIDDistance.SetOutputLimits(0,high-def-.15); // Limits to the differnce between High and Def |
jvfausto | 23:8d11d953ceeb | 178 | myPIDDistance.SetControllerDirection(DIRECT); // PID to Direct |
jvfausto | 23:8d11d953ceeb | 179 | y->write(def+offset); // Sets chair to not turn |
jvfausto | 23:8d11d953ceeb | 180 | |
jvfausto | 23:8d11d953ceeb | 181 | while(Input < Setpoint){ // Stop moving when reaching setpoint |
jvfausto | 23:8d11d953ceeb | 182 | |
jvfausto | 23:8d11d953ceeb | 183 | if(out->readable()) // Emergency Break |
jvfausto | 19:71a6621ee5c3 | 184 | break; |
jvfausto | 23:8d11d953ceeb | 185 | |
jvfausto | 23:8d11d953ceeb | 186 | Input = wheel->getDistance(53.975); // Gets Distance from Encoder onto PID |
jvfausto | 23:8d11d953ceeb | 187 | wait(.05); // Slight Delay: *****Test without |
jvfausto | 23:8d11d953ceeb | 188 | myPIDDistance.Compute(); // Compute Output for chair |
jvfausto | 17:7f3b69300bb6 | 189 | |
jvfausto | 23:8d11d953ceeb | 190 | tempor = Output + def; // Temporary output variable |
jvfausto | 23:8d11d953ceeb | 191 | x->write(tempor); // Sends to chair x output |
jvfausto | 19:71a6621ee5c3 | 192 | out->printf("distance %f\r\n", Input); |
jvfausto | 17:7f3b69300bb6 | 193 | } |
ryanlin97 | 12:921488918749 | 194 | |
jvfausto | 17:7f3b69300bb6 | 195 | } |
jvfausto | 17:7f3b69300bb6 | 196 | void Wheelchair::pid_reverse(double mm) |
jvfausto | 17:7f3b69300bb6 | 197 | { |
jvfausto | 17:7f3b69300bb6 | 198 | |
jvfausto | 17:7f3b69300bb6 | 199 | } |
ryanlin97 | 8:381a4ec3fef8 | 200 | |
ryanlin97 | 12:921488918749 | 201 | float Wheelchair::getDistance() { |
ryanlin97 | 12:921488918749 | 202 | return wheel->getDistance(Diameter); |
ryanlin97 | 12:921488918749 | 203 | } |
ryanlin97 | 12:921488918749 | 204 | |
ryanlin97 | 12:921488918749 | 205 | void Wheelchair::resetDistance(){ |
ryanlin97 | 12:921488918749 | 206 | wheel->reset(); |
ryanlin97 | 12:921488918749 | 207 | } |
jvfausto | 23:8d11d953ceeb | 208 | /*Predetermined paths For Demmo*/ |
jvfausto | 19:71a6621ee5c3 | 209 | void Wheelchair::desk() { |
jvfausto | 19:71a6621ee5c3 | 210 | Wheelchair::pid_forward(5461); |
jvfausto | 19:71a6621ee5c3 | 211 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 212 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 213 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 214 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 215 | } |
ryanlin97 | 10:e5463c11e0a0 | 216 | |
jvfausto | 19:71a6621ee5c3 | 217 | void Wheelchair::kitchen() { |
jvfausto | 19:71a6621ee5c3 | 218 | Wheelchair::pid_forward(5461); |
jvfausto | 20:f42db4ae16f0 | 219 | Wheelchair::pid_right(87); |
jvfausto | 19:71a6621ee5c3 | 220 | Wheelchair::pid_forward(3658); |
jvfausto | 19:71a6621ee5c3 | 221 | Wheelchair::pid_left(90); |
jvfausto | 19:71a6621ee5c3 | 222 | Wheelchair::pid_forward(305); |
jvfausto | 19:71a6621ee5c3 | 223 | } |
ryanlin97 | 12:921488918749 | 224 | |
jvfausto | 19:71a6621ee5c3 | 225 | void Wheelchair::desk_to_kitchen(){ |
jvfausto | 19:71a6621ee5c3 | 226 | Wheelchair::pid_right(180); |
jvfausto | 19:71a6621ee5c3 | 227 | Wheelchair::pid_forward(3700); |
jvfausto | 19:71a6621ee5c3 | 228 | } |
jvfausto | 19:71a6621ee5c3 | 229 |