Jesus Fausto / wheelchaircontrolRosCom

Odometry communication

Dependencies:   PID QEI chair_BNO055 ros_lib_kinetic3

Dependents:   wheelchaircontrolrealtime1

Fork of wheelchaircontrol by Jesus Fausto

wheelchair.cpp@24:6c5b4b82f874, 2018-11-02 (annotated)

Committer:
jvfausto
Date:
Fri Nov 02 02:15:30 2018 +0000
Revision:
24:6c5b4b82f874
Parent:
23:58ec657a44f2
Child:
25:987f3bcd769b
Child:
27:17783779a954
With odometry communication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #include "wheelchair.h"
jvfausto 23:58ec657a44f2 2
jvfausto 21:8d11d953ceeb 3 bool manual_drive = false; // Variable Changes between joystick and auto drive
jvfausto 22:d2f234fbc20d 4 double curr_yaw, curr_vel, curr_velS; // Variable that contains current relative angle
jvfausto 21:8d11d953ceeb 5 double encoder_distance; // Keeps distanse due to original position
jvfausto 23:58ec657a44f2 6
jvfausto 21:8d11d953ceeb 7 volatile double Setpoint, Output, Input, Input2; // Variables for PID
jvfausto 21:8d11d953ceeb 8 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID
jvfausto 22:d2f234fbc20d 9 volatile double vIn, vOut, vDesired;
jvfausto 22:d2f234fbc20d 10 volatile double vInS, vOutS, vDesiredS;
jvfausto 22:d2f234fbc20d 11 volatile double yIn, yOut, yDesired;
jvfausto 19:71a6621ee5c3 12
jvfausto 23:58ec657a44f2 13 double z_angular, dist_old, curr_pos;
jvfausto 23:58ec657a44f2 14 double x_position = 0;
jvfausto 23:58ec657a44f2 15 double y_position = 0;
jvfausto 24:6c5b4b82f874 16
jvfausto 24:6c5b4b82f874 17
jvfausto 23:58ec657a44f2 18
jvfausto 21:8d11d953ceeb 19 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT); // Angle PID object constructor
jvfausto 21:8d11d953ceeb 20 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT); // Distance PID object constructor
jvfausto 22:d2f234fbc20d 21 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT);
jvfausto 22:d2f234fbc20d 22 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);
jvfausto 22:d2f234fbc20d 23 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);
jvfausto 23:58ec657a44f2 24
jvfausto 21:8d11d953ceeb 25 void Wheelchair::compass_thread() { // Thread that measures which angle we are at
ryanlin97 11:d14a1f7f1297 26 curr_yaw = imu->yaw();
jvfausto 23:58ec657a44f2 27 z_angular = curr_yaw;
ryanlin97 11:d14a1f7f1297 28 }
jvfausto 22:d2f234fbc20d 29 void Wheelchair::velosity_thread() {
jvfausto 22:d2f234fbc20d 30 curr_vel = wheel->getVelosity();
jvfausto 22:d2f234fbc20d 31 curr_velS = wheelS->getVelosity();
jvfausto 23:58ec657a44f2 32 curr_pos = wheel->getDistance(53.975);
jvfausto 22:d2f234fbc20d 33 }
jvfausto 24:6c5b4b82f874 34 void Wheelchair::rosCom_thread(){
jvfausto 24:6c5b4b82f874 35 //set and update array
jvfausto 24:6c5b4b82f874 36 odom_vector[0] = curr_pos;//x_position;
jvfausto 24:6c5b4b82f874 37 odom_vector[1] = y_position;
ryanlin97 11:d14a1f7f1297 38
jvfausto 24:6c5b4b82f874 39 //set the position with updated values
jvfausto 24:6c5b4b82f874 40 /* odom.pose.pose.position.x = odom_vector[0];
jvfausto 24:6c5b4b82f874 41 odom.pose.pose.position.y = odom_vector[1];
jvfausto 24:6c5b4b82f874 42 odom.pose.pose.position.z = 0;
jvfausto 24:6c5b4b82f874 43 //set the orientation
jvfausto 24:6c5b4b82f874 44 odom.pose.pose.orientation.z = z_angular;
jvfausto 24:6c5b4b82f874 45 odom.pose.pose.orientation.x = 0;
jvfausto 24:6c5b4b82f874 46 odom.pose.pose.orientation.y = 0;
jvfausto 24:6c5b4b82f874 47 chatter.publish(&odom);
jvfausto 24:6c5b4b82f874 48 nh.spinOnce();
jvfausto 24:6c5b4b82f874 49 */
jvfausto 24:6c5b4b82f874 50 }
jvfausto 22:d2f234fbc20d 51 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS) // Function Constructor for Wheelchair class
ryanlin97 1:c0beadca1617 52 {
jvfausto 21:8d11d953ceeb 53 //Initializes X and Y variables to Pins
jvfausto 21:8d11d953ceeb 54 x = new PwmOut(xPin);
ryanlin97 3:a5e71bfdb492 55 y = new PwmOut(yPin);
jvfausto 24:6c5b4b82f874 56 odom_vector[0] = 0;
jvfausto 24:6c5b4b82f874 57 odom_vector[1] = 0;
jvfausto 24:6c5b4b82f874 58 odom_vector[2] = 0;
jvfausto 21:8d11d953ceeb 59 // Initializes IMU Library
ryanlin97 11:d14a1f7f1297 60 imu = new chair_BNO055(pc, time);
jvfausto 21:8d11d953ceeb 61 Wheelchair::stop(); // Wheelchair is not moving when initializing
jvfausto 21:8d11d953ceeb 62 imu->setup(); // turns on the IMU
jvfausto 21:8d11d953ceeb 63 out = pc; // "out" is called for serial monitor
jvfausto 22:d2f234fbc20d 64 wheelS = qeiS; // "wheel" is called for encoder
jvfausto 22:d2f234fbc20d 65 wheel = qei;
jvfausto 21:8d11d953ceeb 66 out->printf("wheelchair setup done \r\n"); // make sure it initialized
ryanlin97 11:d14a1f7f1297 67 ti = time;
jvfausto 21:8d11d953ceeb 68 myPID.SetMode(AUTOMATIC); // set PID to automatic
ryanlin97 1:c0beadca1617 69 }
jvfausto 23:58ec657a44f2 70
jvfausto 21:8d11d953ceeb 71 void Wheelchair::move(float x_coor, float y_coor) // moves the chair with joystick on manual
ryanlin97 1:c0beadca1617 72 {
jvfausto 23:58ec657a44f2 73
jvfausto 21:8d11d953ceeb 74 float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; // Scales one joystic measurement to the
jvfausto 21:8d11d953ceeb 75 float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; // chair's joystic measurement
ryanlin97 11:d14a1f7f1297 76
jvfausto 21:8d11d953ceeb 77 x->write(scaled_x); // Sends the scaled joystic values to the chair
ryanlin97 4:29a27953fe70 78 y->write(scaled_y);
ryanlin97 5:e0ccaab3959a 79 }
jvfausto 23:58ec657a44f2 80
jvfausto 21:8d11d953ceeb 81 void Wheelchair::forward() // In auto to move foward
ryanlin97 1:c0beadca1617 82 {
ryanlin97 0:fc0c4a184482 83 x->write(high);
ryanlin97 3:a5e71bfdb492 84 y->write(def+offset);
ryanlin97 0:fc0c4a184482 85 }
jvfausto 23:58ec657a44f2 86
jvfausto 21:8d11d953ceeb 87 void Wheelchair::backward() // In auto to move reverse
ryanlin97 1:c0beadca1617 88 {
ryanlin97 0:fc0c4a184482 89 x->write(low);
ryanlin97 0:fc0c4a184482 90 y->write(def);
ryanlin97 0:fc0c4a184482 91 }
jvfausto 23:58ec657a44f2 92
jvfausto 21:8d11d953ceeb 93 void Wheelchair::right() // In auto to move right
ryanlin97 1:c0beadca1617 94 {
ryanlin97 0:fc0c4a184482 95 x->write(def);
ryanlin97 11:d14a1f7f1297 96 y->write(low);
ryanlin97 0:fc0c4a184482 97 }
jvfausto 23:58ec657a44f2 98
jvfausto 21:8d11d953ceeb 99 void Wheelchair::left() // In auto to move left
ryanlin97 1:c0beadca1617 100 {
ryanlin97 0:fc0c4a184482 101 x->write(def);
ryanlin97 11:d14a1f7f1297 102 y->write(high);
ryanlin97 0:fc0c4a184482 103 }
jvfausto 23:58ec657a44f2 104
jvfausto 21:8d11d953ceeb 105 void Wheelchair::stop() // Stops the chair
ryanlin97 1:c0beadca1617 106 {
ryanlin97 0:fc0c4a184482 107 x->write(def);
ryanlin97 0:fc0c4a184482 108 y->write(def);
ryanlin97 6:0cd57bdd8fbc 109 }
ryanlin97 11:d14a1f7f1297 110 // counter clockwise is -
ryanlin97 11:d14a1f7f1297 111 // clockwise is +
jvfausto 21:8d11d953ceeb 112 void Wheelchair::pid_right(int deg) // Takes in degree and turns right
ryanlin97 12:921488918749 113 {
jvfausto 21:8d11d953ceeb 114 bool overturn = false; //Boolean if we have to turn over relative 360˚
ryanlin97 12:921488918749 115
jvfausto 21:8d11d953ceeb 116 out->printf("pid right\r\r\n");
jvfausto 21:8d11d953ceeb 117 x->write(def); // Not moving fowards or reverse
jvfausto 21:8d11d953ceeb 118 Setpoint = curr_yaw + deg; // Relative angle we want to turn
jvfausto 21:8d11d953ceeb 119 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
jvfausto 21:8d11d953ceeb 120
jvfausto 21:8d11d953ceeb 121 if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚
ryanlin97 12:921488918749 122 overturn = true;
ryanlin97 12:921488918749 123 }
jvfausto 21:8d11d953ceeb 124
jvfausto 21:8d11d953ceeb 125 myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D
jvfausto 21:8d11d953ceeb 126 myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low
jvfausto 21:8d11d953ceeb 127 myPID.SetControllerDirection(DIRECT); // PID mode Direct
jvfausto 21:8d11d953ceeb 128
jvfausto 21:8d11d953ceeb 129 while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching
jvfausto 21:8d11d953ceeb 130 // a little less than desired angle
jvfausto 21:8d11d953ceeb 131 if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary
jvfausto 17:7f3b69300bb6 132 {
jvfausto 17:7f3b69300bb6 133 pid_yaw = curr_yaw + 360;
jvfausto 17:7f3b69300bb6 134 }
jvfausto 17:7f3b69300bb6 135 else
jvfausto 17:7f3b69300bb6 136 pid_yaw = curr_yaw;
jvfausto 21:8d11d953ceeb 137
jvfausto 21:8d11d953ceeb 138 myPID.Compute(); // Does PID calculations
jvfausto 21:8d11d953ceeb 139 double tempor = -Output+def; // Temporary value with the voltage output
jvfausto 21:8d11d953ceeb 140 y->write(tempor); // Sends to chair y output command
jvfausto 21:8d11d953ceeb 141
jvfausto 21:8d11d953ceeb 142 out->printf("curr_yaw %f\r\r\n", curr_yaw);
jvfausto 17:7f3b69300bb6 143 out->printf("Setpoint = %f \r\n", Setpoint);
jvfausto 23:58ec657a44f2 144
jvfausto 21:8d11d953ceeb 145 wait(.05); // Small delay
ryanlin97 12:921488918749 146 }
jvfausto 21:8d11d953ceeb 147 Wheelchair::stop(); // Safety Stop
jvfausto 17:7f3b69300bb6 148 out->printf("done \r\n");
jvfausto 21:8d11d953ceeb 149 }
jvfausto 23:58ec657a44f2 150
jvfausto 21:8d11d953ceeb 151 void Wheelchair::pid_left(int deg) // Takes in degree and turns left
ryanlin97 12:921488918749 152 {
jvfausto 21:8d11d953ceeb 153 bool overturn = false; //Boolean if we have to turn under relative 0˚
ryanlin97 12:921488918749 154
jvfausto 21:8d11d953ceeb 155 out->printf("pid Left\r\r\n");
jvfausto 21:8d11d953ceeb 156 x->write(def); // Not moving fowards or reverse
jvfausto 21:8d11d953ceeb 157 Setpoint = curr_yaw - deg; // Relative angle we want to turn
jvfausto 21:8d11d953ceeb 158 pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input)
jvfausto 21:8d11d953ceeb 159 if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚
ryanlin97 12:921488918749 160 overturn = true;
ryanlin97 12:921488918749 161 }
jvfausto 21:8d11d953ceeb 162 myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D
jvfausto 21:8d11d953ceeb 163 myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def
jvfausto 21:8d11d953ceeb 164 myPID.SetControllerDirection(REVERSE); // PID mode Reverse
jvfausto 21:8d11d953ceeb 165 while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching
jvfausto 21:8d11d953ceeb 166 // a little more than desired angle
jvfausto 21:8d11d953ceeb 167 if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary
jvfausto 17:7f3b69300bb6 168 {
jvfausto 17:7f3b69300bb6 169 pid_yaw = curr_yaw - 360;
jvfausto 17:7f3b69300bb6 170 }
jvfausto 17:7f3b69300bb6 171 else
jvfausto 17:7f3b69300bb6 172 pid_yaw = curr_yaw;
jvfausto 21:8d11d953ceeb 173
jvfausto 21:8d11d953ceeb 174 myPID.Compute(); // Does PID calculations
jvfausto 21:8d11d953ceeb 175 double tempor = Output+def; // Temporary value with the voltage output
jvfausto 21:8d11d953ceeb 176 y->write(tempor); // Sends to chair y output command
jvfausto 21:8d11d953ceeb 177
jvfausto 17:7f3b69300bb6 178 out->printf("curr_yaw %f\r\n", curr_yaw);
jvfausto 21:8d11d953ceeb 179 wait(.05); // Small Delay
ryanlin97 12:921488918749 180 }
jvfausto 21:8d11d953ceeb 181 Wheelchair::stop(); // Safety Stop
jvfausto 21:8d11d953ceeb 182 }
jvfausto 23:58ec657a44f2 183
jvfausto 21:8d11d953ceeb 184 void Wheelchair::pid_turn(int deg) { // Determine wether we are turn right or left
jvfausto 23:58ec657a44f2 185
jvfausto 21:8d11d953ceeb 186 if(deg > 180) { // If deg > 180 turn left: coterminal angle
ryanlin97 12:921488918749 187 deg -= 360;
ryanlin97 12:921488918749 188 }
jvfausto 23:58ec657a44f2 189
jvfausto 21:8d11d953ceeb 190 else if(deg < -180) { // If deg < -180 turn right: coterminal angle
ryanlin97 12:921488918749 191 deg+=360;
ryanlin97 12:921488918749 192 }
ryanlin97 12:921488918749 193
jvfausto 21:8d11d953ceeb 194 int turnAmt = abs(deg); // Makes sure input angle is positive
jvfausto 23:58ec657a44f2 195
ryanlin97 12:921488918749 196 if(deg >= 0){
jvfausto 21:8d11d953ceeb 197 Wheelchair::pid_right(turnAmt); // Calls PID right if positive degree
jvfausto 21:8d11d953ceeb 198 }
ryanlin97 12:921488918749 199 else {
jvfausto 21:8d11d953ceeb 200 Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree
ryanlin97 12:921488918749 201 }
jvfausto 21:8d11d953ceeb 202 }
jvfausto 19:71a6621ee5c3 203 void Wheelchair::pid_forward(double mm)
jvfausto 17:7f3b69300bb6 204 {
jvfausto 21:8d11d953ceeb 205 mm -= 20; // Makes sure distance does not overshoot
jvfausto 21:8d11d953ceeb 206 Input = 0; // Initializes imput to cero: Test latter w/o
jvfausto 21:8d11d953ceeb 207 wheel->reset(); // Resets encoders so that they start at 0
jvfausto 17:7f3b69300bb6 208 out->printf("pid foward\r\n");
jvfausto 23:58ec657a44f2 209
jvfausto 21:8d11d953ceeb 210 double tempor; // Initializes Temporary variable for x input
jvfausto 21:8d11d953ceeb 211 Setpoint = mm; // Initializes the setpoint to desired value
jvfausto 23:58ec657a44f2 212
jvfausto 21:8d11d953ceeb 213 myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D
jvfausto 21:8d11d953ceeb 214 myPIDDistance.SetOutputLimits(0,high-def-.15); // Limits to the differnce between High and Def
jvfausto 21:8d11d953ceeb 215 myPIDDistance.SetControllerDirection(DIRECT); // PID to Direct
jvfausto 21:8d11d953ceeb 216 y->write(def+offset); // Sets chair to not turn
jvfausto 21:8d11d953ceeb 217
jvfausto 21:8d11d953ceeb 218 while(Input < Setpoint){ // Stop moving when reaching setpoint
jvfausto 21:8d11d953ceeb 219
jvfausto 21:8d11d953ceeb 220 if(out->readable()) // Emergency Break
jvfausto 19:71a6621ee5c3 221 break;
jvfausto 21:8d11d953ceeb 222
jvfausto 21:8d11d953ceeb 223 Input = wheel->getDistance(53.975); // Gets Distance from Encoder onto PID
jvfausto 21:8d11d953ceeb 224 wait(.05); // Slight Delay: *****Test without
jvfausto 21:8d11d953ceeb 225 myPIDDistance.Compute(); // Compute Output for chair
jvfausto 23:58ec657a44f2 226
jvfausto 21:8d11d953ceeb 227 tempor = Output + def; // Temporary output variable
jvfausto 21:8d11d953ceeb 228 x->write(tempor); // Sends to chair x output
jvfausto 19:71a6621ee5c3 229 out->printf("distance %f\r\n", Input);
jvfausto 17:7f3b69300bb6 230 }
ryanlin97 12:921488918749 231
jvfausto 17:7f3b69300bb6 232 }
jvfausto 17:7f3b69300bb6 233 void Wheelchair::pid_reverse(double mm)
jvfausto 17:7f3b69300bb6 234 {
jvfausto 23:58ec657a44f2 235
jvfausto 17:7f3b69300bb6 236 }
jvfausto 22:d2f234fbc20d 237 void Wheelchair::pid_twistA()
jvfausto 22:d2f234fbc20d 238 {
jvfausto 22:d2f234fbc20d 239 char c;
jvfausto 22:d2f234fbc20d 240 double temporA = def;
jvfausto 22:d2f234fbc20d 241 y->write(def);
jvfausto 22:d2f234fbc20d 242 x->write(def);
jvfausto 23:58ec657a44f2 243
jvfausto 22:d2f234fbc20d 244 PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D
jvfausto 22:d2f234fbc20d 245 PIDAngularV.SetOutputLimits(-.1, .1); // Limits to the differnce between def and low
jvfausto 22:d2f234fbc20d 246 PIDAngularV.SetControllerDirection(DIRECT); // PID mode Direct
jvfausto 22:d2f234fbc20d 247 while(1)
jvfausto 22:d2f234fbc20d 248 {
jvfausto 22:d2f234fbc20d 249 if(out->readable())
jvfausto 22:d2f234fbc20d 250 c = out->getc();
jvfausto 22:d2f234fbc20d 251 if(c == '0')
jvfausto 22:d2f234fbc20d 252 {
jvfausto 22:d2f234fbc20d 253 yDesired = 0;
jvfausto 22:d2f234fbc20d 254 }
jvfausto 22:d2f234fbc20d 255 if(c == '1')
jvfausto 22:d2f234fbc20d 256 yDesired = 30;
jvfausto 22:d2f234fbc20d 257 if(c == '2')
jvfausto 22:d2f234fbc20d 258 yDesired = 90;
jvfausto 22:d2f234fbc20d 259 if(c == '9')
jvfausto 22:d2f234fbc20d 260 yDesired = -30;
jvfausto 22:d2f234fbc20d 261 if(c == '8')
jvfausto 22:d2f234fbc20d 262 yDesired = -90;
jvfausto 22:d2f234fbc20d 263 if(c == 'r')
jvfausto 22:d2f234fbc20d 264 {
jvfausto 22:d2f234fbc20d 265 yDesired = 0;
jvfausto 22:d2f234fbc20d 266 return;
jvfausto 22:d2f234fbc20d 267 }
jvfausto 22:d2f234fbc20d 268
jvfausto 22:d2f234fbc20d 269 yIn = imu->gyro_z();
jvfausto 22:d2f234fbc20d 270 PIDAngularV.Compute();
jvfausto 22:d2f234fbc20d 271 temporA += yOut; // Temporary value with the voltage output
jvfausto 22:d2f234fbc20d 272 y->write(temporA);
jvfausto 23:58ec657a44f2 273 //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
jvfausto 22:d2f234fbc20d 274 wait(.05);
jvfausto 22:d2f234fbc20d 275 }
jvfausto 22:d2f234fbc20d 276 }
jvfausto 22:d2f234fbc20d 277 void Wheelchair::pid_twistV()
jvfausto 22:d2f234fbc20d 278 {
jvfausto 22:d2f234fbc20d 279 double temporV = def;
jvfausto 22:d2f234fbc20d 280 double temporS = def;
jvfausto 22:d2f234fbc20d 281 vDesiredS = 0;
jvfausto 22:d2f234fbc20d 282 char c;
jvfausto 22:d2f234fbc20d 283 x->write(def);
jvfausto 22:d2f234fbc20d 284 y->write(def);
jvfausto 23:58ec657a44f2 285 wheel->reset();
jvfausto 22:d2f234fbc20d 286 PIDVelosity.SetTunings(.00005,0, 0.00); // Sets the constants for P and D
jvfausto 24:6c5b4b82f874 287 PIDSlaveV.SetTunings(.004,0.000001, 0.000001); // Sets the constants for P and D
jvfausto 22:d2f234fbc20d 288 PIDVelosity.SetOutputLimits(-.005, .005); // Limits to the differnce between def and low
jvfausto 22:d2f234fbc20d 289 PIDSlaveV.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low
jvfausto 22:d2f234fbc20d 290 PIDVelosity.SetControllerDirection(DIRECT);
jvfausto 22:d2f234fbc20d 291 PIDSlaveV.SetControllerDirection(DIRECT);
jvfausto 22:d2f234fbc20d 292 while(1)
jvfausto 22:d2f234fbc20d 293 {
jvfausto 22:d2f234fbc20d 294 if(out->readable())
jvfausto 22:d2f234fbc20d 295 c = out->getc();
jvfausto 22:d2f234fbc20d 296 if(c == '0')
jvfausto 22:d2f234fbc20d 297 vDesired = 0;
jvfausto 22:d2f234fbc20d 298 if(c == '3')
jvfausto 22:d2f234fbc20d 299 vDesired = 100;
jvfausto 22:d2f234fbc20d 300 if(c == '4')
jvfausto 22:d2f234fbc20d 301 vDesired = 150;
jvfausto 22:d2f234fbc20d 302 if(c == '7')
jvfausto 22:d2f234fbc20d 303 vDesired = -50;
jvfausto 22:d2f234fbc20d 304 if(c == '6')
jvfausto 22:d2f234fbc20d 305 vDesired = -100;
jvfausto 22:d2f234fbc20d 306 if(c == 'r')
jvfausto 22:d2f234fbc20d 307 {
jvfausto 22:d2f234fbc20d 308 yDesired = 0;
jvfausto 23:58ec657a44f2 309 dist_old = 0;
jvfausto 22:d2f234fbc20d 310 return;
jvfausto 22:d2f234fbc20d 311 }
jvfausto 22:d2f234fbc20d 312 if(vDesired >= 0)
jvfausto 22:d2f234fbc20d 313 {
jvfausto 23:58ec657a44f2 314 PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D
jvfausto 23:58ec657a44f2 315 PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the differnce between def and low
jvfausto 22:d2f234fbc20d 316 }
jvfausto 22:d2f234fbc20d 317 else
jvfausto 22:d2f234fbc20d 318 {
jvfausto 22:d2f234fbc20d 319 PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D
jvfausto 22:d2f234fbc20d 320 PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to the differnce between def and low
jvfausto 22:d2f234fbc20d 321 }
jvfausto 22:d2f234fbc20d 322 if(temporV >= 1)
jvfausto 22:d2f234fbc20d 323 temporV = 1;
jvfausto 22:d2f234fbc20d 324 vIn = curr_vel*100;
jvfausto 22:d2f234fbc20d 325 vInS = curr_vel-curr_velS;
jvfausto 22:d2f234fbc20d 326 PIDVelosity.Compute();
jvfausto 22:d2f234fbc20d 327 PIDSlaveV.Compute();
jvfausto 22:d2f234fbc20d 328 temporV += vOut;
jvfausto 22:d2f234fbc20d 329 temporS += vOutS;
jvfausto 22:d2f234fbc20d 330 x->write(temporV);
jvfausto 22:d2f234fbc20d 331 y->write(temporS);
jvfausto 23:58ec657a44f2 332 //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
jvfausto 23:58ec657a44f2 333 Wheelchair::odomMsg();
jvfausto 22:d2f234fbc20d 334 wait(.01);
jvfausto 22:d2f234fbc20d 335 }
jvfausto 22:d2f234fbc20d 336 }
jvfausto 23:58ec657a44f2 337 void Wheelchair::odomMsg(){
jvfausto 23:58ec657a44f2 338 double dist_new = curr_pos;
jvfausto 23:58ec657a44f2 339 double dist = dist_new-dist_old;
jvfausto 23:58ec657a44f2 340 double temp_x = dist*sin(z_angular*3.14159/180);
jvfausto 23:58ec657a44f2 341 double temp_y = dist*cos(z_angular*3.14159/180);
jvfausto 23:58ec657a44f2 342
jvfausto 23:58ec657a44f2 343 x_position += temp_x;
jvfausto 23:58ec657a44f2 344 y_position += temp_y;
jvfausto 23:58ec657a44f2 345
jvfausto 23:58ec657a44f2 346 dist_old = dist_new;
jvfausto 23:58ec657a44f2 347 }
jvfausto 23:58ec657a44f2 348 void Wheelchair::showOdom(){
jvfausto 23:58ec657a44f2 349 out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular);
jvfausto 23:58ec657a44f2 350 }
ryanlin97 12:921488918749 351 float Wheelchair::getDistance() {
ryanlin97 12:921488918749 352 return wheel->getDistance(Diameter);
ryanlin97 12:921488918749 353 }
ryanlin97 12:921488918749 354
ryanlin97 12:921488918749 355 void Wheelchair::resetDistance(){
ryanlin97 12:921488918749 356 wheel->reset();
ryanlin97 12:921488918749 357 }
jvfausto 21:8d11d953ceeb 358 /*Predetermined paths For Demmo*/
jvfausto 19:71a6621ee5c3 359 void Wheelchair::desk() {
jvfausto 19:71a6621ee5c3 360 Wheelchair::pid_forward(5461);
jvfausto 19:71a6621ee5c3 361 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 362 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 363 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 364 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 365 }
jvfausto 23:58ec657a44f2 366
jvfausto 19:71a6621ee5c3 367 void Wheelchair::kitchen() {
jvfausto 19:71a6621ee5c3 368 Wheelchair::pid_forward(5461);
jvfausto 20:f42db4ae16f0 369 Wheelchair::pid_right(87);
jvfausto 19:71a6621ee5c3 370 Wheelchair::pid_forward(3658);
jvfausto 19:71a6621ee5c3 371 Wheelchair::pid_left(90);
jvfausto 19:71a6621ee5c3 372 Wheelchair::pid_forward(305);
jvfausto 19:71a6621ee5c3 373 }
jvfausto 23:58ec657a44f2 374
jvfausto 19:71a6621ee5c3 375 void Wheelchair::desk_to_kitchen(){
jvfausto 19:71a6621ee5c3 376 Wheelchair::pid_right(180);
jvfausto 19:71a6621ee5c3 377 Wheelchair::pid_forward(3700);
jvfausto 19:71a6621ee5c3 378 }
jvfausto 23:58ec657a44f2 379
jvfausto 23:58ec657a44f2 380