142 lines
4.6 KiB
C++
142 lines
4.6 KiB
C++
#include "motor_control.h"
|
||
|
||
#if defined(ESP32)
|
||
Servo esp_servo; // 使用 ESP32Servo 库的 Servo 对象
|
||
#else
|
||
Servo standard_servo; // 使用标准 Servo 库的 Servo 对象
|
||
#endif
|
||
|
||
void motor_setup() {
|
||
// --- 电机引脚设置 ---
|
||
pinMode(MOTOR_LEFT_IN1_PIN, OUTPUT);
|
||
pinMode(MOTOR_LEFT_IN2_PIN, OUTPUT);
|
||
pinMode(MOTOR_RIGHT_IN3_PIN, OUTPUT);
|
||
pinMode(MOTOR_RIGHT_IN4_PIN, OUTPUT);
|
||
|
||
#if defined(ESP32)
|
||
// 配置 ESP32 LEDC PWM 通道
|
||
ledcSetup(PWM_CHANNEL_LEFT, PWM_FREQUENCY, PWM_RESOLUTION);
|
||
ledcSetup(PWM_CHANNEL_RIGHT, PWM_FREQUENCY, PWM_RESOLUTION);
|
||
// 将 LEDC 通道附加到电机 ENA/ENB 引脚
|
||
ledcAttachPin(MOTOR_LEFT_ENA_PIN, PWM_CHANNEL_LEFT);
|
||
ledcAttachPin(MOTOR_RIGHT_ENB_PIN, PWM_CHANNEL_RIGHT);
|
||
#else
|
||
// 对于非ESP32板，ENA/ENB引脚通常直接用 analogWrite 控制
|
||
pinMode(MOTOR_LEFT_ENA_PIN, OUTPUT);
|
||
pinMode(MOTOR_RIGHT_ENB_PIN, OUTPUT);
|
||
#endif
|
||
|
||
motor_stop(); // 初始状态停止电机
|
||
Serial.println("电机驱动已初始化。");
|
||
|
||
// --- 舵机初始化 ---
|
||
servo_attach();
|
||
servo_center(); // 舵机初始居中
|
||
Serial.println("舵机已初始化并居中。");
|
||
}
|
||
|
||
// --- 内部辅助函数设置单个电机速度和方向 ---
|
||
void set_motor_speed(char motor_char, int speed, bool forward) {
|
||
// speed: 0-255
|
||
int actual_speed = constrain(speed, 0, 255);
|
||
|
||
if (motor_char == 'L') { // 左电机
|
||
digitalWrite(MOTOR_LEFT_IN1_PIN, forward ? HIGH : LOW);
|
||
digitalWrite(MOTOR_LEFT_IN2_PIN, forward ? LOW : HIGH);
|
||
#if defined(ESP32)
|
||
ledcWrite(PWM_CHANNEL_LEFT, actual_speed);
|
||
#else
|
||
analogWrite(MOTOR_LEFT_ENA_PIN, actual_speed);
|
||
#endif
|
||
} else if (motor_char == 'R') { // 右电机
|
||
digitalWrite(MOTOR_RIGHT_IN3_PIN, forward ? HIGH : LOW);
|
||
digitalWrite(MOTOR_RIGHT_IN4_PIN, forward ? LOW : HIGH);
|
||
#if defined(ESP32)
|
||
ledcWrite(PWM_CHANNEL_RIGHT, actual_speed);
|
||
#else
|
||
analogWrite(MOTOR_RIGHT_ENB_PIN, actual_speed);
|
||
#endif
|
||
}
|
||
}
|
||
|
||
// --- 公开的电机控制函数 ---
|
||
void motor_move_forward(int speed) {
|
||
set_motor_speed('L', speed, true);
|
||
set_motor_speed('R', speed, true);
|
||
Serial.print("电机前进，速度: "); Serial.println(speed);
|
||
}
|
||
|
||
void motor_move_backward(int speed) {
|
||
set_motor_speed('L', speed, false);
|
||
set_motor_speed('R', speed, false);
|
||
Serial.print("电机后退，速度: "); Serial.println(speed);
|
||
}
|
||
|
||
void motor_stop() {
|
||
set_motor_speed('L', 0, true); // 速度为0，方向无所谓
|
||
set_motor_speed('R', 0, true);
|
||
Serial.println("电机停止。");
|
||
}
|
||
|
||
void motor_set_raw_speeds(int left_speed, int right_speed) {
|
||
bool left_forward = left_speed >= 0;
|
||
bool right_forward = right_speed >= 0;
|
||
set_motor_speed('L', abs(left_speed), left_forward);
|
||
set_motor_speed('R', abs(right_speed), right_forward);
|
||
Serial.print("设置原始速度 左:"); Serial.print(left_speed);
|
||
Serial.print(" 右:"); Serial.println(right_speed);
|
||
}
|
||
|
||
void motor_turn_left(int speed, float turn_intensity, int steer_angle) {
|
||
servo_set_angle(steer_angle);
|
||
int outer_speed = speed;
|
||
int inner_speed = speed * (1.0 - constrain(turn_intensity, 0.0, 1.0));
|
||
motor_set_raw_speeds(inner_speed, outer_speed);
|
||
Serial.print("左转: 基础速度="); Serial.print(speed);
|
||
Serial.print(", 强度="); Serial.print(turn_intensity);
|
||
Serial.print(", 舵机角度="); Serial.println(steer_angle);
|
||
}
|
||
|
||
void motor_turn_right(int speed, float turn_intensity, int steer_angle) {
|
||
servo_set_angle(steer_angle);
|
||
int outer_speed = speed;
|
||
int inner_speed = speed * (1.0 - constrain(turn_intensity, 0.0, 1.0));
|
||
motor_set_raw_speeds(outer_speed, inner_speed);
|
||
Serial.print("右转: 基础速度="); Serial.print(speed);
|
||
Serial.print(", 强度="); Serial.print(turn_intensity);
|
||
Serial.print(", 舵机角度="); Serial.println(steer_angle);
|
||
}
|
||
|
||
// --- 舵机控制函数 ---
|
||
void servo_attach() {
|
||
#if defined(ESP32)
|
||
esp_servo.attach(SERVO_PIN);
|
||
#else
|
||
standard_servo.attach(SERVO_PIN);
|
||
#endif
|
||
Serial.println("舵机已连接。");
|
||
}
|
||
|
||
void servo_set_angle(int angle) {
|
||
int constrained_angle = constrain(angle, 0, 180); // 标准舵机角度范围
|
||
#if defined(ESP32)
|
||
esp_servo.write(constrained_angle);
|
||
#else
|
||
standard_servo.write(constrained_angle);
|
||
#endif
|
||
// Serial.print("舵机角度设置为: "); Serial.println(constrained_angle);
|
||
}
|
||
|
||
void servo_center() {
|
||
servo_set_angle(90); // 假设90度是舵机中心位置
|
||
Serial.println("舵机已居中 (90度)。");
|
||
}
|
||
|
||
void servo_detach() {
|
||
#if defined(ESP32)
|
||
esp_servo.detach();
|
||
#else
|
||
standard_servo.detach();
|
||
#endif
|
||
Serial.println("舵机已断开连接。");
|
||
}
|