Kristian Lauszus
/
BalancingRobotPS3
Code for my balancing robot, controlled with a PS3 controller via bluetooth
Diff: BalancingRobot.h
- Revision:
- 3:c3963f37d597
- Parent:
- 2:caec5534774d
- Child:
- 4:0b4c320bc948
diff -r caec5534774d -r c3963f37d597 BalancingRobot.h --- a/BalancingRobot.h Thu Feb 16 20:25:52 2012 +0000 +++ b/BalancingRobot.h Sun Feb 26 13:11:10 2012 +0000 @@ -1,14 +1,9 @@ #ifndef _balancingrobot_h_ #define _balancingrobot_h_ -#include "mbed.h" - #define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi -DigitalOut onboardLED1(LED1); -DigitalOut onboardLED2(LED2); -DigitalOut onboardLED3(LED3); -DigitalOut onboardLED4(LED4); +BusOut LEDs(LED1, LED2, LED3, LED4); /* Left motor */ DigitalOut leftA(p21); @@ -77,6 +72,7 @@ bool steerForward; bool steerBackward; +bool steerStop = true; // Stop by default bool steerLeft; bool steerRotateLeft; bool steerRight; @@ -84,6 +80,14 @@ double targetOffset = 0; +uint8_t loopCounter = 0; // Used for wheel velocity +long wheelPosition; +long lastWheelPosition; +long targetPosition; +long wheelVelocity; +double positionScale = 1000; // one resolution is 464 pulses +double velocityScale = 40; + void calibrateSensors(); void PID(double restAngle, double offset); double kalman(double newAngle, double newRate, double looptime);