Skip to content

armgit / moss

Public Imported from GitHub
0

MOSS: a small, mostly 3D-printed litter-picking rover. Cleaner streets, one maker at a time.

Python
3.5 KB
Raw Blame
#pragma once
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

namespace moss {
constexpr int MAX_PERCENT = 25;  // Commissioning limit; raise only after mechanical tests.
constexpr uint32_t COMMAND_TIMEOUT_MS = 300;
constexpr uint32_t ARM_TIMEOUT_MS = 10000;
constexpr uint32_t TEST_MS = 1000;

enum Mode { OFF, READY, DRIVE, TEST };
enum Reply { OK, STATUS, ZERO, INVALID, NOT_ARMED, BUSY };

struct Control {
  Mode mode = OFF;
  int left = 0, right = 0;
  uint32_t since = 0;
  const char* reason = "boot";
  void stop(const char* why) {
    mode = OFF; left = right = 0; reason = why;
  }
  bool poll(uint32_t now) {
    uint32_t age = now - since;  // Defined across millis() rollover.
    if (mode == DRIVE && age >= COMMAND_TIMEOUT_MS) { stop("timeout"); return true; }
    if (mode == READY && age >= ARM_TIMEOUT_MS) { stop("arm_expired"); return true; }
    if (mode == TEST && age >= TEST_MS) { stop("test_done"); return true; }
    return false;
  }
  static bool number(const char* s, int& out) {
    if (!s || !*s) return false;
    errno = 0; char* end;
    long n = strtol(s, &end, 10);
    if (errno || *end || n < -MAX_PERCENT || n > MAX_PERCENT) return false;
    out = static_cast<int>(n); return true;
  }
  Reply command(char* line, uint32_t now) {
    poll(now);
    char* argv[4]; int argc = 0; char* save = nullptr;
    for (char* t = strtok_r(line, " \t", &save); t; t = strtok_r(nullptr, " \t", &save)) {
      if (argc == 4) { stop("invalid"); return INVALID; }
      argv[argc++] = t;
    }
    if (!argc) return OK;
    if (argc == 1 && !strcmp(argv[0], "stop")) { stop("stop"); return OK; }
    if (argc == 1 && !strcmp(argv[0], "status")) return STATUS;
    if (argc == 1 && !strcmp(argv[0], "zero")) {
      if (mode != OFF) { stop("zero_requires_stop"); return BUSY; }
      return ZERO;
    }
    if (argc == 1 && !strcmp(argv[0], "arm")) {
      if (mode == DRIVE || mode == TEST) { stop("arm_while_moving"); return BUSY; }
      mode = READY; left = right = 0; since = now; reason = "armed"; return OK;
    }
    int l, r;
    if (argc == 3 && !strcmp(argv[0], "drive") && number(argv[1], l) && number(argv[2], r)) {
      if (mode != READY && mode != DRIVE) { stop("not_armed"); return NOT_ARMED; }
      left = l; right = r; mode = DRIVE; since = now; reason = "drive"; return OK;
    }
    if (argc == 3 && !strcmp(argv[0], "test") && number(argv[2], l) &&
        (!strcmp(argv[1], "left") || !strcmp(argv[1], "right"))) {
      if (mode != OFF) { stop("test_requires_stop"); return BUSY; }
      left = !strcmp(argv[1], "left") ? l : 0;
      right = !strcmp(argv[1], "right") ? l : 0;
      mode = TEST; since = now; reason = "test"; return OK;
    }
    stop("invalid"); return INVALID;
  }
};

// Sign changes first ramp to zero, then wait before changing DIR.
struct Ramp {
  int value = 0;
  int sign = 1;
  uint32_t zeroSince = 0;
  void stop(uint32_t now) { value = 0; zeroSince = now; }
  int tick(int target, uint32_t now) {
    if (!target) { if (value) stop(now); return 0; }
    int desiredSign = target < 0 ? -1 : target > 0 ? 1 : sign;
    int magnitude = target < 0 ? -target : target;
    if (desiredSign != sign) {
      if (value > 0) {
        --value;
        if (!value) zeroSince = now;
        return sign * value;
      }
      if (uint32_t(now - zeroSince) < 100) return 0;
      sign = desiredSign;
    }
    if (value < magnitude) ++value;
    else if (value > magnitude) { --value; if (!value) zeroSince = now; }
    return sign * value;
  }
};
}