Sheila Pham / Version1-9

Added more code for side and angled sensor

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

wheelchair.cpp@43:9858a580b3d7, 2019-08-13 (annotated)

Committer:
sepham
Date:
Tue Aug 13 19:22:08 2019 +0000
Revision:
43:9858a580b3d7
Parent:
42:62c49ad06707 
Angled side sensor, side sensor more code

Who changed what in which revision?

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