第二章 硬件平台

// 编码器中断处理
#define ENCODER_LEFT_A 2
#define ENCODER_LEFT_B 3
#define ENCODER_RIGHT_A 18
#define ENCODER_RIGHT_B 19

volatile long encoder_left = 0;
volatile long encoder_right = 0;

void setup() {
    pinMode(ENCODER_LEFT_A, INPUT_PULLUP);
    pinMode(ENCODER_LEFT_B, INPUT_PULLUP);
    pinMode(ENCODER_RIGHT_A, INPUT_PULLUP);
    pinMode(ENCODER_RIGHT_B, INPUT_PULLUP);

    attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT_A),
                    leftEncoderA, CHANGE);
    attachInterrupt(digitalPinToInterrupt(ENCODER_RIGHT_A),
                    rightEncoderA, CHANGE);
}

void leftEncoderA() {
    if (digitalRead(ENCODER_LEFT_A) == digitalRead(ENCODER_LEFT_B)) {
        encoder_left++;
    } else {
        encoder_left--;
    }
}

void rightEncoderA() {
    if (digitalRead(ENCODER_RIGHT_A) == digitalRead(ENCODER_RIGHT_B)) {
        encoder_right++;
    } else {
        encoder_right--;
    }
}

// 计算速度
float getSpeed(long encoder_count, float dt) {
    const float PPR = 1000.0;  // 每转脉冲数
    const float WHEEL_RADIUS = 0.05;  // 轮半径(m)

    float rps = encoder_count / PPR / dt;  // 转/秒
    return 2 * PI * WHEEL_RADIUS * rps;  // m/s
}

// 运动学逆解
void omniWheelControl(float vx, float vy, float omega) {
    const float L = 0.2;  // 轮距的一半
    const float R = 0.05; // 轮半径

    // 四轮速度计算
    float v1 = vx - vy - L * omega;  // 左前
    float v2 = vx + vy - L * omega;  // 右前
    float v3 = vx + vy + L * omega;  // 右后
    float v4 = vx - vy + L * omega;  // 左后

    // 转换为轮速(rad/s)
    float w1 = v1 / R;
    float w2 = v2 / R;
    float w3 = v3 / R;
    float w4 = v4 / R;

    setMotorSpeed(1, w1);
    setMotorSpeed(2, w2);
    setMotorSpeed(3, w3);
    setMotorSpeed(4, w4);
}

// L298N控制代码
#define IN1 7
#define IN2 6
#define ENA 5  // PWM引脚

void setup() {
    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(ENA, OUTPUT);
}

// 电机控制函数
void motorControl(int speed) {
    // speed范围: -255到255
    if (speed > 0) {
        // 正转
        digitalWrite(IN1, HIGH);
        digitalWrite(IN2, LOW);
        analogWrite(ENA, speed);
    } else if (speed < 0) {
        // 反转
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, HIGH);
        analogWrite(ENA, -speed);
    } else {
        // 停止
        digitalWrite(IN1, LOW);
        digitalWrite(IN2, LOW);
        analogWrite(ENA, 0);
    }
}

// PWM调速
void loop() {
    // 加速
    for (int speed = 0; speed <= 255; speed += 5) {
        motorControl(speed);
        delay(50);
    }

    // 减速
    for (int speed = 255; speed >= 0; speed -= 5) {
        motorControl(speed);
        delay(50);
    }
}

// TB6612控制
#define PWMA 5
#define AIN1 7
#define AIN2 6
#define STBY 8  // Standby引脚

void setup() {
    pinMode(PWMA, OUTPUT);
    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(STBY, OUTPUT);
    digitalWrite(STBY, HIGH);  // 使能
}

void motorDrive(int speed) {
    if (speed > 0) {
        digitalWrite(AIN1, HIGH);
        digitalWrite(AIN2, LOW);
    } else {
        digitalWrite(AIN1, LOW);
        digitalWrite(AIN2, HIGH);
        speed = -speed;
    }
    analogWrite(PWMA, speed);
}

#define STEP_PIN 3
#define DIR_PIN 4
#define ENABLE_PIN 5

// 微步设置引脚
#define MS1 6
#define MS2 7
#define MS3 8

