Enabling and disabling servo drivers works.

firmware/Dockerfile.template

@@ -1,4 +1,5 @@
 FROM balenalib/%%BALENA_MACHINE_NAME%%-debian-python:latest AS firmware-builder
+RUN install_packages libusb-dev git build-essential
 RUN pip install -U platformio
 RUN mkdir /firmware
 WORKDIR /firmware

firmware/src/firmata_commands.hpp

@@ -4,7 +4,10 @@
 enum FirmataCommands
 {
   TELEMETRY = 0x01,
-  LOG
+  SERVO_SET_ENABLE_PORT,
+  SERVO_SET_ENABLE_STARBOARD,
+  SERVO_GET_ENABLE_PORT,
+  SERVO_GET_ENABLE_STARBOARD
 };
 
 enum FirmataTelemetryType

firmware/src/main.cpp

@@ -10,37 +10,102 @@ extern "C"
   int __exidx_end() { return -1; }
 }
 
+#ifndef SENSOR_I2C_BUS
+#define SENSOR_I2C_BUS Wire
+#endif
+
+#ifndef SERVO_I2C_BUS
+#define SERVO_I2C_BUS Wire1
+#endif
+
+#ifndef SERVO_ENABLE_PORT_PIN
+#define SERVO_ENABLE_PORT_PIN 2
+#endif
+
+#ifndef SERVO_ADDRESS_PORT
+#define SERVO_ADDRESS_PORT 0x41
+#endif
+
+#ifndef SERVO_ENABLE_STARBOARD_PIN
+#define SERVO_ENABLE_STARBOARD_PIN 3
+#endif
+
+#ifndef SERVO_ADDRESS_STARBOARD
+#define SERVO_ADDRESS_STARBOARD 0x42
+#endif
+
 #include "ticks.hpp"
 #include "tasks/blinker_task.hpp"
 #include "tasks/firmata_task.hpp"
 #include "tasks/gps_task.hpp"
 #include "tasks/imu_task.hpp"
 #include "tasks/power_task.hpp"
+#include "tasks/servo_driver.hpp"
+#include "firmata_commands.hpp"
 
-#define SERVO_ENABLE_PORT_PIN 2
+SemaphoreHandle_t sensorMutex = xSemaphoreCreateMutex();
-#define SERVO_ENABLE_STARBOARD_PIN 3
+SemaphoreHandle_t servoMutex = xSemaphoreCreateMutex();
+
+BlinkerTask blinkerTask(LED_BUILTIN);
+FirmataTask firmataTask;
+GpsTask gpsTask(firmataTask);
+ImuTask imuTask(sensorMutex, firmataTask);
+PowerTask powerTask(sensorMutex, firmataTask);
+ServoDriverTask portServos(SERVO_ENABLE_PORT_PIN, SERVO_ADDRESS_PORT, servoMutex);
+ServoDriverTask starboardServos(SERVO_ENABLE_STARBOARD_PIN, SERVO_ADDRESS_STARBOARD, servoMutex);
+
+void sysexCallback(byte command, byte bytec, byte *payload)
+{
+  byte responseBuffer[1];
+
+  switch (command)
+  {
+  case FirmataCommands::SERVO_SET_ENABLE_PORT:
+    if (bytec > 0 && payload[0] > 0)
+      portServos.enable(true);
+    else
+      portServos.enable(false);
+    break;
+
+  case FirmataCommands::SERVO_SET_ENABLE_STARBOARD:
+    if (bytec > 0 && payload[0] > 0)
+      starboardServos.enable(true);
+    else
+      starboardServos.enable(false);
+    break;
+
+  case FirmataCommands::SERVO_GET_ENABLE_PORT:
+    responseBuffer[0] = (byte)portServos.isEnabled();
+    Firmata.sendSysex(FirmataCommands::SERVO_GET_ENABLE_PORT, 1, (byte *)&responseBuffer);
+    break;
+
+  case FirmataCommands::SERVO_GET_ENABLE_STARBOARD:
+    responseBuffer[0] = (byte)starboardServos.isEnabled();
+    Firmata.sendSysex(FirmataCommands::SERVO_GET_ENABLE_STARBOARD, 1, (byte *)&responseBuffer);
+    break;
+  }
+}
 
 void setup()
 {
   pinMode(LED_BUILTIN, OUTPUT);
   Serial.begin(115200);
-  Wire.setClock(400000L);
+  SENSOR_I2C_BUS.setClock(400000L);
-  Wire1.setClock(400000L);
+  SERVO_I2C_BUS.setClock(400000L);
 
-  SemaphoreHandle_t i2c0Mutex = xSemaphoreCreateMutex();
+  xSemaphoreGive(sensorMutex);
-  xSemaphoreGive(i2c0Mutex);
+  xSemaphoreGive(servoMutex);
 
-  BlinkerTask blinkerTask(LED_BUILTIN);
+  Firmata.attach(START_SYSEX, sysexCallback);
-  FirmataTask firmataTask;
-  GpsTask gpsTask(firmataTask);
-  ImuTask imuTask(i2c0Mutex, firmataTask);
-  PowerTask powerTask(i2c0Mutex, firmataTask);
 
   xTaskCreate(BlinkerTask::startTask, "Blinker", 2048, (void *)&blinkerTask, 1, NULL);
   xTaskCreate(FirmataTask::startTask, "Firmata", 2048, (void *)&firmataTask, 2, NULL);
   xTaskCreate(GpsTask::startTask, "GPS", 2048, (void *)&gpsTask, 3, NULL);
   xTaskCreate(PowerTask::startTask, "Power", 2048, (void *)&powerTask, 4, NULL);
   xTaskCreate(ImuTask::startTask, "IMU", 2048, (void *)&imuTask, 5, NULL);
+  xTaskCreate(ServoDriverTask::startTask, "Port Servos", 2048, (void *)&portServos, 6, NULL);
+  xTaskCreate(ServoDriverTask::startTask, "Starboard Servos", 2048, (void *)&starboardServos, 6, NULL);
+
   vTaskStartScheduler();
 }
 

