Jesi Miranda / wheelchaircontrol_jesi

Working file for communication with ROS

Dependencies:   QEI2 PID VL53L1X_Filter

wheelchair.cpp@32:5ca063fc0695, 2019-08-20 (annotated)

Committer:
JesiMiranda
Date:
Tue Aug 20 22:29:16 2019 +0000
Revision:
32:5ca063fc0695
Parent:
31:9283c8cd5362 
no changes

Who changed what in which revision?

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