Enabling and disabling servo drivers works.
This commit is contained in:
parent
a85f114b50
commit
776e02e71b
10 changed files with 265 additions and 24 deletions
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
#define SERVO_ENABLE_STARBOARD_PIN 3
|
||||
SemaphoreHandle_t sensorMutex = xSemaphoreCreateMutex();
|
||||
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);
|
||||
Wire1.setClock(400000L);
|
||||
SENSOR_I2C_BUS.setClock(400000L);
|
||||
SERVO_I2C_BUS.setClock(400000L);
|
||||
|
||||
SemaphoreHandle_t i2c0Mutex = xSemaphoreCreateMutex();
|
||||
xSemaphoreGive(i2c0Mutex);
|
||||
xSemaphoreGive(sensorMutex);
|
||||
xSemaphoreGive(servoMutex);
|
||||
|
||||
BlinkerTask blinkerTask(LED_BUILTIN);
|
||||
FirmataTask firmataTask;
|
||||
GpsTask gpsTask(firmataTask);
|
||||
ImuTask imuTask(i2c0Mutex, firmataTask);
|
||||
PowerTask powerTask(i2c0Mutex, firmataTask);
|
||||
Firmata.attach(START_SYSEX, sysexCallback);
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
|
|
64
firmware/src/tasks/servo_driver.cpp
Normal file
64
firmware/src/tasks/servo_driver.cpp
Normal file
|
@ -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();
|
||||
}
|
56
firmware/src/tasks/servo_driver.hpp
Normal file
56
firmware/src/tasks/servo_driver.hpp
Normal file
|
@ -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__
|
|
@ -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
|
||||
]
|
||||
|
||||
|
|
|
@ -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} ->
|
||||
PubSub.broadcast(Augie.PubSub, name, message)
|
||||
{:ok, {:telemetry, 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(
|
20
webapp/lib/augie/firmata/commands.ex
Normal file
20
webapp/lib/augie/firmata/commands.ex
Normal file
|
@ -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
|
|
@ -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}
|
|
@ -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,
|
||||
assign(socket, connected: SerialTelemetry.connected?(), uarts: Circuits.UART.enumerate())}
|
||||
{:ok, assign(socket, connected: Firmata.connected?(), uarts: Circuits.UART.enumerate())}
|
||||
end
|
||||
|
||||
def handle_info({:connected, false}, socket) do
|
||||
|
|
Reference in a new issue