代码拉取完成，页面将自动刷新
//=============================================================v0.1===2025-01-04
// #include <esp_now.h>
// #include <WiFi.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <FastLED.h>
#include <ESP32Servo.h>
#define LED_PIN 10
#define NUM_LEDS 1
#define BRIGHTNESS 64
#define LED_TYPE WS2812B
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS]; // Define the array of leds
Adafruit_MPU6050 mpu; // Create an MPU6050 object
ESP32PWM pwm; // Create a PWM object
// #define DEBUG // Uncomment to enable debugging output
#define CH1_PIN 6 //接收机pwm输入CH1通道为，GPIO4
#define CH2_PIN 7 //接收机pwm输入CH2通道为，GPIO5
#define CH3_PIN 8 //接收机pwm输入CH3通道为，GPIO6
#define CH4_PIN 9 //接收机pwm输入CH4通道为，GPIO7
#define STEERING_PIN 4 //PIN of Servo
#define THROTTLE_PIN 5 //PIN of ESC
#define CH_STEERING 0 //index of pwm_value[]
#define CH_THROTTLE 1 //index of pwm_value[]
#define CH_PARK 2 //index of pwm_value[]
#define CH_MODE 3 //index of pwm_value[]
#define CAR_MODE_MANUAL 0 //0为遥控模式
#define CAR_MODE_SEMI_AUTO 1 //1为自动方向和手动油门模式
#define CAR_MODE_FULL_AUTO 2 //2为自动驾驶模式
volatile int pwm_value[4] = {0, 0, 0, 0}; // value of CH1, CH2, CH3, CH4
volatile unsigned long rise_time[4] = {0, 0, 0, 0}; // time of rising edge of CH1, CH2, CH3, CH4
void IRAM_ATTR handle_interrupt(int channel) { // interrupt handler
static int pin_state[4] = {0, 0, 0, 0};
pin_state[channel] = digitalRead(channel + CH1_PIN);
if (pin_state[channel] == HIGH) {
rise_time[channel] = micros();
} else {
pwm_value[channel] = micros() - rise_time[channel];
}
}
void IRAM_ATTR CH1_interrupt() { handle_interrupt(0); } // interrupt handler
void IRAM_ATTR CH2_interrupt() { handle_interrupt(1); }
void IRAM_ATTR CH3_interrupt() { handle_interrupt(2); }
void IRAM_ATTR CH4_interrupt() { handle_interrupt(3); }
void (*isr_functions[4])() = {CH1_interrupt, CH2_interrupt, CH3_interrupt, CH4_interrupt}; // array of function pointers
int User_throttle = 0; //RC遥控器发来的用户油门值
int User_steering = 0; //RC遥控器发来的用户转向值
int Pilot_throttle = 0;//上位机发来的油门值
int Pilot_steering = 0;//上位机发来的转向值
const int PWM_MIN = 819; // 'minimum' pulse length count (out of 4096)
const int PWM_MAX = 1638; // 'maximum' pulse length count (out of 4096)
const int MOTOR_MID = 1229; // 需要实际测试
const int MOTOR_RANGE = 390; // Pulse range for Motor Throttle
const int SERVO_MID = 1328; // 需要实际测试
const int SERVO_RANGE = 390; // Pulse range for Motor Throttle
const int MOTOR_OFFSET = 1;
const int SERVO_OFFSET = -1;
// Define a data structure
struct struct_message {
int throttle; // 油门值
int steering; // 转向值
int mode; //驾驶模式，0为遥控模式，1为自动方向和手动油门模式，2为自动驾驶模式
bool park; //停车状态，0为停车，1为起步
} ;
struct struct_message esp_now_data = {0, 0, 0, false}; // Initialize the structure at declaration
struct struct_message rc_data = {0, 0, 0, false}; // Initialize the structure at declaration
struct struct_message pilot_data = {0, 0, 0, false}; // Initialize the structure at declaration
struct struct_message car_output = {0, 0, 0, false}; // Initialize the structure at declaration
// Create a structure object
// struct_message* myData;
// callback function executed when data is received.
// void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
// myData = (struct_message*)incomingData;
// esp_now_data = *myData; //将接收到的数据赋值给esp_now_data
// }
int adj(int v, int s) // v: value, s: step
{
v = v + s;
if(v > 4095) v = 4095;
if(v < 0) v = 0;
return v;
}
void park_change()
{
if(pwm_value[CH_PARK] > 1500){
rc_data.park = false;
}
else{
rc_data.park = true;
}
car_output.park = rc_data.park;
}
void mode_change() //根据遥控器的mode值，切换驾驶模式
{
rc_data.mode = pwm_value[CH_MODE];
if(rc_data.mode <= 1400)
{
car_output.mode = CAR_MODE_MANUAL; //0为遥控模式
}
else if(rc_data.mode > 1400 && rc_data.mode < 1600 )
{
car_output.mode = CAR_MODE_SEMI_AUTO; //1为自动方向和手动油门模式
}
else
{
car_output.mode = CAR_MODE_FULL_AUTO; //2为自动驾驶模式
}
}
bool I2CRead(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t *Data)
{
bool ret = true;
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
if(Wire.endTransmission())
{
ret = false;
Serial.println("I2C Read Errro");
}
// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index = 0;
while (Wire.available())
{
Data[index++] = Wire.read();
}
return ret;
}
uint16_t I2CReadValue(uint8_t addr, uint8_t reg)
{
uint16_t ret = -1;
uint8_t data[2];
if(I2CRead(addr, reg, 2, data))
{
ret = (uint16_t)data[0] << 8 | data[1];
}
return ret;
}
void I2CWriteValue(uint8_t Address, uint8_t Register, uint16_t Data)
{
uint8_t* pData = (uint8_t*)&Data;
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(pData[1]);
Wire.write(pData[0]);
if(Wire.endTransmission())
{
Serial.println("I2C Write Error");
}
}
void read_ina219(){
Serial.printf("%d:%f mA\r\n", 1, I2CReadValue(0x41, 1) / 100.0 / 0.01);
Serial.printf("%d:%f V\r\n", 2, I2CReadValue(0x41, 2) / 2 / 1000.0);
}
void read_mpu6050(){
/* Get new sensor events with the readings */
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
/* Print out the values */
Serial.print("Acceleration X: ");
Serial.print(a.acceleration.x);
Serial.print(", Y: ");
Serial.print(a.acceleration.y);
Serial.print(", Z: ");
Serial.print(a.acceleration.z);
Serial.println(" m/s^2");
Serial.print("Rotation X: ");
Serial.print(g.gyro.x);
Serial.print(", Y: ");
Serial.print(g.gyro.y);
Serial.print(", Z: ");
Serial.print(g.gyro.z);
Serial.println(" rad/s");
Serial.print("Temperature: ");
Serial.print(temp.temperature);
Serial.println(" degC");
Serial.println("");
}
void setup_mpu6050(){
// Try to initialize!
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
Serial.println("MPU6050 Found!");
// set accelerometer Range
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
Serial.print("Accelerometer range set to: ");
switch (mpu.getAccelerometerRange()) {
case MPU6050_RANGE_2_G:
Serial.println("+-2G");
break;
case MPU6050_RANGE_4_G:
Serial.println("+-4G");
break;
case MPU6050_RANGE_8_G:
Serial.println("+-8G");
break;
case MPU6050_RANGE_16_G:
Serial.println("+-16G");
break;
}
// set Gyro Range
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
Serial.print("Gyro range set to: ");
switch (mpu.getGyroRange()) {
case MPU6050_RANGE_250_DEG:
Serial.println("+- 250 deg/s");
break;
case MPU6050_RANGE_500_DEG:
Serial.println("+- 500 deg/s");
break;
case MPU6050_RANGE_1000_DEG:
Serial.println("+- 1000 deg/s");
break;
case MPU6050_RANGE_2000_DEG:
Serial.println("+- 2000 deg/s");
break;
}
// Set filter bandwidth
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
Serial.print("Filter bandwidth set to: ");
switch (mpu.getFilterBandwidth()) {
case MPU6050_BAND_260_HZ:
Serial.println("260 Hz");
break;
case MPU6050_BAND_184_HZ:
Serial.println("184 Hz");
break;
case MPU6050_BAND_94_HZ:
Serial.println("94 Hz");
break;
case MPU6050_BAND_44_HZ:
Serial.println("44 Hz");
break;
case MPU6050_BAND_21_HZ:
Serial.println("21 Hz");
break;
case MPU6050_BAND_10_HZ:
Serial.println("10 Hz");
break;
case MPU6050_BAND_5_HZ:
Serial.println("5 Hz");
break;
}
}
void setup() {
Serial.begin(115200); // TypeC
Serial1.begin(115200, SERIAL_8N1, 20, 21); // RS232: rx = 20, tx = 21
delay(1000);
Serial.println("ESP32 Receiver Serial Ready!");
Serial1.println("ESP32 Receiver Serial1 Ready!");
Wire.begin(2, 3, 400000L); // SDA, SCL, 400kHz
setup_mpu6050();
delay(100);
// Set the RC receiver pins as inputs and attach the interrupts
for (int i = 0; i < 4; i++) {
pinMode(CH1_PIN + i, INPUT);
attachInterrupt(digitalPinToInterrupt(CH1_PIN + i), isr_functions[i], CHANGE);
}
// ledcSetup(CH_STEERING, 50, 14); // 定义通道0（steering）
// ledcSetup(CH_THROTTLE, 50, 14); // 定义通道1（Throttle）PWM输出为pin1，50Hz，14-bit resolution
// 分配PWM输出通道到管脚
// ledcAttach(STEERING_PIN, CH_STEERING, 14);
// ledcAttach(THROTTLE_PIN, CH_THROTTLE, 14);
ledcAttach(STEERING_PIN, 50, 14);
ledcAttach(THROTTLE_PIN, 50, 14);
// WiFi.mode(WIFI_STA);
// for(int i = 0; i < 10; i++)
// Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
// if (esp_now_init() != esp_OK) {
// Serial.println("Error initializing ESP-NOW");
// return;
// }
// esp_now_register_recv_cb(OnDataRecv);
FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
FastLED.setBrightness(BRIGHTNESS);
leds[0] = CRGB::Pink;
FastLED.show();
delay(1000);
leds[0] = CRGB::Green;
FastLED.show();
delay(1000);
leds[0] = CRGB::Yellow;
FastLED.show();
delay(1000);
//SETUP ENDS
}
void loop() {
// Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
// read_ina219();
// read_mpu6050();
if (Serial.available()) {
String CMD = Serial.readStringUntil('\n');
// 检查输入字符串是否以 "T" 开头
if (CMD.startsWith("T")) {
// 查找冒号的位置
int colonIndex = CMD.indexOf(':');
if (colonIndex != -1) {
// 提取油门值
String throttleStr = CMD.substring(1, colonIndex);
int throttle = throttleStr.toInt();
// 提取转向值
String steeringStr = CMD.substring(colonIndex + 2);
int steering = steeringStr.toInt();
// 将解析后的值存储到 pilot_data 结构体中
pilot_data.steering = steering;
pilot_data.throttle = throttle;
// 输出解析后的值
// Serial.print("Throttle: "); Serial.println(throttle);
// Serial.print("Steering: "); Serial.println(steering);
// 发送解析后的值到 Serial1
Serial1.printf("T%d:S%d\n", throttle, steering);
} else {
Serial.println("Invalid input format. Expected 'T<throttle>:S<steering>'");
}
} else {
Serial.println("Invalid input format. Expected input starting with 'T'");
}
}
if (Serial1.available()) {
String CMD = Serial1.readStringUntil('\n');
// 检查输入字符串是否以 "T" 开头
if (CMD.startsWith("T")) {
// 查找冒号的位置
int colonIndex = CMD.indexOf(':');
if (colonIndex != -1) {
// 提取油门值
String throttleStr = CMD.substring(1, colonIndex);
int throttle = throttleStr.toInt();
// 提取转向值
String steeringStr = CMD.substring(colonIndex + 2);
int steering = steeringStr.toInt();
// 将解析后的值存储到 pilot_data 结构体中
pilot_data.steering = steering;
pilot_data.throttle = throttle;
// 输出解析后的值
// Serial.print("Throttle: "); Serial.println(throttle);
// Serial.print("Steering: "); Serial.println(steering);
// 发送解析后的值到 Serial1
Serial.printf("T%d:S%d\n", throttle, steering);
} else {
Serial1.println("Invalid input format. Expected 'T<throttle>:S<steering>'");
}
} else {
Serial1.println("Invalid input format. Expected input starting with 'T'");
}
}
rc_data.steering = pwm_value[CH_STEERING];
rc_data.throttle = pwm_value[CH_THROTTLE];
park_change(); //pwm_value[CH_PARK]
mode_change(); //pwm_value[CH_MODE]
if(car_output.mode== CAR_MODE_FULL_AUTO) {
// Controlled by Pilot
car_output.throttle = pilot_data.throttle;
car_output.steering = pilot_data.steering;
}
else if(car_output.mode == CAR_MODE_SEMI_AUTO){
// Controlled by both RC and Pilot
// car_output.throttle = rc_data.throttle;
car_output.throttle = map(rc_data.throttle-1493,888-1493,2149-1493,-100,100);
car_output.steering = pilot_data.steering;
}
else { // car_output.mode = CAR_MODE_MANUAL
// Controlled by RC Controller
// RC => CAR
car_output.steering = map(rc_data.steering-1488,872-1488,2113-1488,-100,100);
car_output.throttle = map(rc_data.throttle-1493,888-1493,2149-1493,-100,100);
// CAR => Pilot
Serial.printf("T%dS%d\n",car_output.throttle,car_output.steering);
Serial1.printf("T%dS%d\n",car_output.throttle,car_output.steering);
}
if(car_output.park == 1){
car_output.throttle = 0;
}
// CAR => PWM
int pwm_steering = map(car_output.steering,-100,100,SERVO_MID-SERVO_RANGE,SERVO_MID+SERVO_RANGE);
int pwm_throttle = map(car_output.throttle,-100,100,MOTOR_MID-MOTOR_RANGE,MOTOR_MID+MOTOR_RANGE);
pwm_steering = min(max(pwm_steering,PWM_MIN),PWM_MAX);
pwm_throttle = min(max(pwm_throttle,PWM_MIN),PWM_MAX);
#ifdef DEBUG // Print the values for debugging
// Read the RC receiver values
for (int i = 0; i < 4; i++) {
Serial.print(" CH"); Serial.print(i + 1); Serial.print(": "); Serial.print(pwm_value[i]);
}
Serial.printf(" %d, %d, %d, %d, %d\n", car_output.steering, pwm_steering, car_output.throttle, pwm_throttle, car_output.park );
#endif
ledcWrite(STEERING_PIN,pwm_steering);
ledcWrite(THROTTLE_PIN,pwm_throttle);
delay(10);
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手