BalancingRobot.h

00001 #ifndef _balancingrobot_h_
00002 #define _balancingrobot_h_
00003 
00004 #define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi
00005 
00006 BusOut LEDs(LED1, LED2, LED3, LED4);
00007 
00008 /* Left motor */
00009 DigitalOut leftA(D6);
00010 DigitalOut leftB(D5);
00011 PwmOut leftPWM(D7);
00012 
00013 /* Right motor */
00014 DigitalOut rightA(D4);
00015 DigitalOut rightB(D3);
00016 PwmOut rightPWM(D2);
00017 
00018 // Results
00019 double accYangle;
00020 double gyroYrate;
00021 int16_t raw_accYangle;
00022 int16_t raw_gyroYrate;
00023 
00024 /* PID variables */
00025 
00026 double Kp = 0.085; //0.100; //0.0400; // 11 - 7
00027 double Ki = 0.00900; //0.005;//0.00320; //1 0.001 sanki gideri var mı ne
00028 double Kd = 0.050;//0.0125 baya iyi //0.0333; //12
00029 
00030 //double Kp = 0.185; //11 - 7 almost well   //0.1
00031 //double Ki = 0.0000; //1                   //0.005  integralli ilk iyi gibi
00032 //double Kd = 0.1; //12                     //0.01
00033 
00034 double PIDValue = 0.0;
00035 double DesiredAngle;
00036 double ConstantAngle;
00037 unsigned int distF;
00038 unsigned int distB;
00039 double targetOffset = 0;
00040 float pitchAngle = 0;
00041 float rollAngle = 0;
00042 double error1 = 0.0;
00043 double lastError = 0.0;
00044 double iTerm;
00045 
00046 /* Used for timing */
00047 unsigned long timer;
00048 
00049 const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds
00050 long lastLoopTime = STD_LOOP_TIME;
00051 long lastLoopUsefulTime = STD_LOOP_TIME;
00052 long loopStartTime;
00053 
00054 enum Motor {
00055     left,
00056     right,
00057     both,
00058 };
00059 enum Direction {
00060     forward,
00061     backward,
00062 };
00063 
00064 bool steerForward;
00065 bool steerBackward;
00066 bool steerStop = true; // Stop by default
00067 bool steerLeft;
00068 bool steerRotateLeft;
00069 bool steerRight;
00070 bool steerRotateRight;
00071 
00072 bool stopped;
00073 
00074 const double turnSpeed = 0.1;
00075 const double rotateSpeed = 0.2;
00076 
00077 uint8_t loopCounter = 0; // Used for wheel velocity and battery voltage
00078 long wheelPosition;
00079 long lastWheelPosition;
00080 long wheelVelocity;
00081 long targetPosition;
00082 int zoneA = 4000; // 2000
00083 int zoneB = 2000; // 1000
00084 double positionScaleA = 250; // one resolution is 464 pulses
00085 double positionScaleB = 500; 
00086 double positionScaleC = 1000;
00087 double velocityScaleMove = 1; //40;
00088 double velocityScaleStop = 1; //30 - 40 - 60
00089 
00090 void PID(double restAngle, double offset);
00091 double getGyroYrate();
00092 double getAccY();
00093 void move(Motor motor, Direction direction, float speed);
00094 void stop(Motor motor);
00095 void receiveBluetooth();
00096 void stopAndReset();
00097 void toggle_led1();
00098 void toggle_led2();
00099 
00100 #endif