Prototype program for balancing robot based on various examples from other users.
Dependencies: HCSR04 MPU6050_1 Motor Servo ledControl2 mbed
main.cpp
- Committer:
- lakshmananag
- Date:
- 2016-08-26
- Revision:
- 0:cfae0986065f
File content as of revision 0:cfae0986065f:
#include "mbed.h" #include "hcsr04.h" #include "MPU6050.h" #include "ledControl.h" #include "Motor.h" #include "HALLFX_ENCODER.h" Serial pc(USBTX, USBRX); // Create terminal link Serial blue(p28, p27); // Bluetooth TX, RX MPU6050 mpu6050; // mpu6050 object from MPU6050 classs Ticker toggler1; // Ticker for led toggling Ticker filter; // Ticker for periodic call to compFilter funcçs Ticker balance; // Periodic routine for PID control of balance system Ticker speed; // Periodic routine for speed control Ticker bluetooth; // Ticker for navigation Motor A(p23, p5, p6); // pwm, fwd, rev Motor B(p24, p7, p8); // pwm, fwd, rev /* Encoders*/ HALLFX_ENCODER leftEncoder(p13); HALLFX_ENCODER rightEncoder(p14); HCSR04 usensor(p25,p26); //trig pin,echo pin /* Function prototypes */ void toggle_led1(); void toggle_led2(); void compFilter(); void balancePID(); void balanceControl(); void Forward(float); void Reverse(float); void Stop(void); void Navigate(); void TurnRight(float); void TurnLeft(float); /* Variable declarations */ float pitchAngle = 0; float rollAngle = 0; bool dir; // direction of movement float Kp = 0.5; float Ki = 0.00001; float Kd = 0.01;//0.01;//0.05; float set_point = 1.005; // Angle for staying upright float errorPID = 0; // Proportional term (P) in PID float last_errorPID =0; // Previous error value float integral = 0; // Integral (I) float derivative = 0; // Derivative (D) float outputPID = 0; // Output of PID calculations float position = 0.0; float dist=0.0; float range = 0.000; int phone_char; DigitalOut myled(LED4); int main() { pc.baud(9600); // baud rate: 9600 blue.baud(9600); mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading wait(0.5); mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers pc.printf("Calibration is completed. \r\n"); mpu6050.init(); // Initialize the sensor pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) balance.attach(&balancePID, 0.010); // Same period with balancePID speed.attach(&balanceControl, 0.01); bluetooth.attach(&Navigate, 0.05); while(1) { if(blue.readable()) { phone_char = blue.getc(); pc.putc(phone_char); pc.printf("Bluetooth Start\r\n"); myled = !myled; } pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f\r\n",rollAngle,pitchAngle); pc.printf("Error = %f\r\n",outputPID); wait_ms(40); } } void toggle_led1() {ledToggle(1);} void toggle_led2() {ledToggle(2);} /* This function is calls the complementary filter with pitch and roll angles */ void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);} void balancePID() { errorPID = set_point - pitchAngle; //Proportional (P) integral += errorPID; //Integral (I) derivative = errorPID - last_errorPID; //Derivative (D) last_errorPID = errorPID; //Save current value for next iteration outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative); //PID calculation /* errorPID is restricted between -1 and 1 */ if(outputPID > 0.8) outputPID = 0.8; else if(outputPID < -0.8) outputPID = -0.8; } void balanceControl() { int direction=0; // Variable to hold direction we want to drive if (outputPID>0)direction=1; // Positive speed indicates forward if (outputPID<0)direction=2; // Negative speed indicates backwards if((abs(pitchAngle)>10.00))direction=0; switch( direction) { // Depending on what direction was passed case 0: // Stop case Stop(); break; case 1: // Forward case Forward(abs(outputPID)); break; case 2: // Backwards Reverse(abs(outputPID)); break; default: // Catch-all (Stop) Stop(); break; } } void Stop(void) { A.speed(0.0); B.speed(0.0); } void Forward(float fwd_dist) { A.speed(-fwd_dist); B.speed(-fwd_dist); } void TurnRight(float ryt_dist) { A.speed(ryt_dist); B.speed(-ryt_dist); } void TurnLeft(float lft_dist) { A.speed(-lft_dist); B.speed(lft_dist); } void Reverse(float rev_dist) { A.speed(rev_dist); B.speed(rev_dist); } void Navigate() { switch (phone_char) { case 'f': Forward(0.7); break; case 'F': Forward(0.7); break; case 'b': Reverse(0.7); break; case 'B': Reverse(0.7); break; case 'r': TurnRight(0.7); break; case 'R': TurnRight(0.7); break; case 'l': TurnLeft(0.7); break; case 'L': TurnLeft(0.7); break; default: Stop(); } }