PID Balancing Robot
The goal of this mini-project was to use the mbed to implement a self balancing robot. The robot has two wheels on which it balances in an 'inverted pendulum' scheme. The two DC motors are controlled by a 6612FNG H-bridge. The robot uses data from a LSM9DS0 IMU (taking into account inertial and gyroscopic data) and a PID loop to make automatic corrections.. The firmware uses a timer based interrupt to get a consistent sampling rate for the PID algorithm. Below, the MPU and H-bridge circuits are shown:
The IMU was mounted on the chassis of the robot as close to the top as possible to best sense the angular deflection:
The lab goal was to get the robot to a point where it was beginning to balance itself. Here is a video showing the robot in action: /media/uploads/omidece2035/pidrobotedit.mp4
The PID loop is as follows:
PID
Error = setpoint - current_angle; //determine the difference in angle integral = integral + (Error*dt); //Forward Euler approximation derivative = (Error - previous_error) / dt; //change wrt time previous_error = Error; actuator_input = ( (Error*kp) + (integral*ki) + (derivative*kd) ) / SCALAR; MotorDriver(actuator_input);
The code first takes the reading from the IMU and saves it as a setpoint for the PID to achieve. A timer interrupt reads the IMU and adjusts the robot through kp, kd, and ki gains as seen above.
The pin connections are as follows:
H-Bridge
- A01 -> Motor A positive lead
- A02 -> Motor A negative lead
- B01 -> Motor B positive lead
- B02 -> Motor B negative lead
- VMOT -> External 5V power supply
- PWMA -> p21 on MBED
- AIN2 -> p23
- AIN1 -> p23
- nSTBY -> VCC
- BIN1 -> p26
- BIN2 -> p25
- PWMB -> p22
Import programbalancebot
Inverted pendulum style balancing robot
Please log in to post comments.