自平衡自行車

// vehicle.ino

// I2C pins for MPU6050 communication

#define I2C_SDA A4

#define I2C_SCL A5

// MPU6050 I2C address

#define MPU6050_ADDR 0x68

// SPI pins for nRF24L01

#define SPI_MOSI 11

#define SPI_MISO 12

#define SPI_SCK 13

// nRF24L01 control pins

#define NRF24L01_CE 9

#define NRF24L01_CSN 10

// Servo motor (steering) configuration

#define SERVO_1_SIG 8

#define SERVO_1_CENTER 90

#define SERVO_1_MIN 40

#define SERVO_1_MAX 140

// Brushless motor (drive) pins

#define MOTOR_1_PWM 6

#define MOTOR_1_DIR 7

// Brushless motor (balance wheel) pins

#define MOTOR_2_PWM 5

#define MOTOR_2_DIR 4

#define MOTOR_2_EN A1

#define MOTOR_2_ENC_A 2

#define MOTOR_2_ENC_B 3

// Timeout for lost radio connection

#define LOST_CONNECTION_TIMEOUT_MS 200

// Control limits

#define STEERING_MAX 350

#define SPEED_MAX 80

#define STEERING_CENTER 1500

#define SPEED_LIMIT 4

#include <RF24.h>

#include <SPI.h>

#include <Servo.h>

#include <Wire.h>

#include <nRF24L01.h>

#include "printf.h"

// Gyro and accelerometer variables

float gyroAmount = 0.996; // Weight for gyro in angle calculation

bool isVertical = false; // Indicates if vehicle is balanced

float k1 = 150; // PID

float k2 = 8; // PID

float k3 = 80; // PID

float k4 = 0.6; // PID

float loopTime = 10; // Loop time in milliseconds

struct OffsetsObj {

  int16_t acY;

  int16_t acZ;

};

OffsetsObj offsets; // Calibration offsets

float alpha = 0.4; // Filter coefficient for gyro

int16_t acY, acZ, gyX, gyroXfilt; // Raw and filtered sensor data

int16_t acYc, acZc; // Corrected accelerometer data

int16_t gyXOffset = 0; // Gyro offset

int32_t gyXOffsetSum = 0; // Sum for gyro offset calculation

float robotAngle; // Calculated robot angle

float accAngle; // Accelerometer-based angle

volatile byte pos; // Encoder position

volatile int encCount = 0; // Encoder count

int16_t motorSpeed; // Motor speed

int32_t motorPos; // Motor position

int speedValue = 0; // Current speed value

int steeringValue = STEERING_CENTER; // Current steering value

long currentTime, previousTime = 0; // Timing variables

RF24 radio(NRF24L01_CE, NRF24L01_CSN);

const byte address[6] = "00315"; // Radio address

// Data structure for remote control input

struct RemoteData {

  uint16_t joystickX; // Joystick X-axis (speed)

  uint16_t joystickY; // Joystick Y-axis (steering)

  bool joystickSw; // Joystick button

};

RemoteData remoteData;

Servo servo1;

void setupRadio() {

  Serial.println("Radio initializing...");

  radio.begin();

  radio.setAutoAck(true); // Enable auto-acknowledgment

  radio.setChannel(82); // Set RF channel

  radio.setPALevel(RF24_PA_MAX); // Set power level to maximum

  radio.setDataRate(RF24_2MBPS); // Set data rate

  radio.openReadingPipe(1, address); // Open reading pipe

  radio.startListening(); // Enter receive mode

  radio.printDetails(); // Print radio configuration

  Serial.println("Radio initialized");

}

void setupServo1() {

  Serial.println("Servo 1 initializing...");

  servo1.attach(SERVO_1_SIG); // Attach servo to signal pin

  servo1.write(SERVO_1_CENTER); // Center the servo

  Serial.println("Servo 1 initialized");

}

void setupMotor1() {

  Serial.println("Motor 1 initializing...");

  pinMode(MOTOR_1_PWM, OUTPUT);

  digitalWrite(MOTOR_1_PWM, HIGH); // Stop motor

  pinMode(MOTOR_1_DIR, OUTPUT);

  Serial.println("Motor 1 initialized");

}

