Affordable Smart Wheelchair / wheelchairControlSummer2019

added code for PCB on and off, added pointers to e_button and pwm pins

Dependencies:   QEI2 chair_BNO055 PID VL53L1X_Filter

Dependents:   wheelchairControlSumer2019

wheelchair.cpp@28:ad02cb329fe3, 2019-06-28 (annotated)

Committer:
JesiMiranda
Date:
Fri Jun 28 19:35:51 2019 +0000
Revision:
28:ad02cb329fe3
Parent:
27:0b1a837f123c 
added code for PCB on and off, added pointers to e_button and pwm pins

Who changed what in which revision?

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