Jesi Miranda / wheelchaircontrol_jesi

Working file for communication with ROS

Dependencies:   QEI2 PID VL53L1X_Filter

wheelchair.cpp@29:5714715ab1f5, 2019-07-05 (annotated)

Committer:
JesiMiranda
Date:
Fri Jul 05 19:38:55 2019 +0000
Revision:
29:5714715ab1f5
Parent:
27:302d03ee1ae0
Child:
31:9283c8cd5362
no changes

Who changed what in which revision?

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