hackens-org-configurations/machines/agb02/agb/agb.cpp

#include <sys/socket.h>
#include <sys/types.h>
#include <arpa/inet.h>
#include <gpiod.hpp>
#include <iostream>
#include <fstream>
#include <thread>

using namespace std::literals::chrono_literals;

constexpr std::chrono::microseconds debounce = 40ms;
constexpr std::chrono::microseconds poll_period = 5ms;
constexpr std::chrono::microseconds autorepeat_delay = 70ms;
constexpr std::chrono::microseconds server_ratelimit = 50ms;
constexpr std::chrono::microseconds retry_timeout = 500ms;

constexpr double joystick_movement = 0.2;

const gpiod::line::offsets drive_down = { 21, 13, 6 };

const gpiod::line::offsets decoder = { 3, 4, 17, 27, 24, 23, 18, 2 }; // lsbf
const gpiod::line::offsets joystick = { 19, 26, 5, 0 }; // x+, y+, x-, y-
const gpiod::line::offset black_button = 20;
const gpiod::line::offset white_button = 16;

const gpiod::line_settings input_settings =
  gpiod::line_settings()
    .set_direction(gpiod::line::direction::INPUT)
    .set_bias(gpiod::line::bias::PULL_UP)
    .set_active_low(false)
    .set_debounce_period(debounce);

constexpr std::array<uint8_t, 256> decoder_table =
  #include "decoder_table.inl"

uint8_t read_decoder_realpos(gpiod::line_request& line_reader){
  static gpiod::line::values decoder_read(8);
  line_reader.get_values(decoder, decoder_read);
  uint8_t graycode = 0;
  for(uint8_t i = 0; i < 8; ++i) graycode |= uint8_t(decoder_read[i]) << i;
  return decoder_table[graycode];
};

inline void clamp_decoder(uint8_t& decoder, int move){
  decoder = uint8_t(std::clamp(decoder + move, 0, 255));
}

int main(const int argc, char const* const* const argv) {
  if(argc < 2) {
    std::cerr << "usage: agb gpiodevice" << std::endl;
    return 1;
  }

  /// init gpio chip ///

  gpiod::chip chip(argv[1]);
  gpiod::line_request line_reader =
    chip.prepare_request()
      .set_consumer("AGB")
      .add_line_settings(drive_down,
        gpiod::line_settings()
          .set_direction(gpiod::line::direction::OUTPUT)
          .set_drive(gpiod::line::drive::OPEN_DRAIN)
          .set_output_value(gpiod::line::value::INACTIVE)
      )
      .add_line_settings({ black_button, white_button }, input_settings)
      .add_line_settings(joystick,
          gpiod::line_settings(input_settings)
            .set_active_low(true))
      .add_line_settings(decoder,
          gpiod::line_settings(input_settings)
            .set_debounce_period(0ms))
      .do_request();

  // let the settings apply
  std::this_thread::sleep_for(poll_period);

  /// internal state and buffers ///

  std::chrono::time_point now = std::chrono::system_clock::now();

  gpiod::line::values joystick_read(4);
  gpiod::line::values joystick_last_read(4);
  line_reader.get_values(joystick, joystick_read);
  std::vector<std::chrono::time_point<std::chrono::system_clock>> rising_point = { now, now, now, now };
  std::pair<double, double> spot_pos(0.0, 0.0); //TODO: init from server
  auto joystick_move = [&](int i) -> double {
    if (! bool(joystick_read[i]))
      return 0.0;
    else if (bool(joystick_last_read[i])){
      if (now - rising_point[i] < autorepeat_delay)
        return 0.0;
      else
        return joystick_movement;
    } else {
      rising_point[i] = now;
      return 1.0;
    }
  };

  uint8_t decoder_pos = 0; //TODO: init from server
  uint8_t decoder_realpos = read_decoder_realpos(line_reader);

  uint8_t white_state = 0;
  bool white_pressed = false;
  bool black_pressed = false;

  bool has_changed = true;
  std::chrono::time_point last_send = now;
  std::string postData;

  /// init server communication ///

  int socket_file_desc;
connection:
  socket_file_desc = socket(AF_INET, SOCK_STREAM, 0);
  {
    sockaddr_in socket_addr = {
      .sin_family = AF_INET,
      .sin_port = htons(1235),
      .sin_addr = { .s_addr = inet_addr("10.10.10.1") }
    };
    while (connect(socket_file_desc,
                   reinterpret_cast<const sockaddr*>(&socket_addr),
                   sizeof(socket_addr)) < 0) {
      std::cerr << "Failed to open tcp socket, retrying..." << std::endl;
      std::this_thread::sleep_for(retry_timeout);
    }
    std::cout << "Connected." << std::endl;
  }

  for(;;){
    std::this_thread::sleep_for(poll_period);
    now = std::chrono::system_clock::now();

    /// joystick ///
    std::swap(joystick_read, joystick_last_read);
    line_reader.get_values(joystick, joystick_read);

    spot_pos.first  += joystick_move(0);
    spot_pos.second += joystick_move(1);
    spot_pos.first  -= joystick_move(2);
    spot_pos.second -= joystick_move(3);

    if (bool(joystick_read[0]) || bool(joystick_read[1])
        || bool(joystick_read[2]) || bool(joystick_read[3])){
      spot_pos.first = std::clamp(spot_pos.first, 0.0, 255.0);
      spot_pos.second = std::clamp(spot_pos.second, 0.0, 255.0);
      has_changed = true;
    }

    /// Buttons ///
    bool pressed = bool(line_reader.get_value(black_button));
    if(pressed ^ black_pressed)
      has_changed = true;
    black_pressed = pressed;

    pressed = bool(line_reader.get_value(white_button));
    if(pressed && !white_pressed){
      has_changed = true;
      white_state = (white_state + 1)%9;
    }
    white_pressed = pressed;

    /// decoder ///
    uint8_t new_realpos = read_decoder_realpos(line_reader);
    uint8_t seen_travel = std::abs(int(new_realpos) - int(decoder_realpos));

    // CCW
    if(seen_travel < 50 && new_realpos < decoder_realpos)
      clamp_decoder(decoder_pos, -seen_travel);
    if(seen_travel >= 50 && new_realpos > decoder_realpos)
      clamp_decoder(decoder_pos, seen_travel - 128);

    // CW
    if(seen_travel < 50 && new_realpos > decoder_realpos)
      clamp_decoder(decoder_pos, seen_travel);
    if(seen_travel >= 50 && new_realpos < decoder_realpos)
      clamp_decoder(decoder_pos, 128 - seen_travel);

    decoder_realpos = new_realpos;
    if(seen_travel)
      has_changed = true;

    /// server notification
    if(has_changed && (now - last_send > server_ratelimit)){
      postData.clear();
      std::format_to(std::back_inserter(postData), "{{"
          "\"pan\": {},"
          "\"tilt\": {},"
          "\"focus\": {},"
          "\"white_button\": {},"
          "\"black_button\": {}"
        "}}\n",
        uint8_t(spot_pos.first),
        uint8_t(spot_pos.second),
        int(decoder_pos),
        white_state,
        black_pressed
      );

      int wrote = write(socket_file_desc, postData.data(), postData.size());
      if(wrote < postData.size()){
        std::cerr << "Failed to send data, reconnecting..." << std::endl;
        close(socket_file_desc);
        std::this_thread::sleep_for(retry_timeout);
        goto connection;
      } else {
        has_changed = false;
        last_send = now;
      }
    }
  }
}