void setupMotor2() {

  Serial.println("Motor 2 initializing...");

  pinMode(MOTOR_2_PWM, OUTPUT);

  digitalWrite(MOTOR_2_PWM, LOW); // Stop motor

  pinMode(MOTOR_2_DIR, OUTPUT);

  pinMode(MOTOR_2_EN, OUTPUT);

  pinMode(MOTOR_2_ENC_A, INPUT);

  pinMode(MOTOR_2_ENC_B, INPUT);

  Serial.println("Motor 2 initialized");

}

void setup() {

  Serial.begin(115200);

  Serial.println("Vehicle initializing...");

  printf_begin(); // Initialize printf for radio debugging

  setupRadio();

  setupServo1();

  setupMotor1();

  setupMotor2();

  motor2Control(0); // Stop balance motor

  Serial.println("Interrupts initializing...");

  attachInterrupt(0, encRead, CHANGE); // Encoder interrupt

  attachInterrupt(1, encRead, CHANGE);

  Serial.println("Interrupts initialized");

  // Initialize calibration offsets

  offsets.acY = -550;

  offsets.acZ = -2450;

  delay(3000); // Wait for system stabilization

  angleSetup(); // Initialize MPU6050

  Serial.println("Vehicle initialized");

}

void loop() {

  static unsigned long lastReceivedTime = 0;

  static bool isConnected = false;

  // Check for incoming radio data

  if (radio.available()) {

    radio.read(&remoteData, sizeof(remoteData));

    lastReceivedTime = millis(); // Update last received time

    isConnected = true; // Mark as connected

  } else if (millis() - lastReceivedTime > LOST_CONNECTION_TIMEOUT_MS) {

    // Clear remote data on connection loss

    memset(&remoteData, 0, sizeof(remoteData));

    isConnected = false;

  }

  // Control drive motor with joystick X-axis

  if (!isConnected) {

    digitalWrite(MOTOR_1_DIR, LOW);

    digitalWrite(MOTOR_1_PWM, HIGH); // Stop motor

  } else if (remoteData.joystickX == 256) {

    digitalWrite(MOTOR_1_DIR, LOW);

    digitalWrite(MOTOR_1_PWM, HIGH); // Stop motor

  } else if (remoteData.joystickX < 256) {

    digitalWrite(MOTOR_1_DIR, LOW); // Forward

    int pwmValue = map(remoteData.joystickX, 0, 256, 0, 255);

    pwmValue = constrain(pwmValue, 0, 255);

    analogWrite(MOTOR_1_PWM, pwmValue);

  } else {

    digitalWrite(MOTOR_1_DIR, HIGH); // Backward

    int pwmValue = map(remoteData.joystickX, 256, 512, 255, 0);

    pwmValue = constrain(pwmValue, 0, 255);

    analogWrite(MOTOR_1_PWM, pwmValue);

  }

  // Control servo with joystick Y-axis

  if (!isConnected || remoteData.joystickY == 256) {

    servo1.write(SERVO_1_CENTER); // Center servo

  } else if (remoteData.joystickY < 256) {

    servo1.write(map(remoteData.joystickY, 0, 256, SERVO_1_MIN, SERVO_1_CENTER));

  } else {

    servo1.write(map(remoteData.joystickY, 256, 512, SERVO_1_CENTER, SERVO_1_MAX));

  }

  currentTime = millis();

  if (currentTime - previousTime >= loopTime) {

    angleCalc(); // Calculate robot angle

    motorSpeed = -encCount; // Update motor speed

    encCount = 0; // Reset encoder count

    if (isVertical) {

      digitalWrite(MOTOR_2_EN, HIGH); // Enable balance motor

      float gyroX = gyX / 131.0; // Convert gyro to deg/s

      gyroXfilt = alpha * gyroX + (1 - alpha) * gyroXfilt; // Filter gyro

      motorPos += motorSpeed; // Update motor position

      motorPos = constrain(motorPos, -110, 110);

      // Calculate PID control for balance motor

      int pwm = constrain(

          k1 * robotAngle + k2 * gyroXfilt + k3 * motorSpeed + k4 * motorPos,

          -255, 255);

      motor2Control(-pwm); // Apply control

    } else {

      digitalWrite(MOTOR_2_EN, LOW); // Disable motor

      speedValue = 0;

      motor2Control(0);

      motorPos = 0;

    }

    previousTime = currentTime;

  }

}