firmware/src/tasks/servo_driver.cpp

@@ -0,0 +1,64 @@
+#include "servo_driver.hpp"
+#include "ticks.hpp"
+
+ServoDriverTask::ServoDriverTask(const uint8_t enablePin, const uint8_t busAddress, SemaphoreHandle_t &busMutex) : enablePin(enablePin), busMutex(busMutex)
+{
+  this->pwmDriver = Adafruit_PWMServoDriver(busAddress, SERVO_I2C_BUS);
+  pinMode(this->enablePin, OUTPUT);
+  digitalWrite(this->enablePin, SERVO_DRIVER_DISABLED);
+  this->enablePinState = false;
+}
+
+void ServoDriverTask::enable(bool enable)
+{
+  if (enable)
+  {
+    digitalWrite(this->enablePin, SERVO_DRIVER_ENABLED);
+    this->enablePinState = true;
+  }
+  else
+  {
+    digitalWrite(this->enablePin, SERVO_DRIVER_DISABLED);
+    this->enablePinState = false;
+  }
+}
+
+bool ServoDriverTask::isEnabled()
+{
+  return this->enablePinState;
+}
+
+void ServoDriverTask::init()
+{
+  TickType_t delay = Ticks::fromMillis(100L);
+  while (1)
+  {
+    if (xSemaphoreTake(this->busMutex, delay) == pdTRUE)
+    {
+      this->pwmDriver.begin();
+      this->pwmDriver.setOscillatorFrequency(27000000);
+      this->pwmDriver.setPWMFreq(SERVO_FREQ);
+      xSemaphoreGive(this->busMutex);
+      break;
+    }
+    vTaskDelay(delay);
+  }
+}
+
+void ServoDriverTask::run()
+{
+  TickType_t delay = Ticks::fromMillis(100L);
+
+  while (1)
+  {
+    vTaskDelay(delay);
+  }
+}
+
+void ServoDriverTask::startTask(void *arg)
+{
+  ServoDriverTask *servoTask = (ServoDriverTask *)arg;
+  servoTask->init();
+  vTaskDelay(Ticks::fromMillis(100L));
+  servoTask->run();
+}

firmware/src/tasks/servo_driver.hpp

