Skip to content

Instantly share code, notes, and snippets.

@heiher

heiher/mg90s.c

Last active Nov 16, 2020
Embed
What would you like to do?
外置硬盘盒电源开关伺服电机控制程序(树莓派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
You can’t perform that action at this time.