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