Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
BalancingRobot.cpp@11:0a2944e7be23, 2020-01-01 (annotated)
- Committer:
- berkyavuz1997
- Date:
- Wed Jan 01 20:31:22 2020 +0000
- Revision:
- 11:0a2944e7be23
- Parent:
- 9:67f2110fce8e
Committed for sharing.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Lauszus | 4:0b4c320bc948 | 1 | /* |
Lauszus | 4:0b4c320bc948 | 2 | * The code is released under the GNU General Public License. |
Lauszus | 4:0b4c320bc948 | 3 | * This is the algorithm for my balancing robot/segway. |
lakshmananag | 9:67f2110fce8e | 4 | * It is controlled by Bluetooth |
Lauszus | 4:0b4c320bc948 | 5 | */ |
Lauszus | 4:0b4c320bc948 | 6 | |
Lauszus | 4:0b4c320bc948 | 7 | #include "mbed.h" |
Lauszus | 4:0b4c320bc948 | 8 | #include "BalancingRobot.h" |
lakshmananag | 9:67f2110fce8e | 9 | #include "MPU6050.h" |
lakshmananag | 9:67f2110fce8e | 10 | #include "ledControl.h" |
berkyavuz1997 | 11:0a2944e7be23 | 11 | #include "ultrasonic.h" |
berkyavuz1997 | 11:0a2944e7be23 | 12 | |
berkyavuz1997 | 11:0a2944e7be23 | 13 | //#include "HALLFX_ENCODER.h" |
Lauszus | 4:0b4c320bc948 | 14 | |
Lauszus | 4:0b4c320bc948 | 15 | /* Setup serial communication */ |
berkyavuz1997 | 11:0a2944e7be23 | 16 | Serial blue(PTE22, PTE23); // For remote control ps3 |
lakshmananag | 9:67f2110fce8e | 17 | Serial pc(USBTX, USBRX); // USB |
Lauszus | 4:0b4c320bc948 | 18 | |
lakshmananag | 9:67f2110fce8e | 19 | /* IMU */ |
berkyavuz1997 | 11:0a2944e7be23 | 20 | MPU6050 mpu6050; |
berkyavuz1997 | 11:0a2944e7be23 | 21 | |
berkyavuz1997 | 11:0a2944e7be23 | 22 | |
lakshmananag | 9:67f2110fce8e | 23 | |
berkyavuz1997 | 11:0a2944e7be23 | 24 | /* HC-SR04 */ |
berkyavuz1997 | 11:0a2944e7be23 | 25 | void dist(int distance) |
berkyavuz1997 | 11:0a2944e7be23 | 26 | { |
berkyavuz1997 | 11:0a2944e7be23 | 27 | //put code here to happen when the distance is changed |
berkyavuz1997 | 11:0a2944e7be23 | 28 | |
berkyavuz1997 | 11:0a2944e7be23 | 29 | } |
berkyavuz1997 | 11:0a2944e7be23 | 30 | |
berkyavuz1997 | 11:0a2944e7be23 | 31 | ultrasonic frontUltra(PTA13, PTD5, .1, 1, &dist); |
berkyavuz1997 | 11:0a2944e7be23 | 32 | ultrasonic backUltra(PTD0, PTD2, .1, 1, &dist); |
Lauszus | 4:0b4c320bc948 | 33 | |
Lauszus | 4:0b4c320bc948 | 34 | /* Setup timer instance */ |
Lauszus | 4:0b4c320bc948 | 35 | Timer t; |
Lauszus | 4:0b4c320bc948 | 36 | |
lakshmananag | 9:67f2110fce8e | 37 | /* Ticker for led toggling */ |
berkyavuz1997 | 11:0a2944e7be23 | 38 | Ticker toggler1; |
berkyavuz1997 | 11:0a2944e7be23 | 39 | |
lakshmananag | 9:67f2110fce8e | 40 | |
berkyavuz1997 | 11:0a2944e7be23 | 41 | |
Lauszus | 4:0b4c320bc948 | 42 | int main() { |
berkyavuz1997 | 11:0a2944e7be23 | 43 | |
berkyavuz1997 | 11:0a2944e7be23 | 44 | |
berkyavuz1997 | 11:0a2944e7be23 | 45 | |
berkyavuz1997 | 11:0a2944e7be23 | 46 | |
Lauszus | 4:0b4c320bc948 | 47 | /* Set baudrate */ |
berkyavuz1997 | 11:0a2944e7be23 | 48 | blue.baud(115200); |
berkyavuz1997 | 11:0a2944e7be23 | 49 | pc.baud(115200); |
berkyavuz1997 | 11:0a2944e7be23 | 50 | |
Lauszus | 7:f1d24c6119ac | 51 | |
Lauszus | 4:0b4c320bc948 | 52 | /* Set PWM frequency */ |
Lauszus | 4:0b4c320bc948 | 53 | leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005) |
Lauszus | 4:0b4c320bc948 | 54 | rightPWM.period(0.00005); |
Lauszus | 7:f1d24c6119ac | 55 | |
berkyavuz1997 | 11:0a2944e7be23 | 56 | |
berkyavuz1997 | 11:0a2944e7be23 | 57 | |
Lauszus | 4:0b4c320bc948 | 58 | /* Calibrate the gyro and accelerometer relative to ground */ |
lakshmananag | 9:67f2110fce8e | 59 | mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading |
lakshmananag | 9:67f2110fce8e | 60 | wait(0.5); |
lakshmananag | 9:67f2110fce8e | 61 | mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers |
berkyavuz1997 | 11:0a2944e7be23 | 62 | //pc.printf("Calibration is completed. \r\n"); |
lakshmananag | 9:67f2110fce8e | 63 | mpu6050.init(); // Initialize the sensor |
berkyavuz1997 | 11:0a2944e7be23 | 64 | //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); |
berkyavuz1997 | 11:0a2944e7be23 | 65 | |
berkyavuz1997 | 11:0a2944e7be23 | 66 | |
Lauszus | 7:f1d24c6119ac | 67 | |
Lauszus | 4:0b4c320bc948 | 68 | /* Setup timing */ |
Lauszus | 4:0b4c320bc948 | 69 | t.start(); |
berkyavuz1997 | 11:0a2944e7be23 | 70 | |
Lauszus | 4:0b4c320bc948 | 71 | loopStartTime = t.read_us(); |
Lauszus | 4:0b4c320bc948 | 72 | timer = loopStartTime; |
berkyavuz1997 | 11:0a2944e7be23 | 73 | DesiredAngle = -2.5; |
berkyavuz1997 | 11:0a2944e7be23 | 74 | ConstantAngle = DesiredAngle; |
berkyavuz1997 | 11:0a2944e7be23 | 75 | |
Lauszus | 4:0b4c320bc948 | 76 | while (1) { |
berkyavuz1997 | 11:0a2944e7be23 | 77 | |
lakshmananag | 9:67f2110fce8e | 78 | /* Calculate pitch angle using a Complementary filter */ |
lakshmananag | 9:67f2110fce8e | 79 | mpu6050.complementaryFilter(&pitchAngle, &rollAngle); |
lakshmananag | 9:67f2110fce8e | 80 | wait_us(5); |
Lauszus | 4:0b4c320bc948 | 81 | timer = t.read_us(); |
Lauszus | 7:f1d24c6119ac | 82 | |
Lauszus | 4:0b4c320bc948 | 83 | /* Drive motors */ |
lakshmananag | 9:67f2110fce8e | 84 | if (abs(pitchAngle)> 20) // Stop if falling or laying down |
Lauszus | 4:0b4c320bc948 | 85 | stopAndReset(); |
Lauszus | 4:0b4c320bc948 | 86 | else |
berkyavuz1997 | 11:0a2944e7be23 | 87 | PID(DesiredAngle, targetOffset); |
berkyavuz1997 | 11:0a2944e7be23 | 88 | |
berkyavuz1997 | 11:0a2944e7be23 | 89 | /* Update wheel velocity every 100ms |
Lauszus | 4:0b4c320bc948 | 90 | loopCounter++; |
lakshmananag | 9:67f2110fce8e | 91 | if (loopCounter%10 == 0) |
lakshmananag | 9:67f2110fce8e | 92 | { // If remainder is equal 0, it must be 10,20,30 etc. |
lakshmananag | 9:67f2110fce8e | 93 | LeftWheelPosition = leftEncoder.read(); |
lakshmananag | 9:67f2110fce8e | 94 | RightWheelPosition = rightEncoder.read(); |
lakshmananag | 9:67f2110fce8e | 95 | wheelPosition = LeftWheelPosition + RightWheelPosition; |
Lauszus | 4:0b4c320bc948 | 96 | wheelVelocity = wheelPosition - lastWheelPosition; |
Lauszus | 4:0b4c320bc948 | 97 | lastWheelPosition = wheelPosition; |
Lauszus | 7:f1d24c6119ac | 98 | |
lakshmananag | 9:67f2110fce8e | 99 | if (abs(wheelVelocity) <= 20 && !stopped) |
lakshmananag | 9:67f2110fce8e | 100 | { // Set new targetPosition if braking |
Lauszus | 4:0b4c320bc948 | 101 | targetPosition = wheelPosition; |
Lauszus | 4:0b4c320bc948 | 102 | stopped = true; |
Lauszus | 4:0b4c320bc948 | 103 | } |
Lauszus | 4:0b4c320bc948 | 104 | } |
berkyavuz1997 | 11:0a2944e7be23 | 105 | */ |
Lauszus | 7:f1d24c6119ac | 106 | |
berkyavuz1997 | 11:0a2944e7be23 | 107 | //pc.printf("Pitch: %f, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue); |
lakshmananag | 9:67f2110fce8e | 108 | |
berkyavuz1997 | 11:0a2944e7be23 | 109 | //Check for incoming serial data |
lakshmananag | 9:67f2110fce8e | 110 | if (blue.readable()) // Check if there's any incoming data |
lakshmananag | 9:67f2110fce8e | 111 | receiveBluetooth(); |
berkyavuz1997 | 11:0a2944e7be23 | 112 | |
berkyavuz1997 | 11:0a2944e7be23 | 113 | |
berkyavuz1997 | 11:0a2944e7be23 | 114 | |
berkyavuz1997 | 11:0a2944e7be23 | 115 | frontUltra.startUpdates(); |
berkyavuz1997 | 11:0a2944e7be23 | 116 | |
berkyavuz1997 | 11:0a2944e7be23 | 117 | distF = frontUltra.getCurrentDistance(); |
berkyavuz1997 | 11:0a2944e7be23 | 118 | |
berkyavuz1997 | 11:0a2944e7be23 | 119 | backUltra.startUpdates(); |
berkyavuz1997 | 11:0a2944e7be23 | 120 | |
berkyavuz1997 | 11:0a2944e7be23 | 121 | distB = backUltra.getCurrentDistance(); |
berkyavuz1997 | 11:0a2944e7be23 | 122 | |
berkyavuz1997 | 11:0a2944e7be23 | 123 | |
berkyavuz1997 | 11:0a2944e7be23 | 124 | pc.printf("Front distance: %d cm \t Back Distance %d cm \r\n", distF, distB); |
berkyavuz1997 | 11:0a2944e7be23 | 125 | |
Lauszus | 4:0b4c320bc948 | 126 | /* Use a time fixed loop */ |
Lauszus | 4:0b4c320bc948 | 127 | lastLoopUsefulTime = t.read_us() - loopStartTime; |
Lauszus | 4:0b4c320bc948 | 128 | if (lastLoopUsefulTime < STD_LOOP_TIME) |
Lauszus | 4:0b4c320bc948 | 129 | wait_us(STD_LOOP_TIME - lastLoopUsefulTime); |
berkyavuz1997 | 11:0a2944e7be23 | 130 | |
Lauszus | 4:0b4c320bc948 | 131 | lastLoopTime = t.read_us() - loopStartTime; // only used for debugging |
berkyavuz1997 | 11:0a2944e7be23 | 132 | //pc.printf("STD_LOOP_TIME: %d\n\r",STD_LOOP_TIME); |
berkyavuz1997 | 11:0a2944e7be23 | 133 | //pc.printf("anlik: %d\n\r",t.read_us()); |
berkyavuz1997 | 11:0a2944e7be23 | 134 | //pc.printf("lastLoopTime: %d\n\r",lastLoopTime); |
berkyavuz1997 | 11:0a2944e7be23 | 135 | //pc.printf("lastLoopUsefulTime: %d\n\r",lastLoopTime); |
Lauszus | 4:0b4c320bc948 | 136 | loopStartTime = t.read_us(); |
Lauszus | 4:0b4c320bc948 | 137 | } |
Lauszus | 4:0b4c320bc948 | 138 | } |
lakshmananag | 9:67f2110fce8e | 139 | |
lakshmananag | 9:67f2110fce8e | 140 | void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);} |
lakshmananag | 9:67f2110fce8e | 141 | |
lakshmananag | 9:67f2110fce8e | 142 | void toggle_led1() {ledToggle(1);} |
lakshmananag | 9:67f2110fce8e | 143 | void toggle_led2() {ledToggle(2);} |
lakshmananag | 9:67f2110fce8e | 144 | |
lakshmananag | 9:67f2110fce8e | 145 | void PID(double restAngle, double offset) |
lakshmananag | 9:67f2110fce8e | 146 | { |
berkyavuz1997 | 11:0a2944e7be23 | 147 | |
Lauszus | 4:0b4c320bc948 | 148 | /* Steer robot */ |
berkyavuz1997 | 11:0a2944e7be23 | 149 | /*if (steerForward) |
lakshmananag | 9:67f2110fce8e | 150 | { |
berkyavuz1997 | 11:0a2944e7be23 | 151 | /* offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing |
berkyavuz1997 | 11:0a2944e7be23 | 152 | restAngle -= offset; |
berkyavuz1997 | 11:0a2944e7be23 | 153 | restAngle -= -1.0; |
berkyavuz1997 | 11:0a2944e7be23 | 154 | |
berkyavuz1997 | 11:0a2944e7be23 | 155 | |
berkyavuz1997 | 11:0a2944e7be23 | 156 | } |
lakshmananag | 9:67f2110fce8e | 157 | else if (steerBackward) |
lakshmananag | 9:67f2110fce8e | 158 | { |
berkyavuz1997 | 11:0a2944e7be23 | 159 | //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing |
berkyavuz1997 | 11:0a2944e7be23 | 160 | //restAngle += offset; |
berkyavuz1997 | 11:0a2944e7be23 | 161 | restAngle += 1.0; |
berkyavuz1997 | 11:0a2944e7be23 | 162 | }*/ |
lakshmananag | 9:67f2110fce8e | 163 | /* Brake */ |
berkyavuz1997 | 11:0a2944e7be23 | 164 | /*else if (steerStop) |
berkyavuz1997 | 11:0a2944e7be23 | 165 | |
berkyavuz1997 | 11:0a2944e7be23 | 166 | restAngle = 0.0; |
lakshmananag | 9:67f2110fce8e | 167 | { |
lakshmananag | 9:67f2110fce8e | 168 | //long positionError = wheelPosition - targetPosition; |
lakshmananag | 9:67f2110fce8e | 169 | |
lakshmananag | 9:67f2110fce8e | 170 | /* if (abs(positionError) > zoneA) // Inside zone A |
Lauszus | 4:0b4c320bc948 | 171 | restAngle -= (double)positionError/positionScaleA; |
Lauszus | 4:0b4c320bc948 | 172 | else if (abs(positionError) > zoneB) // Inside zone B |
Lauszus | 4:0b4c320bc948 | 173 | restAngle -= (double)positionError/positionScaleB; |
Lauszus | 4:0b4c320bc948 | 174 | else // Inside zone C |
lakshmananag | 9:67f2110fce8e | 175 | restAngle -= (double)positionError/positionScaleC; */ |
Lauszus | 7:f1d24c6119ac | 176 | |
berkyavuz1997 | 11:0a2944e7be23 | 177 | //restAngle -= (double)wheelVelocity/velocityScaleStop; |
Lauszus | 7:f1d24c6119ac | 178 | |
berkyavuz1997 | 11:0a2944e7be23 | 179 | /*if (restAngle < -5.00) // Limit rest Angle |
lakshmananag | 9:67f2110fce8e | 180 | restAngle = -5.00; |
lakshmananag | 9:67f2110fce8e | 181 | else if (restAngle > 5.00) |
lakshmananag | 9:67f2110fce8e | 182 | restAngle = 5.00; |
Lauszus | 4:0b4c320bc948 | 183 | } |
lakshmananag | 9:67f2110fce8e | 184 | |
Lauszus | 4:0b4c320bc948 | 185 | /* Update PID values */ |
berkyavuz1997 | 11:0a2944e7be23 | 186 | error1 = restAngle - pitchAngle; |
berkyavuz1997 | 11:0a2944e7be23 | 187 | double pTerm = Kp * error1; |
berkyavuz1997 | 11:0a2944e7be23 | 188 | iTerm += Ki * error1; |
berkyavuz1997 | 11:0a2944e7be23 | 189 | double dTerm = Kd * (error1 - lastError); |
berkyavuz1997 | 11:0a2944e7be23 | 190 | lastError = error1; |
Lauszus | 7:f1d24c6119ac | 191 | |
lakshmananag | 9:67f2110fce8e | 192 | PIDValue = pTerm + iTerm + dTerm; |
berkyavuz1997 | 11:0a2944e7be23 | 193 | //pc.printf("DesiredAngle = %lf", DesiredAngle); |
lakshmananag | 9:67f2110fce8e | 194 | //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd); |
Lauszus | 7:f1d24c6119ac | 195 | |
Lauszus | 4:0b4c320bc948 | 196 | /* Steer robot sideways */ |
Lauszus | 4:0b4c320bc948 | 197 | double PIDLeft; |
berkyavuz1997 | 11:0a2944e7be23 | 198 | double PIDRight; |
berkyavuz1997 | 11:0a2944e7be23 | 199 | |
Lauszus | 4:0b4c320bc948 | 200 | if (steerLeft) { |
Lauszus | 4:0b4c320bc948 | 201 | PIDLeft = PIDValue-turnSpeed; |
Lauszus | 4:0b4c320bc948 | 202 | PIDRight = PIDValue+turnSpeed; |
Lauszus | 4:0b4c320bc948 | 203 | } else if (steerRotateLeft) { |
Lauszus | 4:0b4c320bc948 | 204 | PIDLeft = PIDValue-rotateSpeed; |
Lauszus | 4:0b4c320bc948 | 205 | PIDRight = PIDValue+rotateSpeed; |
Lauszus | 4:0b4c320bc948 | 206 | } else if (steerRight) { |
Lauszus | 4:0b4c320bc948 | 207 | PIDLeft = PIDValue+turnSpeed; |
Lauszus | 4:0b4c320bc948 | 208 | PIDRight = PIDValue-turnSpeed; |
Lauszus | 4:0b4c320bc948 | 209 | } else if (steerRotateRight) { |
Lauszus | 4:0b4c320bc948 | 210 | PIDLeft = PIDValue+rotateSpeed; |
Lauszus | 4:0b4c320bc948 | 211 | PIDRight = PIDValue-rotateSpeed; |
Lauszus | 4:0b4c320bc948 | 212 | } else { |
berkyavuz1997 | 11:0a2944e7be23 | 213 | |
berkyavuz1997 | 11:0a2944e7be23 | 214 | if (steerForward && (DesiredAngle > ( ConstantAngle - 1.8 ) )) |
berkyavuz1997 | 11:0a2944e7be23 | 215 | DesiredAngle -= rotateSpeed; |
berkyavuz1997 | 11:0a2944e7be23 | 216 | |
berkyavuz1997 | 11:0a2944e7be23 | 217 | else if (steerBackward && (DesiredAngle < (ConstantAngle + 1.8) )) |
berkyavuz1997 | 11:0a2944e7be23 | 218 | DesiredAngle += rotateSpeed; |
berkyavuz1997 | 11:0a2944e7be23 | 219 | |
berkyavuz1997 | 11:0a2944e7be23 | 220 | else { |
berkyavuz1997 | 11:0a2944e7be23 | 221 | if(!steerForward && DesiredAngle < ConstantAngle) |
berkyavuz1997 | 11:0a2944e7be23 | 222 | DesiredAngle += rotateSpeed; |
berkyavuz1997 | 11:0a2944e7be23 | 223 | |
berkyavuz1997 | 11:0a2944e7be23 | 224 | else if(!steerBackward && DesiredAngle > ConstantAngle) |
berkyavuz1997 | 11:0a2944e7be23 | 225 | DesiredAngle -= rotateSpeed; |
berkyavuz1997 | 11:0a2944e7be23 | 226 | } |
berkyavuz1997 | 11:0a2944e7be23 | 227 | |
Lauszus | 4:0b4c320bc948 | 228 | PIDLeft = PIDValue; |
Lauszus | 4:0b4c320bc948 | 229 | PIDRight = PIDValue; |
berkyavuz1997 | 11:0a2944e7be23 | 230 | |
Lauszus | 4:0b4c320bc948 | 231 | } |
lakshmananag | 9:67f2110fce8e | 232 | //PIDLeft *= 0.95; // compensate for difference in the motors |
Lauszus | 7:f1d24c6119ac | 233 | |
Lauszus | 4:0b4c320bc948 | 234 | /* Set PWM Values */ |
berkyavuz1997 | 11:0a2944e7be23 | 235 | if (PIDLeft >= 0.0) |
berkyavuz1997 | 11:0a2944e7be23 | 236 | move(left, backward, PIDLeft + 0.55); |
Lauszus | 4:0b4c320bc948 | 237 | else |
berkyavuz1997 | 11:0a2944e7be23 | 238 | move(left, forward, (PIDLeft - 0.55) * -1); |
berkyavuz1997 | 11:0a2944e7be23 | 239 | if (PIDRight >= 0.0) |
berkyavuz1997 | 11:0a2944e7be23 | 240 | move(right, backward, PIDRight + 0.55); |
Lauszus | 4:0b4c320bc948 | 241 | else |
berkyavuz1997 | 11:0a2944e7be23 | 242 | move(right, forward, (PIDRight - 0.55) * -1); |
berkyavuz1997 | 11:0a2944e7be23 | 243 | //pc.printf("RestAngle3 %lf" ,restAngle); |
Lauszus | 4:0b4c320bc948 | 244 | } |
berkyavuz1997 | 11:0a2944e7be23 | 245 | |
lakshmananag | 9:67f2110fce8e | 246 | void receiveBluetooth() { |
lakshmananag | 9:67f2110fce8e | 247 | //char input[16]; // The serial buffer is only 16 characters |
lakshmananag | 9:67f2110fce8e | 248 | char phone_char; |
lakshmananag | 9:67f2110fce8e | 249 | //int i = 0; |
lakshmananag | 9:67f2110fce8e | 250 | //while (1) { |
lakshmananag | 9:67f2110fce8e | 251 | //input[i] = blue.getc(); |
lakshmananag | 9:67f2110fce8e | 252 | //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer |
lakshmananag | 9:67f2110fce8e | 253 | //break; |
lakshmananag | 9:67f2110fce8e | 254 | //i++; |
lakshmananag | 9:67f2110fce8e | 255 | |
lakshmananag | 9:67f2110fce8e | 256 | //pc.printf("Im here.."); |
lakshmananag | 9:67f2110fce8e | 257 | //} |
lakshmananag | 9:67f2110fce8e | 258 | phone_char = blue.getc(); |
berkyavuz1997 | 11:0a2944e7be23 | 259 | |
berkyavuz1997 | 11:0a2944e7be23 | 260 | /* if(distB < 50 && distB > 4) { |
berkyavuz1997 | 11:0a2944e7be23 | 261 | phone_char = 'S'; |
berkyavuz1997 | 11:0a2944e7be23 | 262 | if (distB < 20) |
berkyavuz1997 | 11:0a2944e7be23 | 263 | phone_char = 'F'; |
berkyavuz1997 | 11:0a2944e7be23 | 264 | } */ |
berkyavuz1997 | 11:0a2944e7be23 | 265 | |
berkyavuz1997 | 11:0a2944e7be23 | 266 | |
berkyavuz1997 | 11:0a2944e7be23 | 267 | if(distB < 20 && distB > 4) |
berkyavuz1997 | 11:0a2944e7be23 | 268 | phone_char = 'F'; |
berkyavuz1997 | 11:0a2944e7be23 | 269 | else if ( distB < 30) |
berkyavuz1997 | 11:0a2944e7be23 | 270 | phone_char = 'S'; |
berkyavuz1997 | 11:0a2944e7be23 | 271 | else if (distB < 50) |
berkyavuz1997 | 11:0a2944e7be23 | 272 | phone_char = 'B'; |
berkyavuz1997 | 11:0a2944e7be23 | 273 | |
berkyavuz1997 | 11:0a2944e7be23 | 274 | |
lakshmananag | 9:67f2110fce8e | 275 | pc.putc(phone_char); |
berkyavuz1997 | 11:0a2944e7be23 | 276 | pc.printf("Input: %c\n",phone_char); |
Lauszus | 7:f1d24c6119ac | 277 | |
Lauszus | 4:0b4c320bc948 | 278 | // Set all false |
Lauszus | 4:0b4c320bc948 | 279 | steerForward = false; |
Lauszus | 4:0b4c320bc948 | 280 | steerBackward = false; |
Lauszus | 4:0b4c320bc948 | 281 | steerStop = false; |
Lauszus | 4:0b4c320bc948 | 282 | steerLeft = false; |
Lauszus | 4:0b4c320bc948 | 283 | steerRotateLeft = false; |
Lauszus | 4:0b4c320bc948 | 284 | steerRight = false; |
Lauszus | 4:0b4c320bc948 | 285 | steerRotateRight = false; |
Lauszus | 7:f1d24c6119ac | 286 | |
Lauszus | 4:0b4c320bc948 | 287 | /* For remote control */ |
lakshmananag | 9:67f2110fce8e | 288 | if (/*input[0]*/phone_char == 'F') { // Forward |
lakshmananag | 9:67f2110fce8e | 289 | //strtok(/*input*/phone_char, ","); // Ignore 'F' |
lakshmananag | 9:67f2110fce8e | 290 | //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double |
lakshmananag | 9:67f2110fce8e | 291 | //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging |
berkyavuz1997 | 11:0a2944e7be23 | 292 | //DesiredAnglee = 0.8; |
berkyavuz1997 | 11:0a2944e7be23 | 293 | pc.printf("F"); |
Lauszus | 4:0b4c320bc948 | 294 | steerForward = true; |
lakshmananag | 9:67f2110fce8e | 295 | } else if (/*input[0]*/phone_char == 'B') { // Backward |
lakshmananag | 9:67f2110fce8e | 296 | //strtok(/*input*/phone_char, ","); // Ignore 'B' |
berkyavuz1997 | 11:0a2944e7be23 | 297 | //targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
lakshmananag | 9:67f2110fce8e | 298 | //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging |
berkyavuz1997 | 11:0a2944e7be23 | 299 | //DesiredAnglee = -5.2; |
berkyavuz1997 | 11:0a2944e7be23 | 300 | pc.printf("B"); |
Lauszus | 4:0b4c320bc948 | 301 | steerBackward = true; |
lakshmananag | 9:67f2110fce8e | 302 | } else if (/*input[0]*/phone_char == 'L') { // Left |
lakshmananag | 9:67f2110fce8e | 303 | if (/*input[0]*/phone_char == 'L') // Left Rotate |
Lauszus | 4:0b4c320bc948 | 304 | steerRotateLeft = true; |
Lauszus | 4:0b4c320bc948 | 305 | else |
Lauszus | 4:0b4c320bc948 | 306 | steerLeft = true; |
lakshmananag | 9:67f2110fce8e | 307 | } else if (/*input[0]*/phone_char == 'R') { // Right |
lakshmananag | 9:67f2110fce8e | 308 | if (/*input[0]*/phone_char == 'R') // Right Rotate |
Lauszus | 4:0b4c320bc948 | 309 | steerRotateRight = true; |
Lauszus | 4:0b4c320bc948 | 310 | else |
Lauszus | 4:0b4c320bc948 | 311 | steerRight = true; |
lakshmananag | 9:67f2110fce8e | 312 | } else if (/*input[0]*/phone_char == 'S') { // Stop |
Lauszus | 4:0b4c320bc948 | 313 | steerStop = true; |
Lauszus | 4:0b4c320bc948 | 314 | stopped = false; |
berkyavuz1997 | 11:0a2944e7be23 | 315 | //targetPosition = wheelPosition; |
lakshmananag | 9:67f2110fce8e | 316 | } else if (/*input[0]*/phone_char == 'A') { // Abort |
Lauszus | 4:0b4c320bc948 | 317 | stopAndReset(); |
lakshmananag | 9:67f2110fce8e | 318 | while (blue.getc() != 'C'); // Wait until continue is send |
berkyavuz1997 | 11:0a2944e7be23 | 319 | |
Lauszus | 4:0b4c320bc948 | 320 | } |
Lauszus | 4:0b4c320bc948 | 321 | } |
lakshmananag | 9:67f2110fce8e | 322 | |
Lauszus | 4:0b4c320bc948 | 323 | void stopAndReset() { |
Lauszus | 4:0b4c320bc948 | 324 | stop(both); |
Lauszus | 4:0b4c320bc948 | 325 | lastError = 0; |
Lauszus | 4:0b4c320bc948 | 326 | iTerm = 0; |
Lauszus | 4:0b4c320bc948 | 327 | targetPosition = wheelPosition; |
Lauszus | 4:0b4c320bc948 | 328 | } |
lakshmananag | 9:67f2110fce8e | 329 | |
Lauszus | 4:0b4c320bc948 | 330 | void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f) |
Lauszus | 4:0b4c320bc948 | 331 | if (motor == left) { |
Lauszus | 4:0b4c320bc948 | 332 | leftPWM = speed; |
Lauszus | 4:0b4c320bc948 | 333 | if (direction == forward) { |
Lauszus | 4:0b4c320bc948 | 334 | leftA = 0; |
Lauszus | 4:0b4c320bc948 | 335 | leftB = 1; |
Lauszus | 4:0b4c320bc948 | 336 | } else if (direction == backward) { |
Lauszus | 4:0b4c320bc948 | 337 | leftA = 1; |
Lauszus | 4:0b4c320bc948 | 338 | leftB = 0; |
Lauszus | 4:0b4c320bc948 | 339 | } |
Lauszus | 4:0b4c320bc948 | 340 | } else if (motor == right) { |
Lauszus | 4:0b4c320bc948 | 341 | rightPWM = speed; |
Lauszus | 4:0b4c320bc948 | 342 | if (direction == forward) { |
Lauszus | 4:0b4c320bc948 | 343 | rightA = 0; |
Lauszus | 4:0b4c320bc948 | 344 | rightB = 1; |
Lauszus | 4:0b4c320bc948 | 345 | } else if (direction == backward) { |
Lauszus | 4:0b4c320bc948 | 346 | rightA = 1; |
Lauszus | 4:0b4c320bc948 | 347 | rightB = 0; |
Lauszus | 4:0b4c320bc948 | 348 | } |
Lauszus | 4:0b4c320bc948 | 349 | } else if (motor == both) { |
Lauszus | 4:0b4c320bc948 | 350 | leftPWM = speed; |
Lauszus | 4:0b4c320bc948 | 351 | rightPWM = speed; |
Lauszus | 4:0b4c320bc948 | 352 | if (direction == forward) { |
Lauszus | 4:0b4c320bc948 | 353 | leftA = 0; |
Lauszus | 4:0b4c320bc948 | 354 | leftB = 1; |
Lauszus | 7:f1d24c6119ac | 355 | |
Lauszus | 4:0b4c320bc948 | 356 | rightA = 0; |
Lauszus | 4:0b4c320bc948 | 357 | rightB = 1; |
Lauszus | 4:0b4c320bc948 | 358 | } else if (direction == backward) { |
Lauszus | 4:0b4c320bc948 | 359 | leftA = 1; |
Lauszus | 4:0b4c320bc948 | 360 | leftB = 0; |
Lauszus | 7:f1d24c6119ac | 361 | |
Lauszus | 4:0b4c320bc948 | 362 | rightA = 1; |
Lauszus | 4:0b4c320bc948 | 363 | rightB = 0; |
Lauszus | 4:0b4c320bc948 | 364 | } |
Lauszus | 4:0b4c320bc948 | 365 | } |
Lauszus | 4:0b4c320bc948 | 366 | } |
Lauszus | 4:0b4c320bc948 | 367 | void stop(Motor motor) { |
Lauszus | 4:0b4c320bc948 | 368 | if (motor == left) { |
Lauszus | 4:0b4c320bc948 | 369 | leftPWM = 1; |
Lauszus | 4:0b4c320bc948 | 370 | leftA = 1; |
Lauszus | 4:0b4c320bc948 | 371 | leftB = 1; |
Lauszus | 4:0b4c320bc948 | 372 | } else if (motor == right) { |
Lauszus | 4:0b4c320bc948 | 373 | rightPWM = 1; |
Lauszus | 4:0b4c320bc948 | 374 | rightA = 1; |
Lauszus | 4:0b4c320bc948 | 375 | rightB = 1; |
Lauszus | 4:0b4c320bc948 | 376 | } else if (motor == both) { |
Lauszus | 4:0b4c320bc948 | 377 | leftPWM = 1; |
Lauszus | 4:0b4c320bc948 | 378 | leftA = 1; |
Lauszus | 4:0b4c320bc948 | 379 | leftB = 1; |
Lauszus | 7:f1d24c6119ac | 380 | |
Lauszus | 4:0b4c320bc948 | 381 | rightPWM = 1; |
Lauszus | 4:0b4c320bc948 | 382 | rightA = 1; |
Lauszus | 4:0b4c320bc948 | 383 | rightB = 1; |
Lauszus | 4:0b4c320bc948 | 384 | } |
Lauszus | 0:f5c02b620688 | 385 | } |