Odometry communication
Dependencies: PID QEI chair_BNO055 ros_lib_kinetic3
Dependents: wheelchaircontrolrealtime1
Fork of wheelchaircontrol by
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?
User | Revision | Line number | New 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 |