Affordable Smart Wheelchair / wheelchaircontrol4

Updated with emergency button and watchdog code

Dependencies:   QEI2 chair_BNO055 PID VL53L1X_Filter

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@27:0b1a837f123c, 2019-06-27 (annotated)

Committer:
t1jain
Date:
Thu Jun 27 18:08:29 2019 +0000
Revision:
27:0b1a837f123c
Parent:
26:662693bd7f31
Child:
28:6b2610acca97
Updated with emergency button and watchdog code

Who changed what in which revision?

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