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