![](/media/cache/profiles/7d5430cf85f78c4b7aa09813b14bce0d.50x50_q85.jpg)
First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
Diff: BalancingRobot.h
- Revision:
- 9:67f2110fce8e
- Parent:
- 8:87dd9d5f7001
--- a/BalancingRobot.h Fri Apr 13 19:45:25 2012 +0000 +++ b/BalancingRobot.h Thu Aug 25 22:50:49 2016 +0000 @@ -6,51 +6,36 @@ BusOut LEDs(LED1, LED2, LED3, LED4); /* Left motor */ -DigitalOut leftA(p21); -DigitalOut leftB(p22); +DigitalOut leftA(p5); +DigitalOut leftB(p6); PwmOut leftPWM(p23); /* Right motor */ -DigitalOut rightA(p24); -DigitalOut rightB(p25); -PwmOut rightPWM(p26); - -/* IMU */ -AnalogIn gyroY(p17); -AnalogIn accX(p18); -AnalogIn accY(p19); -AnalogIn accZ(p20); - -/* Battery voltage */ -AnalogIn batteryVoltage(p15); // The analog pin is connected to a 56k-10k voltage divider -DigitalOut buzzer(p5); // Connected to a BC547 transistor - there is a protection diode at the buzzer +DigitalOut rightA(p7); +DigitalOut rightB(p8); +PwmOut rightPWM(p24); -// Zero voltage values for the sensors - [0] = gyroY, [1] = accX, [2] = accY, [3] = accZ -double zeroValues[4]; - -/* Kalman filter variables and constants */ -const float Q_angle = 0.001; // Process noise covariance for the accelerometer - Sw -const float Q_gyro = 0.003; // Process noise covariance for the gyro - Sw -const float R_angle = 0.03; // Measurement noise covariance - Sv - -double angle = 180; // It starts at 180 degrees -double bias = 0; -double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; -double dt, y, S; -double K_0, K_1; +/* Encoder variables */ +float LeftWheelPosition = 0.0; +float RightWheelPosition = 0.0; // Results double accYangle; double gyroYrate; -double pitch; +int16_t raw_accYangle; +int16_t raw_gyroYrate; /* PID variables */ -double Kp = 8; //11 - 7 -double Ki = 2; //1 -double Kd = 9; //12 -double targetAngle = 90; - -double lastError; +double Kp = 0.5; //11 - 7 +double Ki = 0.00001; //1 +double Kd = 0.01; //12 +double PIDValue = 0.0; +double targetAngle = 1.05; +double targetOffset = 0; +float pitchAngle = 0; +float rollAngle = 0; +double error = 0.0; +double lastError = 0.0; double iTerm; /* Used for timing */ @@ -84,8 +69,6 @@ const double turnSpeed = 0.1; const double rotateSpeed = 0.2; -double targetOffset = 0; - uint8_t loopCounter = 0; // Used for wheel velocity and battery voltage long wheelPosition; long lastWheelPosition; @@ -96,19 +79,17 @@ double positionScaleA = 250; // one resolution is 464 pulses double positionScaleB = 500; double positionScaleC = 1000; -double velocityScaleMove = 40; -double velocityScaleStop = 30;//30 - 40 - 60 +double velocityScaleMove = 1; //40; +double velocityScaleStop = 1; //30 - 40 - 60 -void calibrateSensors(); void PID(double restAngle, double offset); -double kalman(double newAngle, double newRate, double looptime); double getGyroYrate(); double getAccY(); void move(Motor motor, Direction direction, float speed); void stop(Motor motor); -void processing(); -void receivePS3(); -void receiveXbee(); +void receiveBluetooth(); void stopAndReset(); +void toggle_led1(); +void toggle_led2(); #endif \ No newline at end of file