First trial
Dependencies: MPU6050 Motor ledControl2 mbed
Fork of BalancingRobotPS3 by
BalancingRobot.h
- Committer:
- lakshmananag
- Date:
- 2016-08-25
- Revision:
- 9:67f2110fce8e
- Parent:
- 8:87dd9d5f7001
File content as of revision 9:67f2110fce8e:
#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