File content as of revision 10:8050817ae610:

#ifndef _balancingrobot_h_
#define _balancingrobot_h_

#define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi

BusOut LEDs(LED1, LED2, LED3, LED4);

/* Left motor */
DigitalOut leftA(p5);
DigitalOut leftB(p6);
PwmOut leftPWM(p23);

/* Right motor */
DigitalOut rightA(p7);
DigitalOut rightB(p8);
PwmOut rightPWM(p24);

/* Encoder variables */
float LeftWheelPosition = 0.0;
float RightWheelPosition = 0.0;

// Results
double accYangle;
double gyroYrate;
int16_t raw_accYangle;
int16_t raw_gyroYrate;

/* PID variables */
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 */
unsigned long timer;

const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds
long lastLoopTime = STD_LOOP_TIME;
long lastLoopUsefulTime = STD_LOOP_TIME;
long loopStartTime;

enum Motor {
    left,
    right,
    both,
};
enum Direction {
    forward,
    backward,
};

bool steerForward;
bool steerBackward;
bool steerStop = true; // Stop by default
bool steerLeft;
bool steerRotateLeft;
bool steerRight;
bool steerRotateRight;

bool stopped;

const double turnSpeed = 0.1;
const double rotateSpeed = 0.2;

uint8_t loopCounter = 0; // Used for wheel velocity and battery voltage
long wheelPosition;
long lastWheelPosition;
long wheelVelocity;
long targetPosition;
int zoneA = 4000; // 2000
int zoneB = 2000; // 1000
double positionScaleA = 250; // one resolution is 464 pulses
double positionScaleB = 500; 
double positionScaleC = 1000;
double velocityScaleMove = 1; //40;
double velocityScaleStop = 1; //30 - 40 - 60

void PID(double restAngle, double offset);
double getGyroYrate();
double getAccY();
void move(Motor motor, Direction direction, float speed);
void stop(Motor motor);
void receiveBluetooth();
void stopAndReset();
void toggle_led1();
void toggle_led2();

#endif