通过蓝牙控制自平衡小车前进后退·程序

这里提供一个基于Arduino控制器和HC-05蓝牙模块的自平衡小车控制程序示例，供参考：

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_LSM303_U.h>
#include <SoftwareSerial.h>

SoftwareSerial bluetooth(3, 4); // 初始化蓝牙模块，TX连接到pin 3，RX连接到pin 4

Adafruit_MotorShield AFMS = Adafruit_MotorShield(); // 初始化电机驱动板
Adafruit_DCMotor *motor1 = AFMS.getMotor(1); // 初始化左电机
Adafruit_DCMotor *motor2 = AFMS.getMotor(2); // 初始化右电机

// 初始化陀螺仪和加速度计
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);

// 陀螺仪校准值
float gyro_x_cal = 0;
float gyro_y_cal = 0;
float gyro_z_cal = 0;

// PID参数
const float kp = 5.0;
const float ki = 0.1;
const float kd = 0.1;

// 微调参数
const float gyro_gain = 0.07;
const float angle_gain = 0.3;

// 控制变量
float target_speed = 0;
float actual_speed = 0;
float error = 0;
float last_error = 0;
float integral = 0;
float derivative = 0;
float output = 0;
float angle = 0;

void setup() {
  AFMS.begin(); // 启动电机驱动板
  motor1->setSpeed(150); // 设置左电机速度
  motor2->setSpeed(150); // 设置右电机速度

  // 初始化陀螺仪
  if(!gyro.begin())
  {
    while(1);
  }
  gyro.enableAutoRange(true);

  // 初始化加速度计
  if(!accel.begin())
  {
    while(1);
  }
}

void loop() {
  // 读取陀螺仪和加速度计数据
  sensors_event_t gyro_event;
  sensors_event_t accel_event;
  gyro.getEvent(&gyro_event);
  accel.getEvent(&accel_event);

  // 计算角度
  angle = angle_gain * (atan2(accel_event.acceleration.y, accel_event.acceleration.z) * 180 / M_PI);
  angle = angle + gyro_gain * (gyro_event.gyro.x - gyro_x_cal);
  angle = constrain(angle, -45, 45);

  // 计算速度和误差
  actual_speed = (motor1->current() + motor2->current()) / 2.0;
  error = target_speed - actual_speed;

  // 计算PID输出
  integral = integral + error;
  derivative = error - last_error;
  output = kp * error + ki * integral + kd * derivative;
  output = constrain(output, -255, 255);

  // 更新电机速度
  motor1->run(output > 0 ? FORWARD : BACKWARD);
  motor2->run(output > 0 ? FORWARD : BACKWARD);
  motor1->setSpeed(abs(output));
  motor2->setSpeed(abs(output));

  // 发送角度和速度数据到蓝牙模块
  bluetooth.print("Angle:");
  bluetooth.print(angle);
  bluetooth.print(",");
  bluetooth.print("Speed:");
  bluetooth.println(actual_speed);

  // 接收蓝牙模块发送的指令
  if(bluetooth.available())
  {
    char command = bluetooth.read();
    switch(command)
    {
      case 'F':
        target_speed = 100;
        break;
      case 'B':
        target_speed = -100;
        break;
      case 'S':
        target_speed = 0;
        break;
      default:
        break;
    }
  }

  // 更新误差和PID参数
  last_error = error;
  delay(10);
}

在这个程序中，我们使用Arduino的PWM输出来控制电机的速度，并且使用PID控制算法来控制小车的平衡。程序通过蓝牙模块接收来自手机等设备的指令，实现小车的前进、后退等操作。