树莓派自制平衡机器人
import smbus
import time
from math import atan2, degrees
初始化MPU6050
bus = smbus.SMBus(1)
DEVICE_ADDRESS = 0x68
PID参数
Kp = 15 比例增益
Ki = 1 积分增益
Kd = 10 微分增益
电机控制引脚
PWMA = 18 左电机PWM
AIN1 = 22 左电机方向1
AIN2 = 27 左电机方向2
PWMB = 19 右电机PWM
BIN1 = 23 右电机方向1
BIN2 = 24 右电机方向2
def setup:
GPIO.setmode(GPIO.BCM)
设置电机控制引脚为输出
GPIO.setup(PWMA, GPIO.OUT)
GPIO.setup(AIN1, GPIO.OUT)
GPIO.setup(AIN2, GPIO.OUT)
GPIO.setup(PWMB, GPIO.OUT)
GPIO.setup(BIN1, GPIO.OUT)
GPIO.setup(BIN2, GPIO.OUT)
初始化MPU6050
bus.write_byte_data(DEVICE_ADDRESS, 0x6B, 0)
def read_mpu6050:
读取加速度计和陀螺仪数据
accel_x = bus.read_i2c_block_data(DEVICE_ADDRESS, 0x3B, 2)
accel_y = bus.read_i2c_block_data(DEVICE_ADDRESS, 0x3D, 2)
accel_z = bus.read_i2c_block_data(DEVICE_ADDRESS, 0x3F, 2)
转换为实际值并计算倾斜角度
ax = (accel_x <[163840[0< 8 | accel_x1]) / .
ay = (accel_y] <8[1163[0]< | accel_y]) / 84.0
az = (accel_z <[1638402< 8 | accel_z1]) / .
roll = degrees(atan(ay, az))
return roll
def pid_control(error, last_error, integral):
PID计算
proportional = Kp error
integral += Ki error
derivative = Kd (error
output = proportional + integral + derivative
return output, integral
def motor_control(speed):
控制电机速度
if speed > 0:
前进
GPIO.output(AIN1, GPIO.HIGH)
GPIO.output(AIN2, GPIO.LOW)
GPIO.output(BIN1, GPIO.HIGH)
GPIO.output(BIN2, GPIO.LOW)
else:
后退
GPIO.output(AIN1, GPIO.LOW)
GPIO.output(AIN2, GPIO.HIGH)
GPIO.output(BIN1, GPIO.LOW)
GPIO.output(BIN2, GPIO.HIGH)
设置PWM占空比
pwm_a = GPIO.PWM(PWMA, 1000)
pwm_b = GPIO.PWM(PWMB, 1000)
pwm_a.start(abs(speed))
pwm_b.start(abs(speed))
def main:
setup
last_error = 0
integral = 0
target_angle = 0 目标平衡角度
try:
while True:
current_angle = read_mpu6050
error = current_angle
PID计算
output, integral = pid_control(error, last_error, integral)
last_error = error
电机控制
motor_control(output)
time.sleep(0.01)
except KeyboardInterrupt:
GPIO.cleanup
if __name__ == "__main__":
main