外置硬盘盒电源开关伺服电机控制程序(树莓派init单进程版)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
============================================================================ | |
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