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