外置硬盘盒电源开关伺服电机控制程序(树莓派init单进程版)

mg90s.c

/*
 ============================================================================
 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;
}