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