@@ -0,0 +1,56 @@
+#ifndef __SERVO_DRIVER_TASK_HPP__
+#define __SERVO_DRIVER_TASK_HPP__
+
+#ifndef SERVO_I2C_BUS
+#define SERVO_I2C_BUS Wire1
+#endif
+
+#ifndef SERVO_FREQ
+#define SERVO_FREQ 50
+#endif
+
+#ifndef SERVO_MIN
+#define SERVO_MIN 150
+#endif
+
+#ifndef SERVO_MAX
+#define SERVO_MAX 600
+#endif
+
+#ifndef SERVO_USMIN
+#define SERVO_USMIN 600
+#endif
+
+#ifndef SERVO_USMAX
+#define SERVO_USMAX 2400
+#endif
+
+#define SERVO_DRIVER_ENABLED LOW
+#define SERVO_DRIVER_DISABLED HIGH
+
+#include <FreeRTOS_TEENSY4.h>
+#include <Arduino.h>
+#include <Adafruit_PWMServoDriver.h>
+#include "task.hpp"
+#include "firmata_task.hpp"
+#include "firmata_commands.hpp"
+
+class ServoDriverTask : public Task
+{
+public:
+  ServoDriverTask(const uint8_t enablePin, const uint8_t busAddress, SemaphoreHandle_t &busMutex);
+  void run();
+  void init();
+  static void startTask(void *arg);
+
+  void enable(bool enable);
+  bool isEnabled();
+
+private:
+  uint8_t enablePin;
+  SemaphoreHandle_t &busMutex;
+  Adafruit_PWMServoDriver pwmDriver;
+  bool enablePinState;
+};
+
+#endif // __SERVO_DRIVER_TASK_HPP__

webapp/lib/augie/application.ex

@@ -16,7 +16,7 @@ defmodule Augie.Application do
       # Start a worker by calling: Augie.Worker.start_link(arg)
       # {Augie.Worker, arg}
       {Circuits.UART, [name: Circuits.UART]},
-      Augie.SerialTelemetry,
+      Augie.Firmata,
       Augie.Sensor.Camera
     ]
 

webapp/lib/augie/firmata.ex

@@ -1,6 +1,6 @@
-defmodule Augie.SerialTelemetry do
+defmodule Augie.Firmata do
   use GenServer
-  alias Augie.SerialTelemetry.Decoder
+  alias Augie.Firmata.Decoder
   alias Circuits.UART
   alias MidiProto.Parser
   alias Phoenix.PubSub
@@ -17,6 +17,7 @@ defmodule Augie.SerialTelemetry do
   def start_link(_opts), do: GenServer.start_link(__MODULE__, [], name: __MODULE__)
 
   def connected?, do: GenServer.call(__MODULE__, :connected?)
+  def send(binary), do: GenServer.call(__MODULE__, {:send, binary})
 
   @impl true
   def init(_) do
@@ -29,6 +30,19 @@ defmodule Augie.SerialTelemetry do
   def handle_call(:connected?, _from, %{uart: nil} = state), do: {:reply, false, state}
   def handle_call(:connected?, _from, state), do: {:reply, true, state}
 
+  def handle_call({:send, _binary}, _from, %{uart: nil} = state),
+    do: {:reply, {:error, "Not connected"}, state}
+
+  def handle_call({:send, binary}, _from, state) do
+    result =
+      case UART.write(Circuits.UART, binary) do
+        :ok -> {:ok, binary}
+        {:error, reason} -> {:error, reason}
+      end
+
+    {:reply, result, state}
+  end
+
   @impl true
   def handle_info({:circuits_uart, _dev, message}, %{parser: parser} = state) do
     parser = Parser.append(parser, message)
@@ -40,8 +54,11 @@ defmodule Augie.SerialTelemetry do
       {:ok, messages, parser} ->
         for message <- messages do
           case Decoder.decode(message) do
