Berk & Ay / Mbed 2 deprecated Self-Balancing-Robot-BerkYasarYavuz_AdilBerkayTemiz

Committed for sharing.

Dependencies:   mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library

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?

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