1

Dependencies:   QEI2 chair_BNO055 PID Watchdog VL53L1X_Filter ros_lib_kinetic

Dependents:   wheelchairControlSumer2019

Committer:
jvfausto
Date:
Tue Oct 16 23:03:40 2018 +0000
Revision:
23:8d11d953ceeb
Parent:
20:f42db4ae16f0
Child:
24:d2f234fbc20d
revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
ryanlin97 11:d14a1f7f1297 2
jvfausto 23:8d11d953ceeb 3 bool manual_drive = false; // Variable Changes between joystick and auto drive
jvfausto 23:8d11d953ceeb 4 double curr_yaw; // Variable that contains current relative angle
jvfausto 23:8d11d953ceeb 5 double encoder_distance; // Keeps distanse due to original position
jvfausto 19:71a6621ee5c3 6
jvfausto 23:8d11d953ceeb 7 volatile double Setpoint, Output, Input, Input2; // Variables for PID
jvfausto 23:8d11d953ceeb 8 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID
jvfausto 19:71a6621ee5c3 9
jvfausto 23:8d11d953ceeb 10 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor
jvfausto 23:8d11d953ceeb 11 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor
jvfausto 23:8d11d953ceeb 12
jvfausto 23:8d11d953ceeb 13 void Wheelchair::compass_thread() { // Thread that measures which angle we are at
ryanlin97 11:d14a1f7f1297 14 curr_yaw = imu->yaw();
ryanlin97 11:d14a1f7f1297 15 }
ryanlin97 11:d14a1f7f1297 16
jvfausto 23:8d11d953ceeb 17 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei) // Function Constructor for Wheelchair class
ryanlin97 1:c0beadca1617 18 {
jvfausto 23:8d11d953ceeb 19 //Initializes X and Y variables to Pins
jvfausto 23:8d11d953ceeb 20 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 21 y = new PwmOut(yPin);
jvfausto 23:8d11d953ceeb 22
jvfausto 23:8d11d953ceeb 23 // Initializes IMU Library
ryanlin97 11:d14a1f7f1297 24 imu = new chair_BNO055(pc, time);
jvfausto 23:8d11d953ceeb 25 Wheelchair::stop(); // Wheelchair is not moving when initializing
jvfausto 23:8d11d953ceeb 26 imu->setup(); // turns on the IMU
jvfausto 23:8d11d953ceeb 27 out = pc; // "out" is called for serial monitor
jvfausto 23:8d11d953ceeb 28 wheel = qei; // "wheel" is called for encoder
jvfausto 23:8d11d953ceeb 29 out->printf("wheelchair setup done \r\n"); // make sure it initialized
ryanlin97 11:d14a1f7f1297 30 ti = time;
jvfausto 23:8d11d953ceeb 31 myPID.SetMode(AUTOMATIC); // set PID to automatic
ryanlin97 1:c0beadca1617 32 }
ryanlin97 6:0cd57bdd8fbc 33
jvfausto 23:8d11d953ceeb 34 void Wheelchair::move(float x_coor, float y_coor) // moves the chair with joystick on manual
ryanlin97 1:c0beadca1617 35 {
ryanlin97 6:0cd57bdd8fbc 36
jvfausto 23:8d11d953ceeb 37 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; // Scales one joystic measurement to the
jvfausto 23:8d11d953ceeb 38 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; // chair's joystic measurement
ryanlin97 11:d14a1f7f1297 39
jvfausto 23:8d11d953ceeb 40 x->write(scaled_x); // Sends the scaled joystic values to the chair
ryanlin97 4:29a27953fe70 41 y->write(scaled_y);
ryanlin97 5:e0ccaab3959a 42 }
ryanlin97 11:d14a1f7f1297 43
jvfausto 23:8d11d953ceeb 44 void Wheelchair::forward() // In auto to move foward
ryanlin97 1:c0beadca1617 45 {
ryanlin97 0:fc0c4a184482 46 x->write(high);
ryanlin97 3:a5e71bfdb492 47 y->write(def+offset);
ryanlin97 0:fc0c4a184482 48 }
ryanlin97 0:fc0c4a184482 49
jvfausto 23:8d11d953ceeb 50 void Wheelchair::backward() // In auto to move reverse
ryanlin97 1:c0beadca1617 51 {
ryanlin97 0:fc0c4a184482 52 x->write(low);
ryanlin97 0:fc0c4a184482 53 y->write(def);
ryanlin97 0:fc0c4a184482 54 }
ryanlin97 0:fc0c4a184482 55
jvfausto 23:8d11d953ceeb 56 void Wheelchair::right() // In auto to move right
ryanlin97 1:c0beadca1617 57 {
ryanlin97 0:fc0c4a184482 58 x->write(def);
ryanlin97 11:d14a1f7f1297 59 y->write(low);
ryanlin97 0:fc0c4a184482 60 }
ryanlin97 0:fc0c4a184482 61
jvfausto 23:8d11d953ceeb 62 void Wheelchair::left() // In auto to move left
ryanlin97 1:c0beadca1617 63 {
ryanlin97 0:fc0c4a184482 64 x->write(def);
ryanlin97 11:d14a1f7f1297 65 y->write(high);
ryanlin97 0:fc0c4a184482 66 }
ryanlin97 0:fc0c4a184482 67
jvfausto 23:8d11d953ceeb 68 void Wheelchair::stop() // Stops the chair
ryanlin97 1:c0beadca1617 69 {
ryanlin97 0:fc0c4a184482 70 x->write(def);
ryanlin97 0:fc0c4a184482 71 y->write(def);
ryanlin97 6:0cd57bdd8fbc 72 }
ryanlin97 11:d14a1f7f1297 73 // counter clockwise is -
ryanlin97 11:d14a1f7f1297 74 // clockwise is +
jvfausto 23:8d11d953ceeb 75 void Wheelchair::pid_right(int deg) // Takes in degree and turns right
ryanlin97 12:921488918749 76 {
jvfausto 23:8d11d953ceeb 77 bool overturn = false; //Boolean if we have to turn over relative 360˚
ryanlin97 12:921488918749 78
jvfausto 23:8d11d953ceeb 79 out->printf("pid right\r\r\n");
jvfausto 23:8d11d953ceeb 80 x->write(def); // Not moving fowards or reverse
jvfausto 23:8d11d953ceeb 81 Setpoint = curr_yaw + deg; // Relative angle we want to turn
jvfausto 23:8d11d953ceeb 82 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
jvfausto 23:8d11d953ceeb 83
jvfausto 23:8d11d953ceeb 84 if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚
ryanlin97 12:921488918749 85 overturn = true;
ryanlin97 12:921488918749 86 }
jvfausto 23:8d11d953ceeb 87
jvfausto 23:8d11d953ceeb 88 myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D
jvfausto 23:8d11d953ceeb 89 myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low
jvfausto 23:8d11d953ceeb 90 myPID.SetControllerDirection(DIRECT); // PID mode Direct
jvfausto 23:8d11d953ceeb 91
jvfausto 23:8d11d953ceeb 92 while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching
jvfausto 23:8d11d953ceeb 93 // a little less than desired angle
jvfausto 23:8d11d953ceeb 94 if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary
jvfausto 17:7f3b69300bb6 95 {
jvfausto 17:7f3b69300bb6 96 pid_yaw = curr_yaw + 360;
jvfausto 17:7f3b69300bb6 97 }
jvfausto 17:7f3b69300bb6 98 else
jvfausto 17:7f3b69300bb6 99 pid_yaw = curr_yaw;
jvfausto 23:8d11d953ceeb 100
jvfausto 23:8d11d953ceeb 101 myPID.Compute(); // Does PID calculations
jvfausto 23:8d11d953ceeb 102 double tempor = -Output+def; // Temporary value with the voltage output
jvfausto 23:8d11d953ceeb 103 y->write(tempor); // Sends to chair y output command
jvfausto 23:8d11d953ceeb 104
jvfausto 23:8d11d953ceeb 105 out->printf("curr_yaw %f\r\r\n", curr_yaw);
jvfausto 17:7f3b69300bb6 106 out->printf("Setpoint = %f \r\n", Setpoint);
jvfausto 17:7f3b69300bb6 107
jvfausto 23:8d11d953ceeb 108 wait(.05); // Small delay
ryanlin97 12:921488918749 109 }
jvfausto 23:8d11d953ceeb 110 Wheelchair::stop(); // Safety Stop
jvfausto 17:7f3b69300bb6 111 out->printf("done \r\n");
jvfausto 23:8d11d953ceeb 112 }
ryanlin97 6:0cd57bdd8fbc 113
jvfausto 23:8d11d953ceeb 114 void Wheelchair::pid_left(int deg) // Takes in degree and turns left
ryanlin97 12:921488918749 115 {
jvfausto 23:8d11d953ceeb 116 bool overturn = false; //Boolean if we have to turn under relative 0˚
ryanlin97 12:921488918749 117
jvfausto 23:8d11d953ceeb 118 out->printf("pid Left\r\r\n");
jvfausto 23:8d11d953ceeb 119 x->write(def); // Not moving fowards or reverse
jvfausto 23:8d11d953ceeb 120 Setpoint = curr_yaw - deg; // Relative angle we want to turn
jvfausto 23:8d11d953ceeb 121 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
jvfausto 23:8d11d953ceeb 122 if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚
ryanlin97 12:921488918749 123 overturn = true;
ryanlin97 12:921488918749 124 }
jvfausto 23:8d11d953ceeb 125 myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D
jvfausto 23:8d11d953ceeb 126 myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def
jvfausto 23:8d11d953ceeb 127 myPID.SetControllerDirection(REVERSE); // PID mode Reverse
jvfausto 23:8d11d953ceeb 128 while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching
jvfausto 23:8d11d953ceeb 129 // a little more than desired angle
jvfausto 23:8d11d953ceeb 130 if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary
jvfausto 17:7f3b69300bb6 131 {
jvfausto 17:7f3b69300bb6 132 pid_yaw = curr_yaw - 360;
jvfausto 17:7f3b69300bb6 133 }
jvfausto 17:7f3b69300bb6 134 else
jvfausto 17:7f3b69300bb6 135 pid_yaw = curr_yaw;
jvfausto 23:8d11d953ceeb 136
jvfausto 23:8d11d953ceeb 137 myPID.Compute(); // Does PID calculations
jvfausto 23:8d11d953ceeb 138 double tempor = Output+def; // Temporary value with the voltage output
jvfausto 23:8d11d953ceeb 139 y->write(tempor); // Sends to chair y output command
jvfausto 23:8d11d953ceeb 140
jvfausto 17:7f3b69300bb6 141 out->printf("curr_yaw %f\r\n", curr_yaw);
jvfausto 23:8d11d953ceeb 142 wait(.05); // Small Delay
ryanlin97 12:921488918749 143 }
jvfausto 23:8d11d953ceeb 144 Wheelchair::stop(); // Safety Stop
jvfausto 23:8d11d953ceeb 145 }
ryanlin97 12:921488918749 146
jvfausto 23:8d11d953ceeb 147 void Wheelchair::pid_turn(int deg) { // Determine wether we are turn right or left
jvfausto 23:8d11d953ceeb 148
jvfausto 23:8d11d953ceeb 149 if(deg > 180) { // If deg > 180 turn left: coterminal angle
ryanlin97 12:921488918749 150 deg -= 360;
ryanlin97 12:921488918749 151 }
ryanlin97 12:921488918749 152
jvfausto 23:8d11d953ceeb 153 else if(deg < -180) { // If deg < -180 turn right: coterminal angle
ryanlin97 12:921488918749 154 deg+=360;
ryanlin97 12:921488918749 155 }
ryanlin97 12:921488918749 156
jvfausto 23:8d11d953ceeb 157 int turnAmt = abs(deg); // Makes sure input angle is positive
ryanlin97 12:921488918749 158
ryanlin97 12:921488918749 159 if(deg >= 0){
jvfausto 23:8d11d953ceeb 160 Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree
jvfausto 23:8d11d953ceeb 161 }
ryanlin97 12:921488918749 162 else {
jvfausto 23:8d11d953ceeb 163 Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree
ryanlin97 12:921488918749 164 }
jvfausto 23:8d11d953ceeb 165 }
jvfausto 19:71a6621ee5c3 166 void Wheelchair::pid_forward(double mm)
jvfausto 17:7f3b69300bb6 167 {
jvfausto 23:8d11d953ceeb 168 mm -= 20; // Makes sure distance does not overshoot
jvfausto 23:8d11d953ceeb 169 Input = 0; // Initializes imput to cero: Test latter w/o
jvfausto 23:8d11d953ceeb 170 wheel->reset(); // Resets encoders so that they start at 0
jvfausto 17:7f3b69300bb6 171 out->printf("pid foward\r\n");
jvfausto 17:7f3b69300bb6 172
jvfausto 23:8d11d953ceeb 173 double tempor; // Initializes Temporary variable for x input
jvfausto 23:8d11d953ceeb 174 Setpoint = mm; // Initializes the setpoint to desired value
jvfausto 17:7f3b69300bb6 175
jvfausto 23:8d11d953ceeb 176 myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D
jvfausto 23:8d11d953ceeb 177 myPIDDistance.SetOutputLimits(0,high-def-.15); // Limits to the differnce between High and Def
jvfausto 23:8d11d953ceeb 178 myPIDDistance.SetControllerDirection(DIRECT); // PID to Direct
jvfausto 23:8d11d953ceeb 179 y->write(def+offset); // Sets chair to not turn
jvfausto 23:8d11d953ceeb 180
jvfausto 23:8d11d953ceeb 181 while(Input < Setpoint){ // Stop moving when reaching setpoint
jvfausto 23:8d11d953ceeb 182
jvfausto 23:8d11d953ceeb 183 if(out->readable()) // Emergency Break
jvfausto 19:71a6621ee5c3 184 break;
jvfausto 23:8d11d953ceeb 185
jvfausto 23:8d11d953ceeb 186 Input = wheel->getDistance(53.975); // Gets Distance from Encoder onto PID
jvfausto 23:8d11d953ceeb 187 wait(.05); // Slight Delay: *****Test without
jvfausto 23:8d11d953ceeb 188 myPIDDistance.Compute(); // Compute Output for chair
jvfausto 17:7f3b69300bb6 189
jvfausto 23:8d11d953ceeb 190 tempor = Output + def; // Temporary output variable
jvfausto 23:8d11d953ceeb 191 x->write(tempor); // Sends to chair x output
jvfausto 19:71a6621ee5c3 192 out->printf("distance %f\r\n", Input);
jvfausto 17:7f3b69300bb6 193 }
ryanlin97 12:921488918749 194
jvfausto 17:7f3b69300bb6 195 }
jvfausto 17:7f3b69300bb6 196 void Wheelchair::pid_reverse(double mm)
jvfausto 17:7f3b69300bb6 197 {
jvfausto 17:7f3b69300bb6 198
jvfausto 17:7f3b69300bb6 199 }
ryanlin97 8:381a4ec3fef8 200
ryanlin97 12:921488918749 201 float Wheelchair::getDistance() {
ryanlin97 12:921488918749 202 return wheel->getDistance(Diameter);
ryanlin97 12:921488918749 203 }
ryanlin97 12:921488918749 204
ryanlin97 12:921488918749 205 void Wheelchair::resetDistance(){
ryanlin97 12:921488918749 206 wheel->reset();
ryanlin97 12:921488918749 207 }
jvfausto 23:8d11d953ceeb 208 /*Predetermined paths For Demmo*/
jvfausto 19:71a6621ee5c3 209 void Wheelchair::desk() {
jvfausto 19:71a6621ee5c3 210 Wheelchair::pid_forward(5461);
jvfausto 19:71a6621ee5c3 211 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 212 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 213 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 214 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 215 }
ryanlin97 10:e5463c11e0a0 216
jvfausto 19:71a6621ee5c3 217 void Wheelchair::kitchen() {
jvfausto 19:71a6621ee5c3 218 Wheelchair::pid_forward(5461);
jvfausto 20:f42db4ae16f0 219 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 220 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 221 Wheelchair::pid_left(90);
jvfausto 19:71a6621ee5c3 222 Wheelchair::pid_forward(305);
jvfausto 19:71a6621ee5c3 223 }
ryanlin97 12:921488918749 224
jvfausto 19:71a6621ee5c3 225 void Wheelchair::desk_to_kitchen(){
jvfausto 19:71a6621ee5c3 226 Wheelchair::pid_right(180);
jvfausto 19:71a6621ee5c3 227 Wheelchair::pid_forward(3700);
jvfausto 19:71a6621ee5c3 228 }
jvfausto 19:71a6621ee5c3 229