Kristian Lauszus
Code for my balancing robot, controlled with a PS3 controller via bluetooth
BalancingRobot.cpp@8:87dd9d5f7001, 2012-04-13 (annotated)
- Committer:
- Lauszus
- Date:
- Fri Apr 13 19:45:25 2012 +0000
- Revision:
- 8:87dd9d5f7001
- Parent:
- 7:f1d24c6119ac
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 | * Developed by Kristian Lauszus |
| Lauszus | 4:0b4c320bc948 | 4 | * This is the algorithm for my balancing robot/segway. |
| Lauszus | 4:0b4c320bc948 | 5 | * It is controlled by a PS3 Controller via bluetooth. |
| Lauszus | 4:0b4c320bc948 | 6 | * The remote control code can be found at my other github repository: https://github.com/TKJElectronics/BalancingRobot |
| Lauszus | 4:0b4c320bc948 | 7 | * For details, see http://blog.tkjelectronics.dk/2012/02/the-balancing-robot/ |
| Lauszus | 4:0b4c320bc948 | 8 | */ |
| Lauszus | 4:0b4c320bc948 | 9 | |
| Lauszus | 4:0b4c320bc948 | 10 | #include "mbed.h" |
| Lauszus | 4:0b4c320bc948 | 11 | #include "BalancingRobot.h" |
| Lauszus | 4:0b4c320bc948 | 12 | #include "Encoder.h" |
| Lauszus | 4:0b4c320bc948 | 13 | |
| Lauszus | 4:0b4c320bc948 | 14 | /* Setup serial communication */ |
| Lauszus | 4:0b4c320bc948 | 15 | Serial xbee(p13,p14); // For wireless debugging and setting PID constants |
| Lauszus | 4:0b4c320bc948 | 16 | Serial ps3(p9,p10); // For remote control |
| Lauszus | 4:0b4c320bc948 | 17 | Serial debug(USBTX, USBRX); // USB |
| Lauszus | 4:0b4c320bc948 | 18 | |
| Lauszus | 4:0b4c320bc948 | 19 | /* Setup encoders */ |
| Lauszus | 4:0b4c320bc948 | 20 | Encoder leftEncoder(p29,p30); |
| Lauszus | 4:0b4c320bc948 | 21 | Encoder rightEncoder(p28,p27); |
| Lauszus | 4:0b4c320bc948 | 22 | |
| Lauszus | 4:0b4c320bc948 | 23 | /* Setup timer instance */ |
| Lauszus | 4:0b4c320bc948 | 24 | Timer t; |
| Lauszus | 4:0b4c320bc948 | 25 | |
| Lauszus | 4:0b4c320bc948 | 26 | int main() { |
| Lauszus | 4:0b4c320bc948 | 27 | /* Set baudrate */ |
| Lauszus | 4:0b4c320bc948 | 28 | xbee.baud(115200); |
| Lauszus | 4:0b4c320bc948 | 29 | ps3.baud(115200); |
| Lauszus | 4:0b4c320bc948 | 30 | debug.baud(115200); |
| Lauszus | 7:f1d24c6119ac | 31 | |
| Lauszus | 4:0b4c320bc948 | 32 | /* Set PWM frequency */ |
| Lauszus | 4:0b4c320bc948 | 33 | leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005) |
| Lauszus | 4:0b4c320bc948 | 34 | rightPWM.period(0.00005); |
| Lauszus | 7:f1d24c6119ac | 35 | |
| Lauszus | 4:0b4c320bc948 | 36 | /* Calibrate the gyro and accelerometer relative to ground */ |
| Lauszus | 4:0b4c320bc948 | 37 | calibrateSensors(); |
| Lauszus | 7:f1d24c6119ac | 38 | |
| Lauszus | 4:0b4c320bc948 | 39 | xbee.printf("Initialized\n"); |
| Lauszus | 4:0b4c320bc948 | 40 | processing(); // Send output to processing application |
| Lauszus | 7:f1d24c6119ac | 41 | |
| Lauszus | 4:0b4c320bc948 | 42 | /* Setup timing */ |
| Lauszus | 4:0b4c320bc948 | 43 | t.start(); |
| Lauszus | 4:0b4c320bc948 | 44 | loopStartTime = t.read_us(); |
| Lauszus | 4:0b4c320bc948 | 45 | timer = loopStartTime; |
| Lauszus | 7:f1d24c6119ac | 46 | |
| Lauszus | 4:0b4c320bc948 | 47 | while (1) { |
| Lauszus | 4:0b4c320bc948 | 48 | /* Calculate pitch */ |
| Lauszus | 4:0b4c320bc948 | 49 | accYangle = getAccY(); |
| Lauszus | 4:0b4c320bc948 | 50 | gyroYrate = getGyroYrate(); |
| Lauszus | 7:f1d24c6119ac | 51 | |
| Lauszus | 4:0b4c320bc948 | 52 | // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html |
| Lauszus | 4:0b4c320bc948 | 53 | pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter |
| Lauszus | 4:0b4c320bc948 | 54 | timer = t.read_us(); |
| Lauszus | 7:f1d24c6119ac | 55 | |
| Lauszus | 4:0b4c320bc948 | 56 | //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle); |
| Lauszus | 7:f1d24c6119ac | 57 | |
| Lauszus | 4:0b4c320bc948 | 58 | /* Drive motors */ |
| Lauszus | 4:0b4c320bc948 | 59 | if (pitch < 60 || pitch > 120) // Stop if falling or laying down |
| Lauszus | 4:0b4c320bc948 | 60 | stopAndReset(); |
| Lauszus | 4:0b4c320bc948 | 61 | else |
| Lauszus | 4:0b4c320bc948 | 62 | PID(targetAngle,targetOffset); |
| Lauszus | 7:f1d24c6119ac | 63 | |
| Lauszus | 4:0b4c320bc948 | 64 | /* Update wheel velocity every 100ms */ |
| Lauszus | 4:0b4c320bc948 | 65 | loopCounter++; |
| Lauszus | 4:0b4c320bc948 | 66 | if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc. |
| Lauszus | 4:0b4c320bc948 | 67 | //xbee.printf("Wheel - timer: %i\n",t.read_ms()); |
| Lauszus | 4:0b4c320bc948 | 68 | wheelPosition = leftEncoder.read() + rightEncoder.read(); |
| Lauszus | 4:0b4c320bc948 | 69 | wheelVelocity = wheelPosition - lastWheelPosition; |
| Lauszus | 4:0b4c320bc948 | 70 | lastWheelPosition = wheelPosition; |
| Lauszus | 4:0b4c320bc948 | 71 | //xbee.printf("WheelVelocity: %i\n",wheelVelocity); |
| Lauszus | 7:f1d24c6119ac | 72 | |
| Lauszus | 4:0b4c320bc948 | 73 | if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking |
| Lauszus | 4:0b4c320bc948 | 74 | targetPosition = wheelPosition; |
| Lauszus | 4:0b4c320bc948 | 75 | stopped = true; |
| Lauszus | 4:0b4c320bc948 | 76 | //xbee.printf("Stopped\n"); |
| Lauszus | 4:0b4c320bc948 | 77 | } |
| Lauszus | 4:0b4c320bc948 | 78 | } |
| Lauszus | 7:f1d24c6119ac | 79 | |
| Lauszus | 4:0b4c320bc948 | 80 | /* Check for incoming serial data */ |
| Lauszus | 4:0b4c320bc948 | 81 | if (ps3.readable()) // Check if there's any incoming data |
| Lauszus | 4:0b4c320bc948 | 82 | receivePS3(); |
| Lauszus | 4:0b4c320bc948 | 83 | if (xbee.readable()) // For setting the PID values |
| Lauszus | 4:0b4c320bc948 | 84 | receiveXbee(); |
| Lauszus | 7:f1d24c6119ac | 85 | |
| Lauszus | 4:0b4c320bc948 | 86 | /* Read battery voltage every 1s */ |
| Lauszus | 4:0b4c320bc948 | 87 | if (loopCounter == 100) { |
| Lauszus | 4:0b4c320bc948 | 88 | loopCounter = 0; // Reset loop counter |
| Lauszus | 4:0b4c320bc948 | 89 | double analogVoltage = batteryVoltage.read()/1*3.3; // Convert to voltage |
| Lauszus | 5:fc6c7a059759 | 90 | analogVoltage *= 6.6; // The analog pin is connected to a 56k-10k voltage divider |
| Lauszus | 4:0b4c320bc948 | 91 | xbee.printf("analogVoltage: %f - timer: %i\n",analogVoltage,t.read_ms()); |
| Lauszus | 6:defe36ce2346 | 92 | if (analogVoltage < 9 && pitch > 60 && pitch < 120) // Set buzzer on, if voltage gets critical low |
| Lauszus | 7:f1d24c6119ac | 93 | buzzer = 1; // The mbed resets at aproximatly 8.5V |
| Lauszus | 4:0b4c320bc948 | 94 | } |
| Lauszus | 7:f1d24c6119ac | 95 | |
| Lauszus | 4:0b4c320bc948 | 96 | /* Use a time fixed loop */ |
| Lauszus | 4:0b4c320bc948 | 97 | lastLoopUsefulTime = t.read_us() - loopStartTime; |
| Lauszus | 4:0b4c320bc948 | 98 | if (lastLoopUsefulTime < STD_LOOP_TIME) |
| Lauszus | 4:0b4c320bc948 | 99 | wait_us(STD_LOOP_TIME - lastLoopUsefulTime); |
| Lauszus | 4:0b4c320bc948 | 100 | lastLoopTime = t.read_us() - loopStartTime; // only used for debugging |
| Lauszus | 4:0b4c320bc948 | 101 | loopStartTime = t.read_us(); |
| Lauszus | 7:f1d24c6119ac | 102 | |
| Lauszus | 4:0b4c320bc948 | 103 | //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime); |
| Lauszus | 4:0b4c320bc948 | 104 | } |
| Lauszus | 4:0b4c320bc948 | 105 | } |
| Lauszus | 4:0b4c320bc948 | 106 | void PID(double restAngle, double offset) { |
| Lauszus | 4:0b4c320bc948 | 107 | /* Steer robot */ |
| Lauszus | 4:0b4c320bc948 | 108 | if (steerForward) { |
| Lauszus | 4:0b4c320bc948 | 109 | offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing |
| Lauszus | 4:0b4c320bc948 | 110 | restAngle -= offset; |
| Lauszus | 4:0b4c320bc948 | 111 | //xbee.printf("Forward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity); |
| Lauszus | 4:0b4c320bc948 | 112 | } else if (steerBackward) { |
| Lauszus | 4:0b4c320bc948 | 113 | offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing |
| Lauszus | 4:0b4c320bc948 | 114 | restAngle += offset; |
| Lauszus | 4:0b4c320bc948 | 115 | //xbee.printf("Backward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity); |
| Lauszus | 4:0b4c320bc948 | 116 | } |
| Lauszus | 4:0b4c320bc948 | 117 | /* Break */ |
| Lauszus | 4:0b4c320bc948 | 118 | else if (steerStop) { |
| Lauszus | 4:0b4c320bc948 | 119 | long positionError = wheelPosition - targetPosition; |
| Lauszus | 4:0b4c320bc948 | 120 | if (abs(positionError) > zoneA) // Inside zone A |
| Lauszus | 4:0b4c320bc948 | 121 | restAngle -= (double)positionError/positionScaleA; |
| Lauszus | 4:0b4c320bc948 | 122 | else if (abs(positionError) > zoneB) // Inside zone B |
| Lauszus | 4:0b4c320bc948 | 123 | restAngle -= (double)positionError/positionScaleB; |
| Lauszus | 4:0b4c320bc948 | 124 | else // Inside zone C |
| Lauszus | 4:0b4c320bc948 | 125 | restAngle -= (double)positionError/positionScaleC; |
| Lauszus | 7:f1d24c6119ac | 126 | |
| Lauszus | 4:0b4c320bc948 | 127 | restAngle -= (double)wheelVelocity/velocityScaleStop; |
| Lauszus | 7:f1d24c6119ac | 128 | |
| Lauszus | 4:0b4c320bc948 | 129 | if (restAngle < 80) // Limit rest Angle |
| Lauszus | 4:0b4c320bc948 | 130 | restAngle = 80; |
| Lauszus | 4:0b4c320bc948 | 131 | else if (restAngle > 100) |
| Lauszus | 4:0b4c320bc948 | 132 | restAngle = 100; |
| Lauszus | 4:0b4c320bc948 | 133 | //xbee.printf("restAngle: %f\t WheelVelocity: %i positionError: %i\n",restAngle,wheelVelocity,positionError); |
| Lauszus | 4:0b4c320bc948 | 134 | } |
| Lauszus | 4:0b4c320bc948 | 135 | /* Update PID values */ |
| Lauszus | 4:0b4c320bc948 | 136 | double error = (restAngle - pitch)/100; |
| Lauszus | 4:0b4c320bc948 | 137 | double pTerm = Kp * error; |
| Lauszus | 4:0b4c320bc948 | 138 | iTerm += Ki * error; |
| Lauszus | 4:0b4c320bc948 | 139 | double dTerm = Kd * (error - lastError); |
| Lauszus | 4:0b4c320bc948 | 140 | lastError = error; |
| Lauszus | 7:f1d24c6119ac | 141 | |
| Lauszus | 4:0b4c320bc948 | 142 | double PIDValue = pTerm + iTerm + dTerm; |
| Lauszus | 7:f1d24c6119ac | 143 | |
| Lauszus | 4:0b4c320bc948 | 144 | //debug.printf("Pitch: %5.2f\tPIDValue: %5.2f\tpTerm: %5.2f\tiTerm: %5.2f\tdTerm: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,pTerm,iTerm,dTerm,Kp,Ki,Kd); |
| Lauszus | 7:f1d24c6119ac | 145 | |
| Lauszus | 4:0b4c320bc948 | 146 | /* Steer robot sideways */ |
| Lauszus | 4:0b4c320bc948 | 147 | double PIDLeft; |
| Lauszus | 4:0b4c320bc948 | 148 | double PIDRight; |
| Lauszus | 4:0b4c320bc948 | 149 | if (steerLeft) { |
| Lauszus | 4:0b4c320bc948 | 150 | PIDLeft = PIDValue-turnSpeed; |
| Lauszus | 4:0b4c320bc948 | 151 | PIDRight = PIDValue+turnSpeed; |
| Lauszus | 4:0b4c320bc948 | 152 | } else if (steerRotateLeft) { |
| Lauszus | 4:0b4c320bc948 | 153 | PIDLeft = PIDValue-rotateSpeed; |
| Lauszus | 4:0b4c320bc948 | 154 | PIDRight = PIDValue+rotateSpeed; |
| Lauszus | 4:0b4c320bc948 | 155 | } else if (steerRight) { |
| Lauszus | 4:0b4c320bc948 | 156 | PIDLeft = PIDValue+turnSpeed; |
| Lauszus | 4:0b4c320bc948 | 157 | PIDRight = PIDValue-turnSpeed; |
| Lauszus | 4:0b4c320bc948 | 158 | } else if (steerRotateRight) { |
| Lauszus | 4:0b4c320bc948 | 159 | PIDLeft = PIDValue+rotateSpeed; |
| Lauszus | 4:0b4c320bc948 | 160 | PIDRight = PIDValue-rotateSpeed; |
| Lauszus | 4:0b4c320bc948 | 161 | } else { |
| Lauszus | 4:0b4c320bc948 | 162 | PIDLeft = PIDValue; |
| Lauszus | 4:0b4c320bc948 | 163 | PIDRight = PIDValue; |
| Lauszus | 4:0b4c320bc948 | 164 | } |
| Lauszus | 4:0b4c320bc948 | 165 | PIDLeft *= 0.95; // compensate for difference in the motors |
| Lauszus | 7:f1d24c6119ac | 166 | |
| Lauszus | 4:0b4c320bc948 | 167 | /* Set PWM Values */ |
| Lauszus | 4:0b4c320bc948 | 168 | if (PIDLeft >= 0) |
| Lauszus | 4:0b4c320bc948 | 169 | move(left, forward, PIDLeft); |
| Lauszus | 4:0b4c320bc948 | 170 | else |
| Lauszus | 4:0b4c320bc948 | 171 | move(left, backward, PIDLeft * -1); |
| Lauszus | 4:0b4c320bc948 | 172 | if (PIDRight >= 0) |
| Lauszus | 4:0b4c320bc948 | 173 | move(right, forward, PIDRight); |
| Lauszus | 4:0b4c320bc948 | 174 | else |
| Lauszus | 4:0b4c320bc948 | 175 | move(right, backward, PIDRight * -1); |
| Lauszus | 4:0b4c320bc948 | 176 | } |
| Lauszus | 4:0b4c320bc948 | 177 | void receivePS3() { |
| Lauszus | 4:0b4c320bc948 | 178 | char input[16]; // The serial buffer is only 16 characters |
| Lauszus | 4:0b4c320bc948 | 179 | int i = 0; |
| Lauszus | 4:0b4c320bc948 | 180 | while (1) { |
| Lauszus | 4:0b4c320bc948 | 181 | input[i] = ps3.getc(); |
| Lauszus | 4:0b4c320bc948 | 182 | if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer |
| Lauszus | 4:0b4c320bc948 | 183 | break; |
| Lauszus | 4:0b4c320bc948 | 184 | i++; |
| Lauszus | 4:0b4c320bc948 | 185 | } |
| Lauszus | 4:0b4c320bc948 | 186 | //debug.printf("Input: %s\n",input); |
| Lauszus | 7:f1d24c6119ac | 187 | |
| Lauszus | 4:0b4c320bc948 | 188 | // Set all false |
| Lauszus | 4:0b4c320bc948 | 189 | steerForward = false; |
| Lauszus | 4:0b4c320bc948 | 190 | steerBackward = false; |
| Lauszus | 4:0b4c320bc948 | 191 | steerStop = false; |
| Lauszus | 4:0b4c320bc948 | 192 | steerLeft = false; |
| Lauszus | 4:0b4c320bc948 | 193 | steerRotateLeft = false; |
| Lauszus | 4:0b4c320bc948 | 194 | steerRight = false; |
| Lauszus | 4:0b4c320bc948 | 195 | steerRotateRight = false; |
| Lauszus | 7:f1d24c6119ac | 196 | |
| Lauszus | 4:0b4c320bc948 | 197 | /* For remote control */ |
| Lauszus | 4:0b4c320bc948 | 198 | if (input[0] == 'F') { // Forward |
| Lauszus | 4:0b4c320bc948 | 199 | strtok(input, ","); // Ignore 'F' |
| Lauszus | 4:0b4c320bc948 | 200 | targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 201 | //xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging |
| Lauszus | 4:0b4c320bc948 | 202 | steerForward = true; |
| Lauszus | 4:0b4c320bc948 | 203 | } else if (input[0] == 'B') { // Backward |
| Lauszus | 4:0b4c320bc948 | 204 | strtok(input, ","); // Ignore 'B' |
| Lauszus | 4:0b4c320bc948 | 205 | targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 206 | //xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging |
| Lauszus | 4:0b4c320bc948 | 207 | steerBackward = true; |
| Lauszus | 4:0b4c320bc948 | 208 | } else if (input[0] == 'L') { // Left |
| Lauszus | 4:0b4c320bc948 | 209 | if (input[1] == 'R') // Left Rotate |
| Lauszus | 4:0b4c320bc948 | 210 | steerRotateLeft = true; |
| Lauszus | 4:0b4c320bc948 | 211 | else |
| Lauszus | 4:0b4c320bc948 | 212 | steerLeft = true; |
| Lauszus | 4:0b4c320bc948 | 213 | } else if (input[0] == 'R') { // Right |
| Lauszus | 4:0b4c320bc948 | 214 | if (input[1] == 'R') // Right Rotate |
| Lauszus | 4:0b4c320bc948 | 215 | steerRotateRight = true; |
| Lauszus | 4:0b4c320bc948 | 216 | else |
| Lauszus | 4:0b4c320bc948 | 217 | steerRight = true; |
| Lauszus | 4:0b4c320bc948 | 218 | } else if (input[0] == 'S') { // Stop |
| Lauszus | 4:0b4c320bc948 | 219 | steerStop = true; |
| Lauszus | 4:0b4c320bc948 | 220 | stopped = false; |
| Lauszus | 4:0b4c320bc948 | 221 | targetPosition = wheelPosition; |
| Lauszus | 4:0b4c320bc948 | 222 | } |
| Lauszus | 7:f1d24c6119ac | 223 | |
| Lauszus | 4:0b4c320bc948 | 224 | else if (input[0] == 'T') { // Set the target angle |
| Lauszus | 4:0b4c320bc948 | 225 | strtok(input, ","); // Ignore 'T' |
| Lauszus | 4:0b4c320bc948 | 226 | targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 227 | xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging |
| Lauszus | 4:0b4c320bc948 | 228 | } else if (input[0] == 'A') { // Abort |
| Lauszus | 4:0b4c320bc948 | 229 | stopAndReset(); |
| Lauszus | 4:0b4c320bc948 | 230 | while (ps3.getc() != 'C'); // Wait until continue is send |
| Lauszus | 4:0b4c320bc948 | 231 | } |
| Lauszus | 4:0b4c320bc948 | 232 | } |
| Lauszus | 4:0b4c320bc948 | 233 | void receiveXbee() { |
| Lauszus | 4:0b4c320bc948 | 234 | char input[16]; // The serial buffer is only 16 characters |
| Lauszus | 4:0b4c320bc948 | 235 | int i = 0; |
| Lauszus | 4:0b4c320bc948 | 236 | while (1) { |
| Lauszus | 4:0b4c320bc948 | 237 | input[i] = xbee.getc(); |
| Lauszus | 4:0b4c320bc948 | 238 | if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer |
| Lauszus | 4:0b4c320bc948 | 239 | break; |
| Lauszus | 4:0b4c320bc948 | 240 | i++; |
| Lauszus | 4:0b4c320bc948 | 241 | } |
| Lauszus | 4:0b4c320bc948 | 242 | //debug.printf("Input: %s\n",input); |
| Lauszus | 7:f1d24c6119ac | 243 | |
| Lauszus | 4:0b4c320bc948 | 244 | if (input[0] == 'T') { // Set the target angle |
| Lauszus | 4:0b4c320bc948 | 245 | strtok(input, ","); // Ignore 'T' |
| Lauszus | 4:0b4c320bc948 | 246 | targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 247 | } else if (input[0] == 'P') { |
| Lauszus | 4:0b4c320bc948 | 248 | strtok(input, ",");//Ignore 'P' |
| Lauszus | 4:0b4c320bc948 | 249 | Kp = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 250 | } else if (input[0] == 'I') { |
| Lauszus | 4:0b4c320bc948 | 251 | strtok(input, ",");//Ignore 'I' |
| Lauszus | 4:0b4c320bc948 | 252 | Ki = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 253 | } else if (input[0] == 'D') { |
| Lauszus | 4:0b4c320bc948 | 254 | strtok(input, ",");//Ignore 'D' |
| Lauszus | 4:0b4c320bc948 | 255 | Kd = atof(strtok(NULL, ";")); // read until the end and then convert from string to double |
| Lauszus | 4:0b4c320bc948 | 256 | } else if (input[0] == 'A') { // Abort |
| Lauszus | 4:0b4c320bc948 | 257 | stopAndReset(); |
| Lauszus | 4:0b4c320bc948 | 258 | while (xbee.getc() != 'C'); // Wait until continue is send |
| Lauszus | 4:0b4c320bc948 | 259 | } else if (input[0] == 'G') // The processing application sends this at startup |
| Lauszus | 4:0b4c320bc948 | 260 | processing(); // Send output to processing application |
| Lauszus | 4:0b4c320bc948 | 261 | } |
| Lauszus | 4:0b4c320bc948 | 262 | void processing() { |
| Lauszus | 4:0b4c320bc948 | 263 | /* Send output to processing application */ |
| Lauszus | 4:0b4c320bc948 | 264 | xbee.printf("Processing,%5.2f,%5.2f,%5.2f,%5.2f\n",Kp,Ki,Kd,targetAngle); |
| Lauszus | 4:0b4c320bc948 | 265 | } |
| Lauszus | 4:0b4c320bc948 | 266 | void stopAndReset() { |
| Lauszus | 4:0b4c320bc948 | 267 | stop(both); |
| Lauszus | 4:0b4c320bc948 | 268 | lastError = 0; |
| Lauszus | 4:0b4c320bc948 | 269 | iTerm = 0; |
| Lauszus | 4:0b4c320bc948 | 270 | targetPosition = wheelPosition; |
| Lauszus | 7:f1d24c6119ac | 271 | buzzer = 0; |
| Lauszus | 4:0b4c320bc948 | 272 | } |
| Lauszus | 4:0b4c320bc948 | 273 | double kalman(double newAngle, double newRate, double dtime) { |
| Lauszus | 4:0b4c320bc948 | 274 | // KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145 |
| Lauszus | 4:0b4c320bc948 | 275 | // with slightly modifications by Kristian Lauszus |
| Lauszus | 4:0b4c320bc948 | 276 | // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information |
| Lauszus | 4:0b4c320bc948 | 277 | dt = dtime / 1000000; // Convert from microseconds to seconds |
| Lauszus | 7:f1d24c6119ac | 278 | |
| Lauszus | 4:0b4c320bc948 | 279 | // Discrete Kalman filter time update equations - Time Update ("Predict") |
| Lauszus | 4:0b4c320bc948 | 280 | // Update xhat - Project the state ahead |
| Lauszus | 4:0b4c320bc948 | 281 | angle += dt * (newRate - bias); |
| Lauszus | 7:f1d24c6119ac | 282 | |
| Lauszus | 4:0b4c320bc948 | 283 | // Update estimation error covariance - Project the error covariance ahead |
| Lauszus | 4:0b4c320bc948 | 284 | P_00 += -dt * (P_10 + P_01) + Q_angle * dt; |
| Lauszus | 4:0b4c320bc948 | 285 | P_01 += -dt * P_11; |
| Lauszus | 4:0b4c320bc948 | 286 | P_10 += -dt * P_11; |
| Lauszus | 4:0b4c320bc948 | 287 | P_11 += +Q_gyro * dt; |
| Lauszus | 7:f1d24c6119ac | 288 | |
| Lauszus | 4:0b4c320bc948 | 289 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") |
| Lauszus | 4:0b4c320bc948 | 290 | // Calculate Kalman gain - Compute the Kalman gain |
| Lauszus | 4:0b4c320bc948 | 291 | S = P_00 + R_angle; |
| Lauszus | 4:0b4c320bc948 | 292 | K_0 = P_00 / S; |
| Lauszus | 4:0b4c320bc948 | 293 | K_1 = P_10 / S; |
| Lauszus | 7:f1d24c6119ac | 294 | |
| Lauszus | 4:0b4c320bc948 | 295 | // Calculate angle and resting rate - Update estimate with measurement zk |
| Lauszus | 4:0b4c320bc948 | 296 | y = newAngle - angle; |
| Lauszus | 4:0b4c320bc948 | 297 | angle += K_0 * y; |
| Lauszus | 4:0b4c320bc948 | 298 | bias += K_1 * y; |
| Lauszus | 7:f1d24c6119ac | 299 | |
| Lauszus | 4:0b4c320bc948 | 300 | // Calculate estimation error covariance - Update the error covariance |
| Lauszus | 4:0b4c320bc948 | 301 | P_00 -= K_0 * P_00; |
| Lauszus | 4:0b4c320bc948 | 302 | P_01 -= K_0 * P_01; |
| Lauszus | 4:0b4c320bc948 | 303 | P_10 -= K_1 * P_00; |
| Lauszus | 4:0b4c320bc948 | 304 | P_11 -= K_1 * P_01; |
| Lauszus | 7:f1d24c6119ac | 305 | |
| Lauszus | 4:0b4c320bc948 | 306 | return angle; |
| Lauszus | 4:0b4c320bc948 | 307 | } |
| Lauszus | 4:0b4c320bc948 | 308 | double getGyroYrate() { |
| Lauszus | 4:0b4c320bc948 | 309 | // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3=0.001009091 |
| Lauszus | 4:0b4c320bc948 | 310 | double gyroRate = -((gyroY.read() - zeroValues[0]) / 0.001009091); |
| Lauszus | 4:0b4c320bc948 | 311 | return gyroRate; |
| Lauszus | 4:0b4c320bc948 | 312 | } |
| Lauszus | 4:0b4c320bc948 | 313 | double getAccY() { |
| Lauszus | 4:0b4c320bc948 | 314 | // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3=0.1 |
| Lauszus | 4:0b4c320bc948 | 315 | double accXval = (accX.read() - zeroValues[1]) / 0.1; |
| Lauszus | 4:0b4c320bc948 | 316 | double accYval = (accY.read() - zeroValues[2]) / 0.1; |
| Lauszus | 4:0b4c320bc948 | 317 | accYval--;//-1g when lying down |
| Lauszus | 4:0b4c320bc948 | 318 | double accZval = (accZ.read() - zeroValues[3]) / 0.1; |
| Lauszus | 7:f1d24c6119ac | 319 | |
| Lauszus | 7:f1d24c6119ac | 320 | double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the length of force vector |
| Lauszus | 4:0b4c320bc948 | 321 | double angleY = acos(accYval / R) * RAD_TO_DEG; |
| Lauszus | 7:f1d24c6119ac | 322 | |
| Lauszus | 4:0b4c320bc948 | 323 | return angleY; |
| Lauszus | 4:0b4c320bc948 | 324 | } |
| Lauszus | 4:0b4c320bc948 | 325 | void calibrateSensors() { |
| Lauszus | 4:0b4c320bc948 | 326 | LEDs = 0xF; // Turn all onboard LEDs on |
| Lauszus | 7:f1d24c6119ac | 327 | |
| Lauszus | 4:0b4c320bc948 | 328 | double adc[4] = {0,0,0,0}; |
| Lauszus | 4:0b4c320bc948 | 329 | for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings |
| Lauszus | 4:0b4c320bc948 | 330 | adc[0] += gyroY.read(); |
| Lauszus | 4:0b4c320bc948 | 331 | adc[1] += accX.read(); |
| Lauszus | 4:0b4c320bc948 | 332 | adc[2] += accY.read(); |
| Lauszus | 4:0b4c320bc948 | 333 | adc[3] += accZ.read(); |
| Lauszus | 4:0b4c320bc948 | 334 | wait_ms(10); |
| Lauszus | 4:0b4c320bc948 | 335 | } |
| Lauszus | 4:0b4c320bc948 | 336 | zeroValues[0] = adc[0] / 100; // Gyro X-axis |
| Lauszus | 4:0b4c320bc948 | 337 | zeroValues[1] = adc[1] / 100; // Accelerometer X-axis |
| Lauszus | 4:0b4c320bc948 | 338 | zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis |
| Lauszus | 4:0b4c320bc948 | 339 | zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis |
| Lauszus | 7:f1d24c6119ac | 340 | |
| Lauszus | 4:0b4c320bc948 | 341 | LEDs = 0x0; // Turn all onboard LEDs off |
| Lauszus | 4:0b4c320bc948 | 342 | } |
| Lauszus | 4:0b4c320bc948 | 343 | void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f) |
| Lauszus | 4:0b4c320bc948 | 344 | if (motor == left) { |
| Lauszus | 4:0b4c320bc948 | 345 | leftPWM = speed; |
| Lauszus | 4:0b4c320bc948 | 346 | if (direction == forward) { |
| Lauszus | 4:0b4c320bc948 | 347 | leftA = 0; |
| Lauszus | 4:0b4c320bc948 | 348 | leftB = 1; |
| Lauszus | 4:0b4c320bc948 | 349 | } else if (direction == backward) { |
| Lauszus | 4:0b4c320bc948 | 350 | leftA = 1; |
| Lauszus | 4:0b4c320bc948 | 351 | leftB = 0; |
| Lauszus | 4:0b4c320bc948 | 352 | } |
| Lauszus | 4:0b4c320bc948 | 353 | } else if (motor == right) { |
| Lauszus | 4:0b4c320bc948 | 354 | rightPWM = speed; |
| Lauszus | 4:0b4c320bc948 | 355 | if (direction == forward) { |
| Lauszus | 4:0b4c320bc948 | 356 | rightA = 0; |
| Lauszus | 4:0b4c320bc948 | 357 | rightB = 1; |
| Lauszus | 4:0b4c320bc948 | 358 | } else if (direction == backward) { |
| Lauszus | 4:0b4c320bc948 | 359 | rightA = 1; |
| Lauszus | 4:0b4c320bc948 | 360 | rightB = 0; |
| Lauszus | 4:0b4c320bc948 | 361 | } |
| Lauszus | 4:0b4c320bc948 | 362 | } else if (motor == both) { |
| Lauszus | 4:0b4c320bc948 | 363 | leftPWM = speed; |
| Lauszus | 4:0b4c320bc948 | 364 | rightPWM = speed; |
| Lauszus | 4:0b4c320bc948 | 365 | if (direction == forward) { |
| Lauszus | 4:0b4c320bc948 | 366 | leftA = 0; |
| Lauszus | 4:0b4c320bc948 | 367 | leftB = 1; |
| Lauszus | 7:f1d24c6119ac | 368 | |
| Lauszus | 4:0b4c320bc948 | 369 | rightA = 0; |
| Lauszus | 4:0b4c320bc948 | 370 | rightB = 1; |
| Lauszus | 4:0b4c320bc948 | 371 | } else if (direction == backward) { |
| Lauszus | 4:0b4c320bc948 | 372 | leftA = 1; |
| Lauszus | 4:0b4c320bc948 | 373 | leftB = 0; |
| Lauszus | 7:f1d24c6119ac | 374 | |
| Lauszus | 4:0b4c320bc948 | 375 | rightA = 1; |
| Lauszus | 4:0b4c320bc948 | 376 | rightB = 0; |
| Lauszus | 4:0b4c320bc948 | 377 | } |
| Lauszus | 4:0b4c320bc948 | 378 | } |
| Lauszus | 4:0b4c320bc948 | 379 | } |
| Lauszus | 4:0b4c320bc948 | 380 | void stop(Motor motor) { |
| Lauszus | 4:0b4c320bc948 | 381 | if (motor == left) { |
| Lauszus | 4:0b4c320bc948 | 382 | leftPWM = 1; |
| Lauszus | 4:0b4c320bc948 | 383 | leftA = 1; |
| Lauszus | 4:0b4c320bc948 | 384 | leftB = 1; |
| Lauszus | 4:0b4c320bc948 | 385 | } else if (motor == right) { |
| Lauszus | 4:0b4c320bc948 | 386 | rightPWM = 1; |
| Lauszus | 4:0b4c320bc948 | 387 | rightA = 1; |
| Lauszus | 4:0b4c320bc948 | 388 | rightB = 1; |
| Lauszus | 4:0b4c320bc948 | 389 | } else if (motor == both) { |
| Lauszus | 4:0b4c320bc948 | 390 | leftPWM = 1; |
| Lauszus | 4:0b4c320bc948 | 391 | leftA = 1; |
| Lauszus | 4:0b4c320bc948 | 392 | leftB = 1; |
| Lauszus | 7:f1d24c6119ac | 393 | |
| Lauszus | 4:0b4c320bc948 | 394 | rightPWM = 1; |
| Lauszus | 4:0b4c320bc948 | 395 | rightA = 1; |
| Lauszus | 4:0b4c320bc948 | 396 | rightB = 1; |
| Lauszus | 4:0b4c320bc948 | 397 | } |
| Lauszus | 0:f5c02b620688 | 398 | } |