void setup() {
    pinMode(STEP_PIN, OUTPUT);
    pinMode(DIR_PIN, OUTPUT);
    pinMode(ENABLE_PIN, OUTPUT);
    pinMode(MS1, OUTPUT);
    pinMode(MS2, OUTPUT);
    pinMode(MS3, OUTPUT);

    // 设置1/16细分
    digitalWrite(MS1, HIGH);
    digitalWrite(MS2, HIGH);
    digitalWrite(MS3, HIGH);

    digitalWrite(ENABLE_PIN, LOW);  // 使能
}

// S型加减速曲线
void moveWithAccel(long steps, int max_speed, int accel_steps) {
    digitalWrite(DIR_PIN, steps > 0 ? HIGH : LOW);
    steps = abs(steps);

    for (long i = 0; i < steps; i++) {
        int delay_time;

        if (i < accel_steps) {
            // 加速阶段
            float ratio = (float)i / accel_steps;
            delay_time = 2000 - ratio * (2000 - max_speed);
        } else if (i > steps - accel_steps) {
            // 减速阶段
            float ratio = (float)(steps - i) / accel_steps;
            delay_time = 2000 - ratio * (2000 - max_speed);
        } else {
            // 匀速阶段
            delay_time = max_speed;
        }

        digitalWrite(STEP_PIN, HIGH);
        delayMicroseconds(delay_time);
        digitalWrite(STEP_PIN, LOW);
        delayMicroseconds(delay_time);
    }
}

#include <Servo.h>

Servo myservo;

void setup() {
    myservo.attach(9);  // 连接到引脚9
}

void loop() {
    // 扫描0-180度
    for (int pos = 0; pos <= 180; pos += 1) {
        myservo.write(pos);
        delay(15);
    }

    for (int pos = 180; pos >= 0; pos -= 1) {
        myservo.write(pos);
        delay(15);
    }
}

// 自定义PWM控制(更精确)
void setServoPulse(int pin, int angle) {
    int pulse_width = map(angle, 0, 180, 500, 2500);
    digitalWrite(pin, HIGH);
    delayMicroseconds(pulse_width);
    digitalWrite(pin, LOW);
    delay(20 - pulse_width/1000);
}

#define RPLIDAR_MOTOR 3  // PWM控制电机

void setup() {
    Serial.begin(115200);  // 连接RPLIDAR
    Serial1.begin(115200); // 调试输出

    pinMode(RPLIDAR_MOTOR, OUTPUT);
    analogWrite(RPLIDAR_MOTOR, 255);  // 启动电机
}

void loop() {
    if (Serial.available() >= 5) {
        byte sync1 = Serial.read();
        byte sync2 = Serial.read();

        if (sync1 == 0xA5 && sync2 == 0x5A) {
            // 读取数据包
            byte quality = Serial.read();
            byte angle_low = Serial.read();
            byte angle_high = Serial.read();
            byte distance_low = Serial.read();
            byte distance_high = Serial.read();

            float angle = ((angle_high << 8) | angle_low) / 64.0;
            float distance = ((distance_high << 8) | distance_low) / 4.0;

            Serial1.print("Angle: ");
            Serial1.print(angle);
            Serial1.print(" Distance: ");
            Serial1.println(distance);
        }
    }
}

#include <Wire.h>

const int MPU6050_ADDR = 0x68;

void setup() {
    Serial.begin(9600);
    Wire.begin();

    // 唤醒MPU6050
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x6B);  // PWR_MGMT_1寄存器
    Wire.write(0);     // 设置为0(唤醒)
    Wire.endTransmission(true);
}

