20250402 大作业——milkv-duo 小车设计工作日志
milkv-duo 小车工作日志
2025.4.2——2025.4.9
高级程序设计课布置了大作业，要求如图
相比起上个大作业，这一次的大作业似乎更有抓手一些，所以准备先从 milkv-duo 的官网以及询问 AI 来入手，为搭建小车做好准备。
在找寻购买的材料的同时跟随官方网站的步骤进行 milkv-duo 开发板的环境的配置。
这段时间我只购买了一张 sd/tf 卡（要求内存至少大于 1G ，我选择的是 32G 大小的）和一个读卡器，sd/tf 卡用于烧录官方的镜像文件以及储存后续要用到的代码。做嵌入式设计的第一步就是先把自己的硬件跑起来。
首先是下载官方镜像文件和用来烧录镜像的工具，本次项目中我使用到的 milkv-duo-sd-v1.1.4.img 这个镜像文件（直接在官方镜像链接中找到这个的压缩包下载即可）和 balenaEtcher 这个工具。
下载好以后打开烧录工具，根据弹出的画面依次选择烧录文件和目标磁盘耐心等待烧录完成即可。
烧录已经就绪。接下来就可以把 sd 卡插入 milkv-duo 开发板上，然后用 type-c 数据线将开发板与电脑连接。等待一段时间后开发板蓝灯亮起。由于我使用的是较为新版的 windows 系统所以无需安装额外的驱动。
接下来就是对开发板进行一系列的处理，首先就是先查看要远程登录的端口信息，使用命令 ping 192.168.42.1 这个地址是 milkv-duo 开发板默认的地址，直接使用就好，弹出如下信息。
然后尝试密码登录。开发板的密码默认为 milkv
可能第一次登录会弹出一些信息，直接输入 yes 即可。
成功登录以后其实说明开发板已经可以正常使用了，然后我跟着官网的文档又进行了对根分区的扩展。
在这里不过多赘述，详情可以参考官方文档 。
同时，我们为了开发方便需要使用 Linux 系统，那在 windows 系统下推荐 microsoft store 里面自带的 Ubuntu 虚拟机进行开发，这个虚拟机的优势在于虚拟机内外文件的传入传出比较方便，并且直接使用命令行就可以对开发板进行各种操作。
2025.4.10——2025.4.17
在这一周的时间里，我尝试购买了一个小车的底板，四个麦克纳姆轮，四个TT马达编码电机，两个 TB6612 电机驱动板以及一个 7.4V 5000mAh 的可充电锂电池组。进行小车初步的组装，以及尝试用开发板驱动电机，使轮子正常转动。
这里买两个电机驱动的原因是，选择的电机驱动板一个只能驱动两个轮子。而且四个电机的话意味着每个电机都有独立的引脚，驱动的时候要给每个电机都写上代码。
在这之前，我发现我只是配置了最基础的环境，还没有让开发板编译程序运行试一下。所以参考官方文档，我拉取了 duo-examples 的文件夹并下载了 host-tools 文件夹，这样就可以编译自己的程序，创建自己的工程了。
需要注意的是每次启动都需要先 setup 一下环境。
登录运行 hello-world 的例子就不在这里展示了，只要跟着上文提到的官方文档走应该都不会有太大问题。
底板组装效果如图。
后面继续安装了电机驱动和电池。接下来就是通过编写代码，让轮子转起来了。
2025.4.18——2025.4.25
发现自己进度好像有点慢了，得加快一些。接下来的工作是要让电机能够驱动轮子转起来，然后这就需要用杜邦线连接电机驱动板上的排针和开发板上的排针。这个时候一定要参考官方文档 仔细比对每一个引脚的功能，不同的引脚具有的功能不同。
仔细比对过后，我写出了四个代码用来独立测试四个电机。
#include <stdio.h>
#include <unistd.h>
#include <wiringx.h>
// 引脚定义
#define IN1 13 // 方向控制引脚1
#define IN2 12 // 方向控制引脚2
#define PWM_PIN 6 // PWM调速引脚
// PWM参数
#define PWM_FULL_SPEED 1000000 // 1ms周期下的全速（100%占空比）
#define PWM_HALF_SPEED 500000 // 半速（50%占空比）
#define RUN_DURATION 2 // 每个状态持续时间（秒）
// 电机方向控制
void set_motor_direction(int forward) {
digitalWrite(IN1, forward ? 1 : 0);
digitalWrite(IN2, forward ? 0 : 1);
}
// PWM速度控制
void set_motor_speed(long duty_ns) {
wiringXPWMSetDuty(PWM_PIN, duty_ns);
printf("当前速度：%.1f%%\n", (duty_ns * 100.0) / PWM_FULL_SPEED);
}
int main() {
// 初始化wiringX
if(wiringXSetup("milkv_duo", NULL) == -1) {
printf("平台初始化失败！\n");
return -1;
}
printf("===== 电机测试程序已启动 =====\n");
// 配置引脚模式
pinMode(IN1, PINMODE_OUTPUT);
pinMode(IN2, PINMODE_OUTPUT);
// PWM初始化
wiringXPWMSetPeriod(PWM_PIN, 1000000); // 1ms周期（1KHz）
wiringXPWMSetPolarity(PWM_PIN, 0); // 0-正常极性（高电平有效）
wiringXPWMEnable(PWM_PIN, 1); // 启用PWM
// 测试序列
printf("\n[1] 正向全速测试...\n");
set_motor_direction(1);
set_motor_speed(PWM_FULL_SPEED);
sleep(RUN_DURATION);
printf("\n[2] 正向半速测试...\n");
set_motor_speed(PWM_HALF_SPEED);
sleep(RUN_DURATION);
printf("\n[3] 反向全速测试...\n");
set_motor_direction(0);
set_motor_speed(PWM_FULL_SPEED);
sleep(RUN_DURATION);
printf("\n[4] 反向半速测试...\n");
set_motor_speed(PWM_HALF_SPEED);
sleep(RUN_DURATION);
// 停止电机
printf("\n[5] 停止电机\n");
wiringXPWMEnable(PWM_PIN, 0);
digitalWrite(IN1, 0);
digitalWrite(IN2, 0);
return 0;
}
四份代码本质上是一样的，只需要修改 define 中的 IN1，IN2，PWM_PIN即可。IN1 和 IN2 用来控制方向而 PWM 用来调整速度。
编译通过上传代码后遇到第一个问题，就是只有一个电机能转，其它的三个电机都转不了。代码没有问题，赶紧检查接线，然后接线也没有问题。
于是选择询问 AI ，AI 给出的建议是可能会存在信号的干扰，建议我重新规划排线，还建议我使用 STBY 等等引脚使得待机状态也能工作，于是我按照AI的建议修改，仔细比对 AI 的建议，确认没有差错以后接通数据线然后开发板迅速升温，等到我发现的时候，开发板已经被烧毁，导致我的进度被迫搁置。
于是我火速在网上下单新的开发板。顺便买了以后要做的循迹和避障所需要的传感器。
等待开发板到达的过程我也没有闲着，我发现了电机不转的根本原因就是引脚复用。看着官方文档我在想有的引脚有这么多功能，是怎么同时实现的。
输入 duo-pinmux -l 指令可以查看当前每个引脚正在启用的功能，刚登录上去查看，得到的就是开发板引脚的默认启动功能。此时我意识到，原来是因为我想用的引脚虽然官网上写着有这些功能，但我没有启用。
所以我找到了与引脚相关的指令：
duo-pinmux -p <== List all pins
duo-pinmux -l <== List all pins and its func
duo-pinmux -r pin <== Get func from pin
duo-pinmux -w pin/func <== Set func to pin
然后根据我自己的需求设置成（没有设置的说明默认功能就是我需要的）：
duo-pinmux -w GP0/GP0
duo-pinmux -w GP1/GP1
duo-pinmux -w GP6/PWM_9
duo-pinmux -w GP7/PWM_8
duo-pinmux -w GP8/PWM_7
duo-pinmux -w GP9/PWM_4
duo-pinmux -w GP12/GP12
duo-pinmux -w GP13/GP13
开发板到了以后赶紧按照我自己的接线方式连接以后编译运行。终于，四个轮子都可以被驱动了。
然后接下来打算做避障，因为对场地的需求比较低。我的循迹底下还自带一个 df9gms 舵机，那我就有了自己的想法，完全可以让小车一直检测障碍物，当正前方遇到障碍物时，舵机转动，超声装置探测左右障碍物距离，然后根据左右两侧的距离进行决策。采取一个贪心的策略，哪边障碍物远就走向哪边。
于是先组装好超声装置：
期间，我找到了 duo-examples 里面 df9gms 的代码用来测试超声装置的转向，同时自己写了 barrier-test 的代码来模拟超声装置检测到障碍物以后与舵机配合转向。
df9gms.c
#include <stdio.h>
#include <unistd.h>
#include <wiringx.h>
/*
Duo
------------------------------------------
PWM 操作在定频时钟100MHz, 写 Period 单位为 ns
DF9GMS 360度 PWM Duty
------------------------------------------
0.4ms - 1.4ms 顺时针减速
1.5ms 停止
1.6ms - 3ms 逆时针加速
*/
static int PWM_PIN = 4;
int main()
{
long i;
// Duo: milkv_duo
// Duo256M: milkv_duo256m
// DuoS: milkv_duos
if(wiringXSetup("milkv_duo", NULL) == -1) {
wiringXGC();
return -1;
}
wiringXPWMSetPeriod(PWM_PIN, 20000000); // 20ms
wiringXPWMSetDuty(PWM_PIN, 1500000); // 1.5ms stop
wiringXPWMSetPolarity(PWM_PIN, 0); // 0-normal, 1-inversed
wiringXPWMEnable(PWM_PIN, 1); // 1-enable, 0-disable
delayMicroseconds(1000000); // 1s
for (i = 10000; i< 3000000; i += 10000) // 10 us 步进
{
wiringXPWMSetDuty(PWM_PIN, i);
printf("Duty: %ld\n", i);
delayMicroseconds(50000); // 50ms
}
wiringXPWMSetDuty(PWM_PIN, 1500000); // 1.5ms stop
return 0;
}
barrier-test.c
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <wiringx.h>
#include <sys/time.h> // 解决 timeval 错误
#include <sys/select.h> // 解决 fd_set 错误
#include <time.h> // 解决 timespec 和 clock_gettime 错误
// 硬件引脚定义
#define TRIG_PIN 0 // 超声波触发引脚
#define ECHO_PIN 1 // 超声波回波引脚
#define SERVO_PIN 4 // 舵机控制引脚(GPIO4支持PWM)
// 参数配置
#define OBSTACLE_DISTANCE 20 // 障碍物阈值20厘米
#define SERVO_SAFE_ANGLE 90 // 检测到障碍物时转向的角度
#define SERVO_NORMAL_ANGLE 0 // 默认角度
#define SERVO_PERIOD 20000000 // 20ms周期(50Hz)
#define DUTY_0_DEG 500000 // 0度对应0.5ms脉宽
#define DUTY_90_DEG 1500000 // 90度对应1.5ms脉宽
// 新增终端设置函数（检测按键不阻塞）
int kbhit() {
struct timeval tv = {0};
fd_set fds;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
}
void setup_ultrasonic() {
pinMode(TRIG_PIN, PINMODE_OUTPUT);
pinMode(ECHO_PIN, PINMODE_INPUT);
digitalWrite(TRIG_PIN, LOW);
}
float get_distance() {
struct timespec start, end;
// 发送10us触发脉冲
digitalWrite(TRIG_PIN, HIGH);
usleep(10);
digitalWrite(TRIG_PIN, LOW);
// 等待回波高电平
while(digitalRead(ECHO_PIN) == LOW);
clock_gettime(CLOCK_MONOTONIC, &start);
// 等待回波结束
while(digitalRead(ECHO_PIN) == HIGH);
clock_gettime(CLOCK_MONOTONIC, &end);
// 计算时间差（单位为秒）
double duration = (end.tv_sec - start.tv_sec) +
(end.tv_nsec - start.tv_nsec) / 1e9;
return duration * 34300 / 2; // 声波往返距离
}
void set_servo(int angle) {
// 将角度转换为PWM脉宽（0.5ms~2.5ms范围）
long duty = DUTY_0_DEG + (DUTY_90_DEG - DUTY_0_DEG) * angle / 90;
wiringXPWMSetPeriod(SERVO_PIN, SERVO_PERIOD);
wiringXPWMSetDuty(SERVO_PIN, duty);
wiringXPWMEnable(SERVO_PIN, 1);
}
// 修改主循环
int main() {
// 保存原始终端设置
struct termios oldt, newt;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO); // 禁用行缓冲和回显
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
// 初始化硬件（原有代码）
if(wiringXSetup("milkv_duo", NULL) == -1) {
printf("硬件初始化失败！\n");
return -1;
}
setup_ultrasonic();
set_servo(SERVO_NORMAL_ANGLE);
int running = 1; // 新增运行状态标志
while(running) {
// 非阻塞检测按键
if(kbhit()) {
char c = getchar();
if(c == 'Q' || c == 'q') {
printf("\n退出程序...\n");
running = 0; // 终止循环
continue;
}
}
// 原有超声波检测逻辑
float distance = get_distance();
printf("当前距离: %.1fcm\n", distance);
if(distance < OBSTACLE_DISTANCE) {
printf("检测到障碍物！转动舵机...\n");
set_servo(SERVO_SAFE_ANGLE);
sleep(3);
set_servo(SERVO_NORMAL_ANGLE);
}
usleep(500000);
}
// 恢复终端设置
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return 0;
}
经过测试，舵机和超声装置也是磨合的比较好了可以进入避障功能的测试了。
2025.4.25——2025.4.30
这段时间一直在修改磨合 barrier 正式避障的代码。但是这个时候我才发现，我的两个电机驱动的方向是相对的，而且我选的是前面的电机驱动驱动左前（LF）和右前（RF）两个轮子，后面的电机驱动驱动的是左后（LR）和右后（RR）两个轮子，现在电机驱动的方向是对着的，也就是说前轮的正前方向和后轮的正前方向是不一样的。所以后轮的前进代码逻辑都需要跟前轮反一下，经过测试，这一版代码是实现功能比较好的。
barrier.c
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <wiringx.h>
#include <sys/time.h>
#include <sys/select.h>
#include <time.h>
// 硬件引脚定义
#define TRIG_PIN 0 // 超声波触发
#define ECHO_PIN 1 // 超声波回波
#define SERVO_PIN 4 // 舵机PWM
// 四轮电机控制引脚（根据实际接线修改）
#define LF_IN1 13 // 左前轮方向1
#define LF_IN2 12 // 左前轮方向2
#define RF_IN1 14 // 右前轮方向1
#define RF_IN2 15 // 右前轮方向2
#define LR_IN1 17 // 左后轮方向1
#define LR_IN2 16 // 左后轮方向2
#define RR_IN1 18 // 右后轮方向1
#define RR_IN2 19 // 右后轮方向2
#define PWM_LF 6 // 左前轮PWM
#define PWM_RF 7 // 右前轮PWM
#define PWM_LR 8 // 左后轮PWM
#define PWM_RR 9 // 右后轮PWM
// 参数配置
#define OBSTACLE_DIST 25 // 障碍物阈值(cm)
#define SCAN_ANGLE 85 // 扫描角度(±度)
#define SERVO_CENTER 90 // 舵机中位
#define SERVO_PERIOD 20000000 // 20ms周期
#define MAX_PERIOD 1000000
#define MOTOR_SPEED 25 // 25%速度
#define ECHO_TIMEOUT 30000
// 状态机枚举
enum State {
STATE_FORWARD,
STATE_STOP,
STATE_SCAN_LEFT,
STATE_SCAN_RIGHT,
STATE_TURN
};
// 全局状态
static enum State robot_state = STATE_FORWARD;
static float left_distance = 0;
static float right_distance = 0;
void setup_gpio() {
// 超声波
pinMode(TRIG_PIN, PINMODE_OUTPUT);
pinMode(ECHO_PIN, PINMODE_INPUT);
digitalWrite(TRIG_PIN, LOW);
// 电机方向引脚
const int dir_pins[] = {LF_IN1, LF_IN2, RF_IN1, RF_IN2,
LR_IN1, LR_IN2, RR_IN1, RR_IN2};
for(int i=0; i<8; i++) {
pinMode(dir_pins[i], PINMODE_OUTPUT);
digitalWrite(dir_pins[i], LOW);
}
// 初始化PWM
wiringXPWMEnable(SERVO_PIN, 1);
wiringXPWMEnable(PWM_LF, 1);
wiringXPWMEnable(PWM_RF, 1);
wiringXPWMEnable(PWM_LR, 1);
wiringXPWMEnable(PWM_RR, 1);
}
void set_motor1(int in1, int in2, int pwm_pin, int speed) {
// 设置方向
digitalWrite(in1, speed > 0 ? HIGH : LOW);
digitalWrite(in2, speed < 0 ? HIGH : LOW);
//计算实际占空比
int duty=(abs(speed)/100*MAX_PERIOD*MOTOR_SPEED/100);
printf("Duty: %d\n", duty);
wiringXPWMSetPeriod(pwm_pin, MAX_PERIOD); // 新增：设置周期为1ms（1000Hz）
// 设置PWM速度
wiringXPWMSetDuty(pwm_pin, duty);
}
void set_motor2(int in1, int in2, int pwm_pin, int speed) {
// 设置方向
digitalWrite(in1, speed > 0 ? LOW : HIGH);
digitalWrite(in2, speed < 0 ? LOW : HIGH);
//计算实际占空比
int duty=(abs(speed)/100*MAX_PERIOD*MOTOR_SPEED/100);
printf("Duty: %d\n", duty);
wiringXPWMSetPeriod(pwm_pin, MAX_PERIOD); // 新增：设置周期为1ms（1000Hz）
// 设置PWM速度
wiringXPWMSetDuty(pwm_pin, duty);
}
void chassis_control(int left, int right) {
// 控制左右两侧电机
set_motor1(LF_IN1, LF_IN2, PWM_LF, left);
set_motor2(LR_IN1, LR_IN2, PWM_LR, left);
set_motor1(RF_IN1, RF_IN2, PWM_RF, right);
set_motor2(RR_IN1, RR_IN2, PWM_RR, right);
}
void set_servo(int angle) {
// 计算正确的脉宽（单位：纳秒）
long pulse_width = 500000 + (2000000 * angle / 180); // 0.5ms + (2ms * angle/180)
wiringXPWMSetPeriod(SERVO_PIN, 20000000); // 20ms周期（50Hz）
wiringXPWMSetDuty(SERVO_PIN, pulse_width);
usleep(200000); // 等待舵机到位
}
float get_distance() {
struct timespec start, end;
int timeout=ECHO_TIMEOUT;
digitalWrite(TRIG_PIN, HIGH);
usleep(12);
digitalWrite(TRIG_PIN, LOW);
//等待回波上升（增加超时检测）
while(digitalRead(ECHO_PIN) == LOW&&(--timeout>0));
if(timeout<=0) return -1.0;
clock_gettime(CLOCK_MONOTONIC, &start);
timeout=ECHO_TIMEOUT;
//等待回波下降（增加超时检测）
while(digitalRead(ECHO_PIN) == HIGH&&(--timeout>0));
if(timeout<=0) return -2.0;
clock_gettime(CLOCK_MONOTONIC, &end);
double duration = (end.tv_sec - start.tv_sec) +
(end.tv_nsec - start.tv_nsec) / 1e9;
float distance=duration * 34300 / 2;
return (distance>=0.0&&distance<=100.0)?distance:-3.0;
}
void avoidance_decision() {
printf("[决策] 左:%.1fcm 右:%.1fcm\n", left_distance, right_distance);
// 有效性标志
int left_valid = (left_distance >= 0 && left_distance <= OBSTACLE_DIST*5);
int right_valid = (right_distance >= 0 && right_distance <= OBSTACLE_DIST*5);
// 两侧数据均无效时强制后退
if (!left_valid && !right_valid) {
printf("传感器异常，紧急后退\n");
chassis_control(-100, -100);
return;
}
// 至少一侧有效时的决策
if (left_valid && right_valid) {
if (left_distance > right_distance) {
printf("左侧空间更大，左转\n");
chassis_control(-100, 100);
} else {
printf("右侧空间更大，右转\n");
chassis_control(100, -100);
}
} else if (left_valid && left_distance > OBSTACLE_DIST) {
printf("左侧可行，左转\n");
chassis_control(-100, 100);
} else if (right_valid && right_distance > OBSTACLE_DIST) {
printf("右侧可行，右转\n");
chassis_control(100, -100);
} else {
printf("无安全路径，后退\n");
chassis_control(-100, -100);
}
}
int kbhit() {
struct timeval tv = {0};
fd_set fds;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
}
int main() {
struct termios oldt, newt;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
if(wiringXSetup("milkv_duo", NULL) == -1) {
printf("初始化失败\n");
return -1;
}
setup_gpio();
set_servo(SERVO_CENTER);
while(1) {
if(kbhit()) {
char c = getchar();
if(c == 'q' || c == 'Q') {
printf("\n退出程序\n");
break;
}
}
float current_dist = get_distance();
printf("前方距离: %.1fcm 状态: %d\n", current_dist, robot_state);
switch(robot_state) {
case STATE_FORWARD:
chassis_control(100, 100);
if(current_dist < OBSTACLE_DIST) robot_state = STATE_STOP;
break;
case STATE_STOP:
chassis_control(0, 0); // 急停
usleep(300000); // 增加300ms稳定时间
float dist_samples[3];
for(int i=0; i<3; i++) {
dist_samples[i] = get_distance();
usleep(100000);
}
// 中值滤波
float median_dist = (dist_samples[0]+dist_samples[1]+dist_samples[2])/3;
if(median_dist < OBSTACLE_DIST) {
robot_state = STATE_SCAN_LEFT;
} else {
// 可能误检测，继续前进
robot_state = STATE_FORWARD;
}
break;
case STATE_SCAN_LEFT:
set_servo(SERVO_CENTER - SCAN_ANGLE);
usleep(300000); // 等待舵机到位
left_distance = get_distance();
robot_state = STATE_SCAN_RIGHT;
break;
case STATE_SCAN_RIGHT:
set_servo(SERVO_CENTER + SCAN_ANGLE);
usleep(300000); // 等待舵机到位
right_distance = get_distance();
avoidance_decision();
robot_state = STATE_TURN;
break;
case STATE_TURN:
set_servo(SERVO_CENTER);
usleep(1000000); // 转向1秒
robot_state = STATE_FORWARD;
break;
}
usleep(100000);
}
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
chassis_control(0, 0);
return 0;
}
目前的问题是，如果左右两边太过于空旷，会导致超声波回波的时候超时，然后会导致代码判断超声装置异常进行后退。代码逻辑还需要优化一下。不过可以等我做完循迹以后再优化。
接下来调试循迹传感器，但是我发现一个问题就是按照说明书上的操作，怎么校准我的循迹模块都不能正常使用，不能正常检测黑线。询问客服也没有获得很好的解决办法，只能退掉重新买一个。循迹的进度又被耽搁。
好在我在这段时间里可以优化一下我接线的走向，之前一版接线比较乱而且不易管理分辨，我可以在加装好上层亚克力板以后，把线串在一二层之间，这样会更整洁一点。并且更换了麦克纳姆轮使得与车身更相配，然后撕掉了外面保护的牛皮纸（味道感觉比较大）。
最后的组装版如下：
2025.5.1——2025.5.5
假期也是得抓紧赶工了，循迹模块还没到的时候，我打算先写个代码展示一下麦克纳姆轮可以进行很复杂的运动方式。我之前代码中涉及到的所有与转向有关的操作都是一边的轮子正转，另一边的轮子倒转，也就是差速转向。
麦克纳姆轮有个好处是可以直接平动，麦克纳姆轮也被称为万向轮，这里我写了个代码展示一下定轴转动，和45°角平动的代码。
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/select.h>
#include <wiringx.h>
#include <math.h>
// 硬件引脚定义（根据实际接线调整）
#define LF_IN1 13
#define LF_IN2 12
#define RF_IN1 14
#define RF_IN2 15
#define LR_IN1 17
#define LR_IN2 16
#define RR_IN1 18
#define RR_IN2 19
#define PWM_LF 6
#define PWM_RF 7
#define PWM_LR 8
#define PWM_RR 9
// 运动参数
#define MAX_PERIOD 20000000 // 20ms周期(50Hz)
#define CAR_LENGTH 6.2 // 前后轮轴距/2(cm)
#define CAR_WIDTH 6.4 // 左右轮距/2(cm)
#define MOVE_DURATION 750000 // 运动持续时间(微秒)
#define BASE_SPEED 25 // 全局速度系数(0-100)
#define ROTATION_FACTOR 0.1 // 旋转速度系数(0-1)
// 电机方向类型
typedef enum { MOTOR_NORMAL, MOTOR_REVERSED } MotorDir;
// 初始化GPIO
void setup_gpio() {
const int dir_pins[] = {LF_IN1, LF_IN2, RF_IN1, RF_IN2,
LR_IN1, LR_IN2, RR_IN1, RR_IN2};
for(int i=0; i<8; i++) {
pinMode(dir_pins[i], PINMODE_OUTPUT);
digitalWrite(dir_pins[i], LOW);
}
wiringXPWMEnable(PWM_LF, 1);
wiringXPWMEnable(PWM_RF, 1);
wiringXPWMEnable(PWM_LR, 1);
wiringXPWMEnable(PWM_RR, 1);
}
// 电机控制（带自动限幅）
void set_motor(int in1, int in2, int pwm_pin, float speed, MotorDir dir) {
// 限幅处理
float clamped_speed = fminf(fmaxf(speed, -100.0), 100.0);
// 方向控制
int in1_state = (clamped_speed > 0) ^ (dir == MOTOR_REVERSED);
int in2_state = (clamped_speed < 0) ^ (dir == MOTOR_REVERSED);
digitalWrite(in1, in1_state ? HIGH : LOW);
digitalWrite(in2, in2_state ? HIGH : LOW);
// 计算占空比
int duty = (fabsf(clamped_speed) * MAX_PERIOD) / 100;
wiringXPWMSetPeriod(pwm_pin, MAX_PERIOD);
wiringXPWMSetDuty(pwm_pin, duty);
// 调试信息
printf("MOTOR: Speed=%.1f Duty=%d\n",
clamped_speed, duty);
}
// 全向运动控制（带自动归一化）
void mecanum_move(float vx, float vy, float omega) {
printf("vx: %.1f vy: %.1f omega: %.1f\n", vx, vy, omega);
float l = CAR_LENGTH;
float w = CAR_WIDTH;
// 对omega应用速度限制
omega *= ROTATION_FACTOR;
// 计算原始轮速
float wheel_lf = vx - vy - omega * (l + w);
float wheel_rf = vx + vy + omega * (l + w);
float wheel_lr = vx + vy - omega * (l + w);
float wheel_rr = vx - vy + omega * (l + w);
// 自动归一化
float max_speed = 0;
max_speed = fmaxf(max_speed, fabsf(wheel_lf));
max_speed = fmaxf(max_speed, fabsf(wheel_rf));
max_speed = fmaxf(max_speed, fabsf(wheel_lr));
max_speed = fmaxf(max_speed, fabsf(wheel_rr));
if(max_speed > 100.0) {
float scale = 100.0 / max_speed;
wheel_lf *= scale;
wheel_rf *= scale;
wheel_lr *= scale;
wheel_rr *= scale;
}
// 设置电机（适配前后轮反向）
set_motor(LF_IN1, LF_IN2, PWM_LF, wheel_lf, MOTOR_NORMAL);
set_motor(RF_IN1, RF_IN2, PWM_RF, wheel_rf, MOTOR_NORMAL);
set_motor(LR_IN1, LR_IN2, PWM_LR, wheel_lr, MOTOR_REVERSED);
set_motor(RR_IN1, RR_IN2, PWM_RR, wheel_rr, MOTOR_REVERSED);
}
// 非阻塞键盘检测
int kbhit() {
struct timeval tv = { .tv_sec = 0, .tv_usec = 0 };
fd_set fds;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
return select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv);
}
// 运动模式清单
void print_menu() {
printf("\n=== 全向运动演示 ===\n");
printf("w - 前进 a - 左平移\n");
printf("s - 后退 d - 右平移\n");
printf("q - 顺转 e - 逆转\n");
printf("z - 左上45 c - 右下45\n");
printf("r - 绕前轴公转 x - 停止\n");
printf("0 - 退出\n");
}
int main() {
// 终端设置
struct termios oldt, newt;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
if(wiringXSetup("milkv_duo", NULL) == -1) {
printf("GPIO初始化失败\n");
return -1;
}
setup_gpio();
print_menu();
while(1) {
if(kbhit()) {
char cmd = getchar();
float speed_factor = BASE_SPEED / 100.0;
switch(cmd) {
case 'w': // 前进
mecanum_move(100 * speed_factor, 0, 0);
usleep(MOVE_DURATION);
break;
case 's': // 后退
mecanum_move(-100 * speed_factor, 0, 0);
usleep(MOVE_DURATION);
break;
case 'a': // 左平移
mecanum_move(0, 100 * speed_factor, 0);
usleep(MOVE_DURATION);
break;
case 'd': // 右平移
mecanum_move(0, -100 * speed_factor, 0);
usleep(MOVE_DURATION);
break;
case 'q': // 顺时针自转
mecanum_move(0, 0, 80 * speed_factor);
usleep(MOVE_DURATION);
break;
case 'e': // 逆时针自转
mecanum_move(0, 0, -80 * speed_factor);
usleep(MOVE_DURATION);
break;
case 'z': // 左上45
mecanum_move(70 * speed_factor, 70 * speed_factor, 0);
usleep(MOVE_DURATION);
break;
case 'c': // 右下45
mecanum_move(-70 * speed_factor, -70 * speed_factor, 0);
usleep(MOVE_DURATION);
break;
case 'r': {
float radius = 15.0; // 旋转半径30cm
float omega = 2.0; // 角速度2rad/s (≈114°/s)
float vy = omega * radius; // 线速度=2 * 30=60cm/s
// 计算理论持续时间（2π/ω）
unsigned int duration_us = (2 * M_PI / omega) * 1000000;
// 同时施加横向速度和旋转
mecanum_move(0, -vy, omega);
usleep(duration_us);
mecanum_move(0, 0, 0);
break;
}
case 'x': // 急停
mecanum_move(0, 0, 0);
break;
case '0': // 退出
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
mecanum_move(0, 0, 0);
return 0;
default:
print_menu();
}
mecanum_move(0, 0, 0);
}
usleep(100000);
}
}
循迹传感器也是重新到了一个。这次校准一遍就通过了，看来确实是上次的传感器出问题了。那么我们赶紧换上新的循迹传感器。我打算采取中央黑线的循迹策略，然后这里我先去学习了一下 PID ，好像就是动态计算误差，然后根据上一次的误差来进行决策使得能够更好地沿着黑线前进。
但是有一个问题是我发现转急弯还是不太合适，所以可能需要引入一些别的参数来调整。然后修改过这些参数以后我来到场地测试，发现急转弯有明显的好转，但是还是有一定的概率就是小车会离开中央黑线，所以可能还需要后续的完善。
trace.c
#include <stdio.h>
#include <stdlib.h>
#include <wiringx.h>
#include <unistd.h>
#include <termios.h>
#include <sys/select.h>
#include <math.h>
// ==== 硬件引脚定义 ====
#define MOTOR_LF_IN1 13
#define MOTOR_LF_IN2 12
#define MOTOR_RF_IN1 14
#define MOTOR_RF_IN2 15
#define MOTOR_LR_IN1 17
#define MOTOR_LR_IN2 16
#define MOTOR_RR_IN1 18
#define MOTOR_RR_IN2 19
#define PWM_LF 6
#define PWM_RF 7
#define PWM_LR 8
#define PWM_RR 9
// ==== 传感器参数 ====
#define I2C_DEV "/dev/i2c-1"
#define SENSOR_ADDR 0x12
#define SENSOR_BITS 8
/*
传感器位定义（按您的需求）：
D7 D6 D5 D4 D3 D2 D1 D0 ← 探头物理位置（左到右）
1=检测到黑线（数据已取反）
*/
const float sensor_weights[] = {
-3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5 // D7到D0对应左到右
};
// ==== 运动控制参数 ====
#define PWM_PERIOD 20000 // 20ms周期
#define BASE_SPEED 30 // 基础速度
#define KP 15.0 // 比例系数
#define KI 0.05 // 积分系数
#define KD 12.0 // 微分系数
#define TURN_BOOST 2.4 // 转向增益
#define CTRL_INTERVAL 50000 // 50ms控制周期
typedef enum { MOTOR_FORWARD, MOTOR_REVERSE } MotorDirection;
// ==== 全局状态 ====
float integral = 0;
float last_error = 0;
struct termios orig_termios;
// ==== 终端控制 ====
void reset_terminal() {
tcsetattr(STDIN_FILENO, TCSANOW, &orig_termios);
}
void set_nonblock_terminal() {
struct termios newt;
tcgetattr(STDIN_FILENO, &orig_termios);
newt = orig_termios;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
}
int key_pressed() {
struct timeval tv = {0L, 0L};
fd_set fds;
FD_ZERO(&fds);
FD_SET(STDIN_FILENO, &fds);
return select(1, &fds, NULL, NULL, &tv);
}
// ==== 电机控制核心 ====
void drive_motor(int in1, int in2, int pwm_pin, int speed, MotorDirection dir) {
speed = (speed > 100) ? 100 : (speed < -100) ? -100 : speed;
// 方向控制（保留原始逻辑）
int in1_state = (dir == MOTOR_FORWARD) ? (speed > 0) : (speed < 0);
int in2_state = (dir == MOTOR_FORWARD) ? (speed < 0) : (speed > 0);
digitalWrite(in1, in1_state ? HIGH : LOW);
digitalWrite(in2, in2_state ? HIGH : LOW);
wiringXPWMSetPeriod(pwm_pin, PWM_PERIOD);
wiringXPWMSetDuty(pwm_pin, (abs(speed) * PWM_PERIOD)/100);
}
// ==== 传感器处理（完全保留您的数据格式）===
float get_sensor_error(int data) {
float weighted_sum = 0;
int active_count = 0;
// 按D7到D0处理（左到右）
for(int i=7; i>=0; i--) {
int val = (data >> i) & 1; // 此处保持1=黑线
if(val) {
weighted_sum += sensor_weights[7-i]; // 权重数组反向匹配
active_count++;
}
}
return active_count ? weighted_sum/active_count : 0;
}
// ==== 循迹控制 ====
void line_follow(int i2c_fd) {
// 读取传感器数据
int data = wiringXI2CReadReg8(i2c_fd, 0x00);
if(data < 0) return;
// ==== 新增调试输出 ====
printf("\rSensor: ");
for(int i=7; i>=0; i--) { // D7到D0显示
printf("%d ", (data >> i) & 1);
if(i == 4) printf(" "); // 中间分隔
}
// ==== 核心修改点1：确保error正确定义 ====
float error = get_sensor_error(data); // 必须保留此行！
// ==== 新增动态速度控制 ====
// 动态参数计算
float dynamic_KP = KP * (1.0 + BASE_SPEED/100.0);
float dynamic_boost = TURN_BOOST * (BASE_SPEED/20.0);
// 速度限制器（误差越大，速度越低）
static float speed_factor = 1.0;
if(fabs(error) > 1.2) { // 急弯检测阈值
speed_factor *= 0.85; // 每次衰减15%速度
} else {
speed_factor = fminf(speed_factor * 1.1, 1.0); // 逐渐恢复速度
}
int dynamic_speed = BASE_SPEED * speed_factor;
// ==== PID计算（使用动态参数）====
integral += error;
integral = fmaxf(fminf(integral, 50), -50);
float derivative = error - last_error;
float output = dynamic_KP * error + KI * integral + KD * derivative;
last_error = error;
// ==== 差速计算（带速度保护）====
int left = dynamic_speed - output * dynamic_boost;
int right = dynamic_speed + output * dynamic_boost;
if(left > 100 || right > 100) {
// 计算超限比例
float scale = 100.0f / fmaxf(left, right);
left = (int)(left * scale);
right = (int)(right * scale);
}
// 强制最小差速（防止高速时转向不足）
int min_diff = (dynamic_speed > 30) ? 25 : 15;
if(abs(left - right) < min_diff) {
left -= min_diff/2;
right += min_diff/2;
}
// ==== 电机驱动（保持您的原始方向设置）====
drive_motor(MOTOR_LF_IN1, MOTOR_LF_IN2, PWM_LF, left, MOTOR_FORWARD);
drive_motor(MOTOR_LR_IN1, MOTOR_LR_IN2, PWM_LR, left, MOTOR_REVERSE);
drive_motor(MOTOR_RF_IN1, MOTOR_RF_IN2, PWM_RF, right, MOTOR_FORWARD);
drive_motor(MOTOR_RR_IN1, MOTOR_RR_IN2, PWM_RR, right, MOTOR_REVERSE);
// ==== 增强调试输出 ====
printf("| SPD:%d Err:%.1f L:%d R:%d\n",
dynamic_speed, error, left, right);
fflush(stdout);
}
// ==== 主程序 ====
int main() {
wiringXSetup("milkv_duo", NULL);
set_nonblock_terminal();
atexit(reset_terminal);
int i2c_fd = wiringXI2CSetup(I2C_DEV, SENSOR_ADDR);
if(i2c_fd < 0) return EXIT_FAILURE;
// 初始化电机引脚
const int pins[] = {MOTOR_LF_IN1, MOTOR_LF_IN2, MOTOR_RF_IN1, MOTOR_RF_IN2,
MOTOR_LR_IN1, MOTOR_LR_IN2, MOTOR_RR_IN1, MOTOR_RR_IN2};
for(int i=0; i<8; i++) {
pinMode(pins[i], PINMODE_OUTPUT);
digitalWrite(pins[i], LOW);
}
printf("黑线循迹运行中（按Q退出）\n");
while(1) {
if(key_pressed()) {
char c = getchar();
if(c == 'q' || c == 'Q') {
drive_motor(MOTOR_LF_IN1, MOTOR_LF_IN2, PWM_LF, 0, MOTOR_FORWARD);
drive_motor(MOTOR_RF_IN1, MOTOR_RF_IN2, PWM_RF, 0, MOTOR_FORWARD);
drive_motor(MOTOR_LR_IN1, MOTOR_LR_IN2, PWM_LR, 0, MOTOR_REVERSE);
drive_motor(MOTOR_RR_IN1, MOTOR_RR_IN2, PWM_RR, 0, MOTOR_REVERSE);
break;
}
}
line_follow(i2c_fd);
usleep(CTRL_INTERVAL);
}
return EXIT_SUCCESS;
}
整体上来说这次嵌入式设计还是一波三折，但是最后也是较为完整地做下来啦。
浙公网安备 33010602011771号