Prototype program for balancing robot based on various examples from other users.
Dependencies: HCSR04 MPU6050_1 Motor Servo ledControl2 mbed
Diff: main.cpp
- Revision:
- 0:cfae0986065f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Aug 26 07:11:47 2016 +0000 @@ -0,0 +1,210 @@ +#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(); + } + } + \ No newline at end of file