-            {:ok, name, message} ->
+            {:ok, {:telemetry, name}, message} ->
-              PubSub.broadcast(Augie.PubSub, name, message)
+              PubSub.broadcast(Augie.PubSub, "telemetry.#{name}", message)
+
+            {:ok, {:command, name}, message} ->
+              IO.inspect({:command, name, message}, logger: "FIRMATA COMMAND")
 
             {:error, reason} ->
               Logger.warn(

webapp/lib/augie/firmata/commands.ex

@@ -0,0 +1,20 @@
+defmodule Augie.Firmata.Commands do
+  alias Augie.Firmata
+  alias MidiProto.Message
+  alias MidiProto.Message.SystemExclusive
+
+  def servo_set_enable(:port, true), do: send_sysex(2, <<1, 0>>)
+  def servo_set_enable(:port, false), do: send_sysex(2, <<0, 0>>)
+  def servo_set_enable(:starboard, true), do: send_sysex(3, <<1, 0>>)
+  def servo_set_enable(:starboard, false), do: send_sysex(3, <<0, 0>>)
+
+  def servo_get_enable(:port), do: send_sysex(4, <<>>)
+  def servo_get_enable(:starboard), do: send_sysex(5, <<>>)
+
+  defp send_sysex(command, payload) do
+    command
+    |> SystemExclusive.init(payload)
+    |> Message.encode()
+    |> Firmata.send()
+  end
+end

webapp/lib/augie/firmata/decoder.ex

@@ -1,4 +1,4 @@
-defmodule Augie.SerialTelemetry.Decoder do
+defmodule Augie.Firmata.Decoder do
   alias Augie.Sensor.{GPS, IMU, Power}
   alias MidiProto.Message.SystemExclusive
   use Bitwise
@@ -14,7 +14,7 @@ defmodule Augie.SerialTelemetry.Decoder do
             current::little-float-size(32),
             power::little-float-size(32)>>} <- depad_binary(<<>>, payload),
          {:ok, update} <- Power.build([bus_voltage, shunt_voltage, current, power]) do
-      {:ok, "telemetry.power", update}
+      {:ok, {:telemetry, :power}, update}
     end
   end
 
@@ -24,7 +24,7 @@ defmodule Augie.SerialTelemetry.Decoder do
             ow::little-float-size(64),
             temp::little-signed-integer-size(8)>>} <- depad_binary(<<>>, payload),
          {:ok, update} <- IMU.build([ox, oy, oz, ow, temp]) do
-      {:ok, "telemetry.imu", update}
+      {:ok, {:telemetry, :imu}, update}
     end
   end
 
@@ -36,10 +36,26 @@ defmodule Augie.SerialTelemetry.Decoder do
             status::little-integer-size(8)>>} <- depad_binary(<<>>, payload),
          {:ok, update} <-
            GPS.build([latitude, longitude, altitude, heading, speed, satellites, status]) do
-      {:ok, "telemetry.gps", update}
+      {:ok, {:telemetry, :gps}, update}
     end
   end
 
+  def decode(%SystemExclusive{vendor_id: 2, payload: binary}) do
+    {:ok, {:command, :servo_set_enable_port}, binary}
+  end
+
+  def decode(%SystemExclusive{vendor_id: 3, payload: binary}) do
+    {:ok, {:command, :servo_set_enable_starboard}, binary}
+  end
+
+  def decode(%SystemExclusive{vendor_id: 4, payload: binary}) do
+    {:ok, {:command, :servo_get_enable_port}, binary}
+  end
+
+  def decode(%SystemExclusive{vendor_id: 5, payload: binary}) do
+    {:ok, {:command, :servo_get_enable_starboard}, binary}
+  end
+
   def decode(_), do: {:error, "Unknown message"}
 
   defp depad_binary(result, ""), do: {:ok, result}

webapp/lib/augie_web/live/dashboard_live.ex

@@ -1,6 +1,6 @@
 defmodule AugieWeb.DashboardLive do
   use Phoenix.LiveView, layout: {AugieWeb.LayoutView, "live.html"}
-  alias Augie.SerialTelemetry
+  alias Augie.Firmata
   alias Phoenix.PubSub
 
   @moduledoc """
@@ -11,8 +11,7 @@ defmodule AugieWeb.DashboardLive do
   def mount(_params, _context, socket) do
     if connected?(socket), do: PubSub.subscribe(Augie.PubSub, "serial_telemetry")
 
-    {:ok,
+    {:ok, assign(socket, connected: Firmata.connected?(), uarts: Circuits.UART.enumerate())}
-     assign(socket, connected: SerialTelemetry.connected?(), uarts: Circuits.UART.enumerate())}
   end
 
   def handle_info({:connected, false}, socket) do