intial commit

src/main.cpp

@@ -0,0 +1,14 @@
+#include <Arduino.h>
+#include <mp32.h>
+#include <system/flysky.h>
+#include <system/holomonic.h>
+
+void setup() {
+  flysky::begin(A0, flysky::Mode::ppm); }
+
+void loop() {
+  if (!flysky::read()) {
+    ao();
+    return; }
+
+  holonomic::drive(flysky::ch(1), flysky::ch(2), flysky::ch(4)); }

src/system/flysky.cpp

@@ -0,0 +1,115 @@
+#include "flysky.h"
+
+#include <PPMReader.h>
+
+namespace flysky {
+
+static uint8_t ppmPin = 0;
+static Mode readMode = Mode::autoDetect;
+static int channelValues[kChannelCount] = {0 };
+constexpr unsigned long kPulseTimeoutUs = 30000;
+constexpr int kValidPulseMin = 750;
+constexpr int kValidPulseMax = 2250;
+static PPMReader* ppmReader = nullptr;
+
+static bool isValidPulse(unsigned long width) {
+  return width >= kValidPulseMin && width <= kValidPulseMax; }
+
+static bool readPpmFrame() {
+  if (ppmReader == nullptr) return false;
+
+  bool hasSignal = false;
+
+  for (uint8_t i = 0; i < kChannelCount; ++i) {
+    const unsigned value = ppmReader->latestValidChannelValue(i + 1, 0);
+
+    if (value == 0) {
+      channelValues[i] = kChannelMid;
+      continue; }
+
+    channelValues[i] = value;
+    hasSignal = true; }
+
+  return hasSignal; }
+
+static bool readSinglePwmChannel() {
+  const unsigned long width = pulseIn(ppmPin, HIGH, kPulseTimeoutUs);
+
+  if (!isValidPulse(width)) return false;
+
+  channelValues[0] = width;
+
+  for (uint8_t i = 1; i < kChannelCount; ++i) {
+    channelValues[i] = kChannelMid; }
+
+  return true; }
+
+void begin(uint8_t pin) {
+  begin(pin, Mode::autoDetect); }
+
+void begin(uint8_t pin, Mode mode) {
+  if (ppmReader != nullptr) {
+    delete ppmReader;
+    ppmReader = nullptr; }
+
+  ppmPin = pin;
+  readMode = mode;
+  pinMode(ppmPin, INPUT);
+
+  for (uint8_t i = 0; i < kChannelCount; ++i) {
+    channelValues[i] = kChannelMid; }
+
+  if (mode != Mode::pwm) {
+    ppmReader = new PPMReader(ppmPin, kChannelCount);
+    ppmReader->minChannelValue = kChannelMin;
+    ppmReader->maxChannelValue = kChannelMax;
+    ppmReader->blankTime = 3000; } }
+
+bool read() {
+  switch (readMode) {
+  case Mode::ppm:
+    return readPpmFrame();
+
+  case Mode::pwm:
+    return readSinglePwmChannel();
+
+  case Mode::autoDetect:
+    return readPpmFrame() || readSinglePwmChannel(); }
+
+  return false; }
+
+int channel(uint8_t index) {
+  if (index >= kChannelCount) return 0;
+
+  return channelValues[index]; }
+
+int channelPercent(uint8_t index) {
+  const int value = channel(index);
+
+  if (value <= kChannelMin) return -100;
+
+  if (value >= kChannelMax) return 100;
+
+  return map(value, kChannelMin, kChannelMax, -100, 100); }
+
+int channelNumber(uint8_t number) {
+  if (number == 0 || number > kChannelCount) return 0;
+
+  return channel(number - 1); }
+
+int channelPercentNumber(uint8_t number) {
+  if (number == 0 || number > kChannelCount) return 0;
+
+  return channelPercent(number - 1); }
+
+int ch(uint8_t number) {
+  const int value = channelPercentNumber(number);
+
+  if(abs(value) < 5) return 0;
+
+  return value; }
+
+const int* channels() {
+  return channelValues; }
+
+}

src/system/flysky.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <Arduino.h>
+
+namespace flysky {
+
+constexpr uint8_t kChannelCount = 8;
+constexpr int kChannelMin = 1000;
+constexpr int kChannelMid = 1500;
+constexpr int kChannelMax = 2000;
+
+enum class Mode {
+  autoDetect,
+  ppm,
+  pwm, };
+
+void begin(uint8_t pin);
+void begin(uint8_t pin, Mode mode);
+bool read();
+int channel(uint8_t index);
+int channelPercent(uint8_t index);
+int channelNumber(uint8_t number);
+int channelPercentNumber(uint8_t number);
+int ch(uint8_t index);
+const int *channels(); }

src/system/holomonic.cpp

@@ -0,0 +1,27 @@
+#include <mp32.h>
+#include "holomonic.h"
+
+#define degToRad 0.0174f
+#define sin30 sin(30.f * degToRad)
+#define cos30 cos(30.f * degToRad)
+
+namespace holonomic {
+
+void wheel(int s1, int s2, int s3) {
+  m(1, s1);
+  m(2, s2);
+  m(3, s3); }
+
+void polar(float spd, float theta, float omega) {
+  const float thetaRad = theta * degToRad;
+  const float forwardBackward = spd * cos(thetaRad);
+  const float strafe = spd * sin(thetaRad);
+  drive(forwardBackward, strafe, omega); }
+
+void drive(float forwardBackward, float strafe, float rotation) {
+  const float spd1 = strafe * cos30 - forwardBackward * sin30 + rotation;
+  const float spd2 = -strafe * cos30 - forwardBackward * sin30 + rotation;
+  const float spd3 = forwardBackward + rotation;
+  wheel(spd1, spd2, spd3 * 0.9f); }
+
+}

src/system/holomonic.h

@@ -0,0 +1,9 @@
+#pragma once
+
+namespace holonomic {
+
+void wheel(int s1, int s2, int s3);
+void polar(float spd, float theta, float omega);
+void drive(float forwardBackward, float strafe, float rotation = 0);
+
+}

src/system/line.cpp

@@ -0,0 +1,7 @@
+#include "line.h"
+
+namespace line {
+
+int line() {
+  // Implementation for line tracking
+  return 0; } }

src/system/line.h

@@ -0,0 +1,5 @@
+#pragma once
+
+namespace line {
+
+}

src/system/tracker.cpp

@@ -0,0 +1,7 @@
+#include "tracker.h"
+
+namespace tracker {
+
+int line() {
+  // Implementation for line tracking
+  return 0; } }

src/system/tracker.h

@@ -0,0 +1,5 @@
+#pragma once
+
+namespace tracker {
+
+}