spdis
123 lines
3.8 KiB
C++
123 lines
3.8 KiB
C++
/*
|
|
* encoder_brake_demo — 编码器刹车测试 v2
|
|
*
|
|
* 编码器2 (gpio67脉冲/gpio72方向) → 控制后面两个轮子同时反转
|
|
* 左后轮: pwmchip8/pwm2, gpio12
|
|
* 右后轮: pwmchip8/pwm1, gpio13
|
|
* 使能: gpio73
|
|
*/
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <cmath>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <ctime>
|
|
#include <csignal>
|
|
|
|
static volatile bool running = true;
|
|
void sig_handler(int) { running = false; }
|
|
|
|
static void write_file(const char *path, const char *val) {
|
|
int fd = open(path, O_WRONLY);
|
|
if (fd >= 0) { write(fd, val, strlen(val)); close(fd); }
|
|
}
|
|
|
|
static int read_pin(const char *path) {
|
|
int fd = open(path, O_RDONLY);
|
|
char v = '0';
|
|
if (fd >= 0) { read(fd, &v, 1); close(fd); }
|
|
return (v == '1') ? 1 : 0;
|
|
}
|
|
|
|
static long now_ns() {
|
|
struct timespec ts;
|
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
|
return ts.tv_sec * 1000000000L + ts.tv_nsec;
|
|
}
|
|
|
|
int main() {
|
|
signal(SIGINT, sig_handler);
|
|
signal(SIGTERM, sig_handler);
|
|
|
|
printf("=== Encoder Brake Demo v2 ===\n");
|
|
printf("编码器: gpio67(脉冲) gpio72(方向)\n");
|
|
printf("左后轮: gpio12 pwm2 | 右后轮: gpio13 pwm1\n");
|
|
printf("掰任意后轮 → 两轮同时反向制动\n\n");
|
|
|
|
write_file("/sys/class/gpio/gpio73/value", "1");
|
|
|
|
write_file("/sys/class/gpio/gpio12/direction", "out");
|
|
write_file("/sys/class/gpio/gpio13/direction", "out");
|
|
write_file("/sys/class/gpio/gpio67/direction", "in");
|
|
write_file("/sys/class/gpio/gpio72/direction", "in");
|
|
|
|
write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", "0");
|
|
write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", "0");
|
|
|
|
const long WINDOW_NS = 100000000L; // 100ms 测速窗口
|
|
const int MAX_DUTY = 10000; // 最大占空比 20%
|
|
const double SCALE = 10.0; // 速度 → 占空比 缩放系数
|
|
|
|
int prev_lsb = read_pin("/sys/class/gpio/gpio67/value");
|
|
int pulse_cnt = 0;
|
|
long t0 = now_ns();
|
|
int brake_duty = 0;
|
|
long last_print = 0;
|
|
int last_dir = read_pin("/sys/class/gpio/gpio72/value");
|
|
|
|
printf("%-8s %8s %8s %6s %8s %s\n",
|
|
"raw_pulse", "speed", "duty_ns", "enc_dir", "mot_dir", "");
|
|
|
|
while (running) {
|
|
int cur_lsb = read_pin("/sys/class/gpio/gpio67/value");
|
|
int cur_dir = read_pin("/sys/class/gpio/gpio72/value");
|
|
|
|
if (cur_lsb != prev_lsb) {
|
|
pulse_cnt++;
|
|
prev_lsb = cur_lsb;
|
|
}
|
|
if (cur_dir != last_dir) last_dir = cur_dir;
|
|
|
|
long t1 = now_ns();
|
|
long dt = t1 - t0;
|
|
|
|
if (dt >= WINDOW_NS) {
|
|
double speed = pulse_cnt / (dt / 1e9);
|
|
|
|
if (speed > 0.5) {
|
|
brake_duty = (int)(speed * SCALE);
|
|
if (brake_duty > MAX_DUTY) brake_duty = MAX_DUTY;
|
|
|
|
const char *mdir = cur_dir ? "0" : "1";
|
|
write_file("/sys/class/gpio/gpio12/value", mdir);
|
|
write_file("/sys/class/gpio/gpio13/value", mdir);
|
|
} else {
|
|
brake_duty = 0;
|
|
}
|
|
|
|
char buf[16];
|
|
snprintf(buf, sizeof(buf), "%d", brake_duty);
|
|
write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", buf);
|
|
write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", buf);
|
|
|
|
if (t1 - last_print > 300000000L) {
|
|
printf("%-8d %8.1f %8d %6d %8s\r",
|
|
pulse_cnt, speed, brake_duty, cur_dir,
|
|
cur_dir ? "rev" : "fwd");
|
|
fflush(stdout);
|
|
last_print = t1;
|
|
}
|
|
|
|
pulse_cnt = 0;
|
|
t0 = t1;
|
|
}
|
|
}
|
|
|
|
write_file("/sys/class/pwm/pwmchip8/pwm1/duty_cycle", "0");
|
|
write_file("/sys/class/pwm/pwmchip8/pwm2/duty_cycle", "0");
|
|
write_file("/sys/class/gpio/gpio73/value", "0");
|
|
printf("\n=== Demo stopped ===\n");
|
|
return 0;
|
|
}
|