void loop() {
    int16_t ax, ay, az, gx, gy, gz;

    // 读取加速度计和陀螺仪
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x3B);  // 从ACCEL_XOUT_H开始读
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDR, 14, true);

    ax = Wire.read() << 8 | Wire.read();
    ay = Wire.read() << 8 | Wire.read();
    az = Wire.read() << 8 | Wire.read();
    Wire.read(); Wire.read();  // 跳过温度
    gx = Wire.read() << 8 | Wire.read();
    gy = Wire.read() << 8 | Wire.read();
    gz = Wire.read() << 8 | Wire.read();

    // 转换为实际值
    float accel_x = ax / 16384.0;  // ±2g
    float accel_y = ay / 16384.0;
    float accel_z = az / 16384.0;
    float gyro_x = gx / 131.0;     // ±250°/s
    float gyro_y = gy / 131.0;
    float gyro_z = gz / 131.0;

    Serial.print("AccX: "); Serial.print(accel_x);
    Serial.print(" AccY: "); Serial.print(accel_y);
    Serial.print(" AccZ: "); Serial.print(accel_z);
    Serial.print(" GyroX: "); Serial.print(gyro_x);
    Serial.print(" GyroY: "); Serial.print(gyro_y);
    Serial.print(" GyroZ: "); Serial.println(gyro_z);

    delay(100);
}

#include <Adafruit_BNO055.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55);

void setup() {
    Serial.begin(9600);
    if (!bno.begin()) {
        Serial.println("BNO055未检测到");
        while (1);
    }

    bno.setExtCrystalUse(true);
}

void loop() {
    // 获取欧拉角(内部融合算法)
    sensors_event_t event;
    bno.getEvent(&event);

    Serial.print("Heading: "); Serial.print(event.orientation.x);
    Serial.print(" Roll: "); Serial.print(event.orientation.y);
    Serial.print(" Pitch: "); Serial.println(event.orientation.z);

    // 获取四元数
    imu::Quaternion quat = bno.getQuat();
    Serial.print("Quat w: "); Serial.print(quat.w());
    Serial.print(" x: "); Serial.print(quat.x());
    Serial.print(" y: "); Serial.print(quat.y());
    Serial.print(" z: "); Serial.println(quat.z());

    delay(100);
}

#define TRIG_PIN 9
#define ECHO_PIN 10

void setup() {
    Serial.begin(9600);
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
}

float getDistance() {
    // 发送触发信号
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // 测量回响时间
    long duration = pulseIn(ECHO_PIN, HIGH, 30000);  // 超时30ms

    if (duration == 0) {
        return -1;  // 超时
    }

    // 计算距离(cm)
    float distance = duration * 0.034 / 2;
    return distance;
}

void loop() {
    float dist = getDistance();

    if (dist > 0) {
        Serial.print("距离: ");
        Serial.print(dist);
        Serial.println(" cm");
    } else {
        Serial.println("测量失败");
    }

    delay(200);
}

// 差速机器人控制
#define MOTOR_L1 7
#define MOTOR_L2 6
#define PWM_L 5
#define MOTOR_R1 4
#define MOTOR_R2 3
#define PWM_R 2

#define ENCODER_L_A 18
#define ENCODER_L_B 19
#define ENCODER_R_A 20
#define ENCODER_R_B 21

volatile long encoder_left = 0;
volatile long encoder_right = 0;

const float WHEEL_RADIUS = 0.05;  // 轮半径(m)
const float WHEEL_BASE = 0.3;     // 轮距(m)
const float PPR = 1000.0;         // 编码器精度

// PID参数
float Kp = 1.0, Ki = 0.1, Kd = 0.05;
float error_left = 0, error_right = 0;
float integral_left = 0, integral_right = 0;
float last_error_left = 0, last_error_right = 0;

void setup() {
    Serial.begin(115200);

    // 电机引脚
    pinMode(MOTOR_L1, OUTPUT);
    pinMode(MOTOR_L2, OUTPUT);
    pinMode(PWM_L, OUTPUT);
    pinMode(MOTOR_R1, OUTPUT);
    pinMode(MOTOR_R2, OUTPUT);
    pinMode(PWM_R, OUTPUT);

    // 编码器引脚
    pinMode(ENCODER_L_A, INPUT_PULLUP);
    pinMode(ENCODER_L_B, INPUT_PULLUP);
    pinMode(ENCODER_R_A, INPUT_PULLUP);
    pinMode(ENCODER_R_B, INPUT_PULLUP);

    attachInterrupt(digitalPinToInterrupt(ENCODER_L_A), leftEncoder, CHANGE);
    attachInterrupt(digitalPinToInterrupt(ENCODER_R_A), rightEncoder, CHANGE);
}

void leftEncoder() {
    if (digitalRead(ENCODER_L_A) == digitalRead(ENCODER_L_B)) {
        encoder_left++;
    } else {
        encoder_left--;
    }
}

