Affordable Smart Wheelchair / Mbed OS Version1-2

Fully integrated ToF/IMU codes

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

wheelchairControlSideTof/wheelchair.cpp@21:d1faccb96146, 2019-07-09 (annotated)

Committer:
isagmz
Date:
Tue Jul 09 17:52:32 2019 +0000
Revision:
21:d1faccb96146
Parent:
20:3c1b58654e67 
Finished IMU side sensor code

Who changed what in which revision?

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