Jakob Højgård Olsen / Mbed 2 deprecated BalancingRobot

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.

Dependencies:   mbed

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?

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