Sheila Pham / wheelchaircontrol

Added variable Test SideToF sensore

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

wheelchair.cpp@34:f10a892f5e85, 2019-07-02 (annotated)

Committer:
sepham
Date:
Tue Jul 02 16:27:16 2019 +0000
Revision:
34:f10a892f5e85
Parent:
32:fb26baa75d44
Child:
35:a3585c52723e
Added some variable for SideToF sensor

Who changed what in which revision?

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