365 lines
11 KiB
Elixir
365 lines
11 KiB
Elixir
defmodule FarmbotFirmware.StubTransport do
|
|
@moduledoc "Stub for transporting GCODES. Simulates the _real_ Firmware."
|
|
use GenServer
|
|
|
|
alias FarmbotFirmware.StubTransport, as: State
|
|
alias FarmbotFirmware.{GCODE, Param}
|
|
require Logger
|
|
|
|
defstruct status: :boot,
|
|
handle_gcode: nil,
|
|
position: [x: 0.0, y: 0.0, z: 0.0],
|
|
encoders_scaled: [x: 0.0, y: 0.0, z: 0.0],
|
|
encoders_raw: [x: 0.0, y: 0.0, z: 0.0],
|
|
pins: %{},
|
|
params: []
|
|
|
|
@type t :: %State{
|
|
status: FarmbotFirmware.status(),
|
|
handle_gcode: (FarmbotFirmware.GCODE.t() -> :ok),
|
|
position: [x: float(), y: float(), z: float()],
|
|
encoders_scaled: [x: float(), y: float(), z: float()],
|
|
encoders_raw: [x: float(), y: float(), z: float()],
|
|
pins: %{},
|
|
params: [{Param.t(), float() | nil}]
|
|
}
|
|
|
|
def init(args) do
|
|
handle_gcode = Keyword.fetch!(args, :handle_gcode)
|
|
{:ok, %State{status: :boot, handle_gcode: handle_gcode}, 0}
|
|
end
|
|
|
|
def handle_info(:timeout, %{status: :boot} = state) do
|
|
state.handle_gcode.(GCODE.new(:report_debug_message, ["ARDUINO STARTUP COMPLETE"]))
|
|
{:noreply, goto(state, :no_config), 0}
|
|
end
|
|
|
|
def handle_info(:timeout, %{status: :no_config} = state) do
|
|
state.handle_gcode.(GCODE.new(:report_no_config, []))
|
|
{:noreply, state}
|
|
end
|
|
|
|
def handle_info(:timeout, %{status: :emergency_lock} = state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_emergency_lock, [])
|
|
]
|
|
|
|
{:noreply, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_info(:timeout, %{status: :idle} = state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_encoders_scaled, state.encoders_scaled),
|
|
GCODE.new(:report_encoders_raw, state.encoders_raw),
|
|
GCODE.new(:report_idle, [])
|
|
]
|
|
|
|
{:noreply, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_emergency_lock, _}} = code, _from, state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_emergency_lock, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, %{state | status: :emergency_lock}, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_emergency_unlock, _}} = code, _from, state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, %{state | status: :idle}, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call(
|
|
{tag, {:paramater_write, [{:param_config_ok = param, 1.0 = value}]}} = code,
|
|
_from,
|
|
state
|
|
) do
|
|
new_state = %{state | params: Keyword.put(state.params, param, value)}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, goto(new_state, :idle), {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:paramater_write, [{param, value}]}} = code, _from, state) do
|
|
new_state = %{state | params: Keyword.put(state.params, param, value)}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, new_state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:paramater_read_all, []}} = code, _from, state) do
|
|
resp_codes =
|
|
[
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
Enum.map(state.params, fn {p, v} ->
|
|
GCODE.new(:report_paramater_value, [{p, v}])
|
|
end),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|> List.flatten()
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:paramater_read, [param]}} = code, _from, state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_paramater_value, [{param, state.params[param] || -1.0}]),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:position_write_zero, [:x, :y, :z]}} = code, _from, state) do
|
|
position = [
|
|
x: 0.0,
|
|
y: 0.0,
|
|
z: 0.0
|
|
]
|
|
|
|
state = %{state | position: position}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_busy, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:position_write_zero, [axis]}} = code, _from, state) do
|
|
position = Keyword.put(state.position, axis, 0.0) |> ensure_order()
|
|
state = %{state | position: position}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_movement_calibrate, [axis]}} = code, _from, state) do
|
|
position = [x: 0.0, y: 0.0, z: 0.0]
|
|
state = %{state | position: position}
|
|
param_nr_steps = :"movement_axis_nr_steps_#{axis}"
|
|
param_nr_steps_val = Keyword.get(state.params, :param_nr_steps, 10_0000.00)
|
|
|
|
param_endpoints = :"movement_invert_endpoints_#{axis}"
|
|
param_endpoints_val = Keyword.get(state.params, :param_endpoints, 1.0)
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_busy, [], tag),
|
|
GCODE.new(:report_calibration_state, [:idle]),
|
|
GCODE.new(:report_calibration_state, [:home]),
|
|
GCODE.new(:report_calibration_state, [:end]),
|
|
GCODE.new(:report_calibration_paramater_value, [{param_nr_steps, param_nr_steps_val}]),
|
|
GCODE.new(:report_calibration_paramater_value, [{param_endpoints, param_endpoints_val}]),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({_tag, {:software_version_read, _}}, _from, state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_software_version, ["8.0.0.S"])
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
# Everything under this clause should be blocked if emergency_locked
|
|
def handle_call({_tag, {_, _}} = code, _from, %{status: :emergency_lock} = state) do
|
|
Logger.error("Stub Transport emergency lock")
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_emergency_lock, [])
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:pin_read, args}} = code, _from, state) do
|
|
p = Keyword.fetch!(args, :p)
|
|
m = Keyword.get(args, :m, state.pins[p][:m] || 0)
|
|
|
|
state =
|
|
case Map.get(state.pins, p) do
|
|
nil -> %{state | pins: Map.put(state.pins, p, m: m, v: 0)}
|
|
[m: ^m, v: v] -> %{state | pins: Map.put(state.pins, p, m: m, v: v)}
|
|
_ -> state
|
|
end
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_pin_value, p: p, v: Map.get(state.pins, p)[:v]),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:pin_write, args}} = code, _from, state) do
|
|
p = Keyword.fetch!(args, :p)
|
|
m = Keyword.get(args, :m, state.pins[p][:m] || 0)
|
|
v = Keyword.fetch!(args, :v)
|
|
state = %{state | pins: Map.put(state.pins, p, m: m, v: v)}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:position_read, _}} = code, _from, state) do
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_movement, args}} = code, _from, state) do
|
|
position = [
|
|
x: args[:x] || state.position[:x],
|
|
y: args[:y] || state.position[:y],
|
|
z: args[:z] || state.position[:z]
|
|
]
|
|
|
|
state = %{state | position: position}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_busy, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_movement_home, [:x, :y, :z]}} = code, _from, state) do
|
|
position = [
|
|
x: 0.0,
|
|
y: 0.0,
|
|
z: 0.0
|
|
]
|
|
|
|
state = %{state | position: position}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_busy, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_movement_home, [axis]}} = code, _from, state) do
|
|
position = Keyword.put(state.position, axis, 0.0) |> ensure_order()
|
|
state = %{state | position: position}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_busy, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {:command_movement_find_home, [axis]}} = code, _from, state) do
|
|
position = Keyword.put(state.position, axis, 0.0) |> ensure_order()
|
|
state = %{state | position: position}
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_begin, [], tag),
|
|
GCODE.new(:report_busy, [], tag),
|
|
GCODE.new(:report_position, state.position),
|
|
GCODE.new(:report_success, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_call({tag, {_, _}} = code, _from, state) do
|
|
Logger.error("STUB HANDLER: unknown code: #{inspect(code)} for state: #{state.status}")
|
|
|
|
resp_codes = [
|
|
GCODE.new(:report_echo, [GCODE.encode(code)]),
|
|
GCODE.new(:report_invalid, [], tag)
|
|
]
|
|
|
|
{:reply, :ok, state, {:continue, resp_codes}}
|
|
end
|
|
|
|
def handle_continue([code | rest], state) do
|
|
state.handle_gcode.(code)
|
|
{:noreply, state, {:continue, rest}}
|
|
end
|
|
|
|
def handle_continue([], %{status: :idle} = state) do
|
|
{:noreply, state, 5_000}
|
|
end
|
|
|
|
def handle_continue([], %{status: _} = state) do
|
|
{:noreply, state}
|
|
end
|
|
|
|
defp goto(%{status: _old} = state, status), do: %{state | status: status}
|
|
|
|
defp ensure_order(pos) do
|
|
[
|
|
x: Keyword.fetch!(pos, :x),
|
|
y: Keyword.fetch!(pos, :y),
|
|
z: Keyword.fetch!(pos, :z)
|
|
]
|
|
end
|
|
end
|