外置硬盘盒电源开关伺服电机控制程序(树莓派init单进程版)
/* | |
============================================================================ | |
Name : mg90s.c | |
Author : Heiher <r@hev.cc> | |
Copyright : Copyright (c) 2020 everyone. | |
Description : MG90S Motor Controller | |
============================================================================ | |
*/ | |
#include <poll.h> | |
#include <errno.h> | |
#include <fcntl.h> | |
#include <stdio.h> | |
#include <sched.h> | |
#include <unistd.h> | |
#include <stdlib.h> | |
#include <sys/stat.h> | |
#include <sys/types.h> | |
#include <sys/mount.h> | |
static const char *led_prefix = "/sys/class/leds"; | |
static const char *gpio_prefix = "/sys/class/gpio"; | |
static int | |
mount_sysfs (void) | |
{ | |
struct stat s; | |
int ret; | |
if (stat ("/sys/class", &s) == 0) { | |
return 0; | |
} | |
ret = mkdir ("/sys", 0755); | |
if (ret < 0 && errno != EEXIST) { | |
return -1; | |
} | |
return mount ("none", "/sys", "sysfs", 0, NULL); | |
} | |
static int | |
set_scheduler_rt (void) | |
{ | |
struct sched_param sp = { .sched_priority = 1 }; | |
return sched_setscheduler (0, SCHED_RR, &sp); | |
} | |
static int | |
set_led (int led, int brightness) | |
{ | |
char path[256]; | |
char buf[16]; | |
int ret = -1; | |
int len; | |
int fd; | |
snprintf (path, sizeof (path), "%s/led%d/brightness", led_prefix, led); | |
fd = open (path, O_WRONLY); | |
if (fd < 0) { | |
goto exit; | |
} | |
len = snprintf (buf, sizeof (buf), "%d\n", brightness); | |
ret = write (fd, buf, len); | |
close (fd); | |
exit: | |
return ret; | |
} | |
static int | |
gpio_set_export (int gpio, int unexport) | |
{ | |
char path[256]; | |
char pin[16]; | |
int ret = -1; | |
int len; | |
int fd; | |
if (unexport) { | |
snprintf (path, sizeof (path), "%s/unexport", gpio_prefix); | |
} else { | |
snprintf (path, sizeof (path), "%s/export", gpio_prefix); | |
} | |
fd = open (path, O_WRONLY); | |
if (fd < 0) { | |
goto exit; | |
} | |
len = snprintf (pin, sizeof (pin), "%d", gpio); | |
ret = write (fd, pin, len); | |
if (ret <= 0) { | |
goto exit_close; | |
} | |
usleep (10 * 1000); | |
exit_close: | |
close (fd); | |
exit: | |
return ret; | |
} | |
static int | |
gpio_set_direction (int gpio, int direction) | |
{ | |
char path[256]; | |
int ret = -1; | |
int fd; | |
snprintf (path, sizeof (path), "%s/gpio%d/direction", gpio_prefix, gpio); | |
fd = open (path, O_WRONLY); | |
if (fd < 0) { | |
goto exit; | |
} | |
if (direction) { | |
ret = write (fd, "out", 3); | |
} else { | |
ret = write (fd, "in", 2); | |
} | |
close (fd); | |
exit: | |
return ret; | |
} | |
static int | |
gpio_set_edge (int gpio, int edge) | |
{ | |
char path[256]; | |
int ret = -1; | |
int fd; | |
snprintf (path, sizeof (path), "%s/gpio%d/edge", gpio_prefix, gpio); | |
fd = open (path, O_WRONLY); | |
if (fd < 0) { | |
goto exit; | |
} | |
switch (edge) { | |
case 1: | |
ret = write (fd, "rising", 6); | |
break; | |
case 2: | |
ret = write (fd, "falling", 7); | |
break; | |
case 3: | |
ret = write (fd, "both", 4); | |
break; | |
default: | |
ret = write (fd, "none", 4); | |
} | |
close (fd); | |
exit: | |
return ret; | |
} | |
static int | |
gpio_read (int fd) | |
{ | |
char buf[16]; | |
if (pread (fd, buf, 16, 0) <= 0) { | |
return -1; | |
} | |
if (buf[0] == '1') { | |
return 1; | |
} | |
return 0; | |
} | |
static int | |
gpio_write (int fd, int value) | |
{ | |
if (value) { | |
return write (fd, "1", 1); | |
} | |
return write (fd, "0", 1); | |
} | |
static int | |
gpio_poll (int fd, int timeout) | |
{ | |
struct pollfd pfd = { .fd = fd, .events = POLLPRI | POLLERR }; | |
return poll (&pfd, 1, timeout); | |
} | |
static int | |
gpio_open (int gpio, int direction) | |
{ | |
char path[256]; | |
int fd; | |
gpio_set_export (gpio, 1); | |
if (gpio_set_export (gpio, 0) < 0) { | |
fprintf (stderr, "ERR: Export gpio%d failed!\n", gpio); | |
return -1; | |
} | |
if (gpio_set_direction (gpio, direction) < 0) { | |
fprintf (stderr, "ERR: Set direction of gpio%d failed!\n", gpio); | |
return -1; | |
} | |
snprintf (path, sizeof (path), "%s/gpio%d/value", gpio_prefix, gpio); | |
fd = open (path, O_RDWR); | |
if (fd < 0) { | |
fprintf (stderr, "ERR: Open value of gpio%d failed!\n", gpio); | |
return -1; | |
} | |
return fd; | |
} | |
static void | |
gpio_close (int gpio, int fd) | |
{ | |
close (fd); | |
gpio_set_export (gpio, 1); | |
} | |
static void | |
click_button (int fd) | |
{ | |
const double duty_up = 2.0; | |
const double duty_down = 4.0; | |
int i; | |
set_led (0, 255); | |
for (i = 0; i < 10; i++) { | |
gpio_write (fd, 1); | |
usleep (duty_down * 200); | |
gpio_write (fd, 0); | |
usleep ((100.0 - duty_down) * 200); | |
} | |
for (i = 0; i < 10; i++) { | |
gpio_write (fd, 1); | |
usleep (duty_up * 200); | |
gpio_write (fd, 0); | |
usleep ((100.0 - duty_up) * 200); | |
} | |
set_led (0, 0); | |
} | |
static void | |
red_led_blink (void) | |
{ | |
for (;;) { | |
set_led (0, 0); | |
set_led (1, 255); | |
usleep (100 * 1000); | |
set_led (0, 0); | |
set_led (1, 0); | |
usleep (100 * 1000); | |
} | |
} | |
int | |
main (int argc, char *argv[]) | |
{ | |
int fd1; | |
int fd2; | |
int fd3; | |
if (mount_sysfs () < 0) { | |
fprintf (stderr, "ERR: Mount sysfs failed!\n"); | |
goto exit; | |
} | |
if (set_scheduler_rt () < 0) { | |
fprintf (stderr, "WARN: Set realtime scheduler failed!\n"); | |
} | |
fd1 = gpio_open (18, 1); | |
if (fd1 < 0) { | |
goto exit; | |
} | |
fd2 = gpio_open (23, 0); | |
if (fd2 < 0) { | |
goto exit; | |
} | |
if (gpio_set_edge (23, 2) < 0) { | |
goto exit; | |
} | |
fd3 = gpio_open (24, 0); | |
if (fd3 < 0) { | |
goto exit; | |
} | |
set_led (1, 255); | |
for (;;) { | |
gpio_read (fd2); | |
gpio_poll (fd2, -1); | |
gpio_read (fd2); | |
if (gpio_read (fd3)) { | |
click_button (fd1); | |
} | |
} | |
gpio_close (18, fd1); | |
gpio_close (23, fd2); | |
gpio_close (24, fd3); | |
return 0; | |
exit: | |
red_led_blink (); | |
return -1; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment