File content as of revision 11:0a2944e7be23:

#ifndef _balancingrobot_h_
#define _balancingrobot_h_

#define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi

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

/* Left motor */
DigitalOut leftA(D6);
DigitalOut leftB(D5);
PwmOut leftPWM(D7);

/* Right motor */
DigitalOut rightA(D4);
DigitalOut rightB(D3);
PwmOut rightPWM(D2);

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

/* PID variables */

double Kp = 0.085; //0.100; //0.0400; // 11 - 7
double Ki = 0.00900; //0.005;//0.00320; //1 0.001 sanki gideri var mı ne
double Kd = 0.050;//0.0125 baya iyi //0.0333; //12

//double Kp = 0.185; //11 - 7 almost well   //0.1
//double Ki = 0.0000; //1                   //0.005  integralli ilk iyi gibi
//double Kd = 0.1; //12                     //0.01

double PIDValue = 0.0;
double DesiredAngle;
double ConstantAngle;
unsigned int distF;
unsigned int distB;
double targetOffset = 0;
float pitchAngle = 0;
float rollAngle = 0;
double error1 = 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