Jesi Miranda / wheelchaircontrol1

finished commenting on the .cpp file and changed the statistics library name

Dependencies:   QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   Version1-3 Version1-5

wheelchair.cpp@37:e0e6d3fe06a2, 2019-07-09 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 09 23:36:58 2019 +0000
Revision:
37:e0e6d3fe06a2
Parent:
35:5a2fed4c2e9f 
ff

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