Committed for sharing.
Dependencies: mbed SelfBalancingRobot_BerkYasarYavuz_AdilBerkayTemiz Motor ledControl2 HC_SR04_Ultrasonic_Library
BalancingRobot.cpp
- Committer:
- Lauszus
- Date:
- 2012-02-14
- Revision:
- 0:f5c02b620688
- Child:
- 1:01295228342f
File content as of revision 0:f5c02b620688:
#include "BalancingRobot.h" /* Serial communication */ Serial xbee(p13,p14); // For wireless debugging Serial ps3(p9,p10); // For remote control Serial debug(USBTX, USBRX); // USB /* Timer instance */ Timer t; int main() { xbee.baud(115200); ps3.baud(115200); debug.baud(115200); leftPWM.period(0.00005); //The motor driver can handle pwm values up to 20kHz (1/20000=0.00005) rightPWM.period(0.00005); calibrateSensors(); // Calibrate the gyro and accelerometer relative to ground xbee.printf("Initialized\n"); // Start timing t.start(); loopStartTime = t.read_us(); timer = loopStartTime; while (1) { // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html accYangle = getAccY(); gyroYrate = getGyroYrate(); pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter timer = t.read_us(); if (ps3.readable()) // Check if there's any incoming data receiveSerial(); //debug.printf("Pitch: %f, accYangle: %f\n",pitch,accYangle); if (pitch < 75 || pitch > 105) //Stop if falling or laying down stopAndReset(); else PID(targetAngle); /* Used 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); } } void stopAndReset() { stop(both); lastError = 0; iTerm = 0; } void receiveSerial() { char input[16]; // The serial buffer is only 16 characters, so the data has to be split up int i = 0; while (ps3.readable()) { input[i] = ps3.getc(); i++; } LPC_UART0->FCR |= 0x06; // Flush Serial //debug.printf("Input: %s\n",input); /* For remote control */ if (input[0] == 'F') { // Forward steerForward = true; steerBackward = false; steerLeft = false; steerRotateLeft = false; steerRight = false; steerRotateRight = false; } else if (input[0] == 'B') { // Backward steerForward = false; steerBackward = true; steerLeft = false; steerRotateLeft = false; steerRight = false; steerRotateRight = false; } else if (input[0] == 'L') { // Left if (input[1] == 'R') { // Left Rotate steerLeft = false; steerRotateLeft = true; } else { steerLeft = true; steerRotateLeft = false; } steerForward = false; steerBackward = false; steerRight = false; steerRotateRight = false; } else if (input[0] == 'R') { // Right if (input[1] == 'R') { // Right Rotate steerRight = false; steerRotateRight = true; } else { steerRight = true; steerRotateRight = false; } steerForward = false; steerBackward = false; steerLeft = false; steerRotateLeft = false; } else if (input[0] == 'S') { // Stop steerForward = false; steerBackward = false; steerLeft = false; steerRotateLeft = false; steerRight = false; steerRotateRight = false; } 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 if (targetAngle < 75 || targetAngle > 105) // The serial communication sometimes behaves weird targetAngle = 90; xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging } else if (input[0] == 'A') { // Abort stopAndReset(); while (ps3.getc() != 'C'); // Wait until continue is send } } void PID(double restAngle) { if (steerForward) restAngle -= 1; else if (steerBackward) restAngle += 1; double error = (restAngle - pitch)/100; double pTerm = Kp * error; 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); double PIDLeft; double PIDRight; if (steerLeft) { PIDLeft = PIDValue-(0.1); PIDRight = PIDValue+(0.1); } else if (steerRotateLeft) { PIDLeft = PIDValue-(0.2); PIDRight = PIDValue+(0.2); } else if (steerRight) { PIDLeft = PIDValue-(-0.1); PIDRight = PIDValue+(-0.1); } else if (steerRotateRight) { PIDLeft = PIDValue-(-0.2); PIDRight = PIDValue+(-0.2); } else { PIDLeft = PIDValue; PIDRight = PIDValue; } PIDLeft *= 0.9; // compensate for difference in the motors //Set PWM Values if (PIDLeft >= 0) move(left, forward, PIDLeft); else move(left, backward, PIDLeft * -1); if (PIDRight >= 0) move(right, forward, PIDRight); else move(right, backward, PIDRight * -1); } 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 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() { // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3*4095=4.132227273 double gyroRate = -(((gyroY.read()*4095) - zeroValues[0]) / 4.132227273); return gyroRate; } double getAccY() { // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3*4095=409.5 double accXval = ((accX.read()*4095) - zeroValues[1]) / 409.5; double accYval = ((accY.read()*4095) - zeroValues[2]) / 409.5; accYval--;//-1g when lying down double accZval = ((accZ.read()*4095) - zeroValues[3]) / 409.5; double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the force vector double angleY = acos(accYval / R) * RAD_TO_DEG; return angleY; } void calibrateSensors() { onboardLED1 = 1; onboardLED2 = 1; onboardLED3 = 1; onboardLED4 = 1; double adc[4]; for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings adc[0] += gyroY.read()*4095; adc[1] += accX.read()*4095; adc[2] += accY.read()*4095; adc[3] += accZ.read()*4095; wait_ms(10); } zeroValues[0] = adc[0] / 100; // Gyro X-axis zeroValues[1] = adc[1] / 100; // Accelerometer X-axis zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis onboardLED1 = 0; onboardLED2 = 0; onboardLED3 = 0; onboardLED4 = 0; } void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f) if (motor == right) { leftPWM = speed; if (direction == forward) { leftA = 0; leftB = 1; } else if (direction == backward) { leftA = 1; leftB = 0; } } else if (motor == left) { rightPWM = speed; if (direction == forward) { rightA = 0; rightB = 1; } else if (direction == backward) { rightA = 1; rightB = 0; } } else if (motor == both) { leftPWM = speed; rightPWM = speed; if (direction == forward) { leftA = 0; leftB = 1; rightA = 0; rightB = 1; } else if (direction == backward) { leftA = 1; leftB = 0; rightA = 1; rightB = 0; } } } void stop(Motor motor) { if (motor == left) { leftPWM = 1; leftA = 1; leftB = 1; } else if (motor == right) { rightPWM = 1; rightA = 1; rightB = 1; } else if (motor == both) { leftPWM = 1; leftA = 1; leftB = 1; rightPWM = 1; rightA = 1; rightB = 1; } }