Jesus Fausto / wheelchaircontrol1

1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@30:b24d73663499, 2020-06-03 (annotated)

Committer:
jvfausto
Date:
Wed Jun 03 20:25:18 2020 +0000
Revision:
30:b24d73663499
Parent:
27:da718b990837 
For sharing

Who changed what in which revision?

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