void rightEncoder() {
    if (digitalRead(ENCODER_R_A) == digitalRead(ENCODER_R_B)) {
        encoder_right++;
    } else {
        encoder_right--;
    }
}

void setMotor(int motor, int speed) {
    // motor: 0=左, 1=右
    // speed: -255到255
    int pin1, pin2, pwm_pin;

    if (motor == 0) {
        pin1 = MOTOR_L1; pin2 = MOTOR_L2; pwm_pin = PWM_L;
    } else {
        pin1 = MOTOR_R1; pin2 = MOTOR_R2; pwm_pin = PWM_R;
    }

    if (speed > 0) {
        digitalWrite(pin1, HIGH);
        digitalWrite(pin2, LOW);
        analogWrite(pwm_pin, constrain(speed, 0, 255));
    } else if (speed < 0) {
        digitalWrite(pin1, LOW);
        digitalWrite(pin2, HIGH);
        analogWrite(pwm_pin, constrain(-speed, 0, 255));
    } else {
        digitalWrite(pin1, LOW);
        digitalWrite(pin2, LOW);
        analogWrite(pwm_pin, 0);
    }
}

int pidControl(float target, float current, float &integral,
               float &last_error) {
    float error = target - current;
    integral += error;
    integral = constrain(integral, -100, 100);  // 抗积分饱和

    float derivative = error - last_error;
    float output = Kp * error + Ki * integral + Kd * derivative;

    last_error = error;
    return constrain((int)output, -255, 255);
}

void setVelocity(float linear, float angular) {
    // 运动学逆解
    float v_left = linear - angular * WHEEL_BASE / 2;
    float v_right = linear + angular * WHEEL_BASE / 2;

    // 这里应该使用PID闭环控制
    // 简化版直接映射
    int pwm_left = (int)(v_left * 255 / 1.0);   // 假设最大速度1m/s
    int pwm_right = (int)(v_right * 255 / 1.0);

    setMotor(0, pwm_left);
    setMotor(1, pwm_right);
}

void loop() {
    // 串口接收速度命令
    if (Serial.available() >= 8) {
        float linear, angular;
        Serial.readBytes((char*)&linear, 4);
        Serial.readBytes((char*)&angular, 4);

        setVelocity(linear, angular);
    }

    // 发送里程计数据
    static unsigned long last_time = 0;
    unsigned long current_time = millis();

    if (current_time - last_time >= 50) {  // 20Hz
        float dt = (current_time - last_time) / 1000.0;

        // 计算速度
        float v_left = (encoder_left / PPR) * (2 * PI * WHEEL_RADIUS) / dt;
        float v_right = (encoder_right / PPR) * (2 * PI * WHEEL_RADIUS) / dt;

        encoder_left = 0;
        encoder_right = 0;

        // 发送数据
        Serial.print(v_left); Serial.print(",");
        Serial.println(v_right);

        last_time = current_time;
    }
}

#define BATTERY_PIN A0
#define VOLTAGE_DIVIDER_RATIO 4.0  // 分压比

float getBatteryVoltage() {
    int raw = analogRead(BATTERY_PIN);
    float voltage = (raw / 1023.0) * 5.0 * VOLTAGE_DIVIDER_RATIO;
    return voltage;
}

void checkBattery() {
    float voltage = getBatteryVoltage();

    if (voltage < 9.0) {
        // 电池电压过低,停止电机
        Serial.println("警告: 电池电压过低!");
        stopAllMotors();
        digitalWrite(LED_PIN, HIGH);  // 报警LED
    } else if (voltage < 10.0) {
        // 低电量警告
        Serial.println("警告: 电池电量低");
    }
}

// 电压监测
void batteryMonitor() {
    float voltage = getBatteryVoltage();

    if (voltage < 9.0) {
        emergencyStop();  // 紧急停止
    } else if (voltage < 10.0) {
        reducePower();    // 降低功率
        warning();        // 发出警告
    }
}

// 功率管理
void powerManagement() {
    if (isLowBattery()) {
        disableNonCritical();  // 关闭非关键功能
        enableReturnHome();    // 启动返航
    }
}