Affordable Smart Wheelchair / wheelchaircontrol8

Updated references from MPU6050 to BNO080

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter BNOWrapper ros_lib_kinetic

Dependents:   Version1-8

wheelchair.cpp@39:5438557a8b4e, 2019-07-10 (annotated)

Committer:
t1jain
Date:
Wed Jul 10 19:30:57 2019 +0000
Revision:
39:5438557a8b4e
Parent:
38:cb87f4e353b9
Child:
40:11b8567c507b
Added Back ToF (untested)

Who changed what in which revision?

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