First trial

Dependencies:   MPU6050 Motor ledControl2 mbed

Fork of BalancingRobotPS3 by Kristian Lauszus

Committer:
Lauszus
Date:
Mon Apr 09 21:40:47 2012 +0000
Revision:
7:f1d24c6119ac
Parent:
6:defe36ce2346
Child:
9:67f2110fce8e

        

Who changed what in which revision?

UserRevisionLine numberNew 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 }