Kristian Lauszus
/
BalancingRobotPS3
Code for my balancing robot, controlled with a PS3 controller via bluetooth
Diff: BalancingRobot.cpp
- Revision:
- 0:f5c02b620688
- Child:
- 1:01295228342f
diff -r 000000000000 -r f5c02b620688 BalancingRobot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BalancingRobot.cpp Tue Feb 14 10:48:01 2012 +0000 @@ -0,0 +1,306 @@ +#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; + } +} \ No newline at end of file