First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
Diff: BalancingRobot.cpp
- Revision:
- 7:f1d24c6119ac
- Parent:
- 6:defe36ce2346
- Child:
- 9:67f2110fce8e
--- a/BalancingRobot.cpp Tue Mar 06 22:38:41 2012 +0000 +++ b/BalancingRobot.cpp Mon Apr 09 21:40:47 2012 +0000 @@ -28,39 +28,39 @@ xbee.baud(115200); ps3.baud(115200); debug.baud(115200); - + /* Set PWM frequency */ leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005) rightPWM.period(0.00005); - + /* Calibrate the gyro and accelerometer relative to ground */ calibrateSensors(); - + xbee.printf("Initialized\n"); processing(); // Send output to processing application - + /* Setup timing */ t.start(); loopStartTime = t.read_us(); timer = loopStartTime; - + while (1) { /* Calculate pitch */ accYangle = getAccY(); gyroYrate = getGyroYrate(); - + // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter timer = t.read_us(); - + //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle); - + /* Drive motors */ if (pitch < 60 || pitch > 120) // Stop if falling or laying down stopAndReset(); else PID(targetAngle,targetOffset); - + /* Update wheel velocity every 100ms */ loopCounter++; if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc. @@ -69,20 +69,20 @@ wheelVelocity = wheelPosition - lastWheelPosition; lastWheelPosition = wheelPosition; //xbee.printf("WheelVelocity: %i\n",wheelVelocity); - + if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking targetPosition = wheelPosition; stopped = true; //xbee.printf("Stopped\n"); } } - + /* Check for incoming serial data */ if (ps3.readable()) // Check if there's any incoming data receivePS3(); if (xbee.readable()) // For setting the PID values receiveXbee(); - + /* Read battery voltage every 1s */ if (loopCounter == 100) { loopCounter = 0; // Reset loop counter @@ -90,16 +90,16 @@ analogVoltage *= 6.6; // The analog pin is connected to a 56k-10k voltage divider xbee.printf("analogVoltage: %f - timer: %i\n",analogVoltage,t.read_ms()); if (analogVoltage < 9 && pitch > 60 && pitch < 120) // Set buzzer on, if voltage gets critical low - buzzer = 1; // The mbed resets at aproximatly 1V + buzzer = 1; // The mbed resets at aproximatly 8.5V } - + /* Use a time fixed loop */ lastLoopUsefulTime = t.read_us() - loopStartTime; if (lastLoopUsefulTime < STD_LOOP_TIME) wait_us(STD_LOOP_TIME - lastLoopUsefulTime); lastLoopTime = t.read_us() - loopStartTime; // only used for debugging loopStartTime = t.read_us(); - + //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime); } } @@ -123,9 +123,9 @@ restAngle -= (double)positionError/positionScaleB; else // Inside zone C restAngle -= (double)positionError/positionScaleC; - + restAngle -= (double)wheelVelocity/velocityScaleStop; - + if (restAngle < 80) // Limit rest Angle restAngle = 80; else if (restAngle > 100) @@ -138,11 +138,11 @@ iTerm += Ki * error; double dTerm = Kd * (error - lastError); lastError = error; - + double PIDValue = pTerm + iTerm + dTerm; - + //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); - + /* Steer robot sideways */ double PIDLeft; double PIDRight; @@ -163,7 +163,7 @@ PIDRight = PIDValue; } PIDLeft *= 0.95; // compensate for difference in the motors - + /* Set PWM Values */ if (PIDLeft >= 0) move(left, forward, PIDLeft); @@ -184,7 +184,7 @@ i++; } //debug.printf("Input: %s\n",input); - + // Set all false steerForward = false; steerBackward = false; @@ -193,7 +193,7 @@ steerRotateLeft = false; steerRight = false; steerRotateRight = false; - + /* For remote control */ if (input[0] == 'F') { // Forward strtok(input, ","); // Ignore 'F' @@ -220,7 +220,7 @@ stopped = false; targetPosition = wheelPosition; } - + else if (input[0] == 'T') { // Set the target angle strtok(input, ","); // Ignore 'T' targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double @@ -240,7 +240,7 @@ i++; } //debug.printf("Input: %s\n",input); - + if (input[0] == 'T') { // Set the target angle strtok(input, ","); // Ignore 'T' targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double @@ -268,41 +268,41 @@ lastError = 0; iTerm = 0; targetPosition = wheelPosition; - buzzer= 0; + buzzer = 0; } double kalman(double newAngle, double newRate, double dtime) { // KasBot V2 - Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145 // with slightly modifications by Kristian Lauszus // 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 dt = dtime / 1000000; // Convert from microseconds to seconds - + // Discrete Kalman filter time update equations - Time Update ("Predict") // Update xhat - Project the state ahead angle += dt * (newRate - bias); - + // Update estimation error covariance - Project the error covariance ahead P_00 += -dt * (P_10 + P_01) + Q_angle * dt; P_01 += -dt * P_11; P_10 += -dt * P_11; P_11 += +Q_gyro * dt; - + // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") // Calculate Kalman gain - Compute the Kalman gain S = P_00 + R_angle; K_0 = P_00 / S; K_1 = P_10 / S; - + // Calculate angle and resting rate - Update estimate with measurement zk y = newAngle - angle; angle += K_0 * y; bias += K_1 * y; - + // Calculate estimation error covariance - Update the error covariance P_00 -= K_0 * P_00; P_01 -= K_0 * P_01; P_10 -= K_1 * P_00; P_11 -= K_1 * P_01; - + return angle; } double getGyroYrate() { @@ -316,15 +316,15 @@ double accYval = (accY.read() - zeroValues[2]) / 0.1; accYval--;//-1g when lying down double accZval = (accZ.read() - zeroValues[3]) / 0.1; - - double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the force vector + + double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the length of force vector double angleY = acos(accYval / R) * RAD_TO_DEG; - + return angleY; } void calibrateSensors() { LEDs = 0xF; // Turn all onboard LEDs on - + double adc[4] = {0,0,0,0}; for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings adc[0] += gyroY.read(); @@ -337,7 +337,7 @@ zeroValues[1] = adc[1] / 100; // Accelerometer X-axis zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis - + LEDs = 0x0; // Turn all onboard LEDs off } void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f) @@ -365,13 +365,13 @@ if (direction == forward) { leftA = 0; leftB = 1; - + rightA = 0; rightB = 1; } else if (direction == backward) { leftA = 1; leftB = 0; - + rightA = 1; rightB = 0; } @@ -390,7 +390,7 @@ leftPWM = 1; leftA = 1; leftB = 1; - + rightPWM = 1; rightA = 1; rightB = 1;