// functions.ino

// MPU6050 register addresses

#define ACCEL_CONFIG 0x1C

#define GYRO_CONFIG 0x1B

#define ACCEL_YOUT_H 0x3D

#define ACCEL_ZOUT_H 0x3F

#define GYRO_XOUT_H 0x43

#define PWR_MGMT_1 0x6B

#define ACC_SENS 0 // Accelerometer sensitivity

#define GYRO_SENS 1 // Gyro sensitivity

void writeTo(byte device, byte address, byte value) {

  // Write a single byte to the specified I2C device register

  Wire.beginTransmission(device);

  Wire.write(address);

  Wire.write(value);

  Wire.endTransmission(true);

}

void angleCalc() {

  // Read accelerometer Y-axis

  Wire.beginTransmission(MPU6050_ADDR);

  Wire.write(ACCEL_YOUT_H);

  Wire.endTransmission(false);

  Wire.requestFrom(MPU6050_ADDR, 2, true);

  acY = Wire.read() << 8 | Wire.read();

  // Read accelerometer Z-axis

  Wire.beginTransmission(MPU6050_ADDR);

  Wire.write(ACCEL_ZOUT_H);

  Wire.endTransmission(false);

  Wire.requestFrom(MPU6050_ADDR, 2, true);

  acZ = Wire.read() << 8 | Wire.read();

  // Read gyro X-axis

  Wire.beginTransmission(MPU6050_ADDR);

  Wire.write(GYRO_XOUT_H);

  Wire.endTransmission(false);

  Wire.requestFrom(MPU6050_ADDR, 2, true);

  gyX = Wire.read() << 8 | Wire.read();

  // Apply calibration offsets

  acYc = acY - offsets.acY;

  acZc = acZ - offsets.acZ;

  gyX -= gyXOffset;

  // Calculate robot angle using complementary filter

  robotAngle += gyX * loopTime / 1000 / 65.536; // Integrate gyro

  accAngle = -atan2(acYc, -acZc) * 57.2958; // Accelerometer angle

  robotAngle = robotAngle * gyroAmount + accAngle * (1.0 - gyroAmount);

  // Update balance state

  isVertical = (abs(robotAngle) < 0.1);

}

void angleSetup() {

  // Initialize MPU6050

  Wire.begin();

  delay(100);

  writeTo(MPU6050_ADDR, PWR_MGMT_1, 0); // Wake up MPU6050

  writeTo(MPU6050_ADDR, ACCEL_CONFIG, ACC_SENS << 3); // Set accelerometer sensitivity

  writeTo(MPU6050_ADDR, GYRO_CONFIG, GYRO_SENS << 3); // Set gyro sensitivity

  delay(100);

  // Calibrate gyro offset

  for (int i = 0; i < 1024; i++) {

    angleCalc();

    gyXOffsetSum += gyX;

    delay(3);

  }

  gyXOffset = gyXOffsetSum >> 10; // Average offset

  Serial.print("GyX offset: ");

  Serial.println(gyXOffset);

}

void motor2Control(int sp) {

  // Control balance motor speed and direction

  if (sp > 0)

    digitalWrite(MOTOR_2_DIR, HIGH); // Forward

  else

    digitalWrite(MOTOR_2_DIR, LOW); // Reverse

  analogWrite(MOTOR_2_PWM, 255 - abs(sp)); // Apply PWM value

}

void encRead() {

  // Read encoder state for balance motor

  byte cur = (!digitalRead(MOTOR_2_ENC_A) << 1) + !digitalRead(MOTOR_2_ENC_B);

  byte old = pos & B00000011;

  byte dir = (pos & B00110000) >> 4;

  if (cur == 3)

    cur = 2;

  else if (cur == 2)

    cur = 3;

  if (cur != old) {

    if (dir == 0) {

      if (cur == 1 || cur == 3)

        dir = cur;

    } else {

      if (cur == 0) {

        if (dir == 1 && old == 3)

          encCount--;

        else if (dir == 3 && old == 1)

          encCount++;

        dir = 0;

      }

    }

    pos = (dir << 4) + (old << 2) + cur;

  }

}