192 lines
6.5 KiB
Elixir
192 lines
6.5 KiB
Elixir
defmodule FarmbotFirmware.GCODE.Encoder do
|
|
@moduledoc false
|
|
|
|
alias FarmbotFirmware.{GCODE, Param}
|
|
|
|
@doc false
|
|
@spec do_encode(GCODE.kind(), GCODE.args()) :: binary()
|
|
def do_encode(:report_idle, []), do: "R00"
|
|
def do_encode(:report_begin, []), do: "R01"
|
|
def do_encode(:report_success, []), do: "R02"
|
|
def do_encode(:report_error, []), do: "R03"
|
|
def do_encode(:report_error, error), do: "R03 " <> encode_error(error)
|
|
def do_encode(:report_busy, []), do: "R04"
|
|
|
|
def do_encode(:report_axis_state, xyz), do: "R05 " <> encode_axis_state(xyz)
|
|
|
|
def do_encode(:report_calibration_state, xyz),
|
|
do: "R06 " <> encode_calibration_state(xyz)
|
|
|
|
def do_encode(:report_retry, []), do: "R07"
|
|
def do_encode(:report_echo, [echo]), do: "R08 * #{echo} *"
|
|
def do_encode(:report_invalid, []), do: "R09"
|
|
|
|
def do_encode(:report_home_complete, [:x]), do: "R11"
|
|
def do_encode(:report_home_complete, [:y]), do: "R12"
|
|
def do_encode(:report_home_complete, [:z]), do: "R13"
|
|
|
|
def do_encode(:report_position_change, [x: _] = arg),
|
|
do: "R15 " <> encode_floats(arg)
|
|
|
|
def do_encode(:report_position_change, [y: _] = arg),
|
|
do: "R16 " <> encode_floats(arg)
|
|
|
|
def do_encode(:report_position_change, [z: _] = arg),
|
|
do: "R16 " <> encode_floats(arg)
|
|
|
|
def do_encode(:report_parameters_complete, []), do: "R20"
|
|
|
|
def do_encode(:report_parameter_value, pv), do: "R21 " <> encode_pv(pv)
|
|
|
|
def do_encode(:report_calibration_parameter_value, pv),
|
|
do: "R23 " <> encode_pv(pv)
|
|
|
|
def do_encode(:report_pin_value, pv), do: "R41 " <> encode_ints(pv)
|
|
|
|
def do_encode(:report_axis_timeout, [:x]), do: "R71"
|
|
def do_encode(:report_axis_timeout, [:y]), do: "R72"
|
|
def do_encode(:report_axis_timeout, [:z]), do: "R73"
|
|
|
|
def do_encode(:report_end_stops, xxyyzz),
|
|
do: "R81 " <> encode_end_stops(xxyyzz)
|
|
|
|
def do_encode(:report_position, xyzs), do: "R82 " <> encode_floats(xyzs)
|
|
|
|
def do_encode(:report_software_version, [version]), do: "R83 " <> version
|
|
|
|
def do_encode(:report_encoders_scaled, xyz), do: "R84 " <> encode_floats(xyz)
|
|
def do_encode(:report_encoders_raw, xyz), do: "R85 " <> encode_floats(xyz)
|
|
|
|
def do_encode(:report_emergency_lock, []), do: "R87"
|
|
def do_encode(:report_no_config, []), do: "R88"
|
|
def do_encode(:report_load, uxvywz), do: "R89 " <> encode_uxvywz(uxvywz)
|
|
def do_encode(:report_debug_message, [message]), do: "R99 " <> message
|
|
|
|
def do_encode(:command_movement, xyzs), do: "G00 " <> encode_floats(xyzs)
|
|
def do_encode(:command_movement_home, [:x, :y, :z]), do: "G28"
|
|
|
|
def do_encode(:command_movement_home, [:x]),
|
|
do: "G00 " <> encode_floats(x: 0.0)
|
|
|
|
def do_encode(:command_movement_home, [:y]),
|
|
do: "G00 " <> encode_floats(y: 0.0)
|
|
|
|
def do_encode(:command_movement_home, [:z]),
|
|
do: "G00 " <> encode_floats(z: 0.0)
|
|
|
|
def do_encode(:command_movement_find_home, [:x]), do: "F11"
|
|
def do_encode(:command_movement_find_home, [:y]), do: "F12"
|
|
def do_encode(:command_movement_find_home, [:z]), do: "F13"
|
|
|
|
def do_encode(:command_movement_calibrate, [:x]), do: "F14"
|
|
def do_encode(:command_movement_calibrate, [:y]), do: "F15"
|
|
def do_encode(:command_movement_calibrate, [:z]), do: "F16"
|
|
|
|
def do_encode(:parameter_read_all, []), do: "F20"
|
|
|
|
def do_encode(:parameter_read, [parameter]),
|
|
do: "F21 P#{Param.encode(parameter)}"
|
|
|
|
def do_encode(:parameter_write, pv), do: "F22 " <> encode_pv(pv)
|
|
def do_encode(:calibration_parameter_write, pv), do: "F23 " <> encode_pv(pv)
|
|
|
|
def do_encode(:pin_write, pv), do: "F41 " <> encode_ints(pv)
|
|
def do_encode(:pin_read, p), do: "F42 " <> encode_ints(p)
|
|
def do_encode(:pin_mode_write, pm), do: "F43 " <> encode_ints(pm)
|
|
|
|
def do_encode(:servo_write, pv), do: "F61 " <> encode_ints(pv)
|
|
def do_encode(:end_stops_read, []), do: "F81"
|
|
def do_encode(:position_read, []), do: "F82"
|
|
def do_encode(:software_version_read, []), do: "F83"
|
|
def do_encode(:position_write_zero, [:x, :y, :z]), do: "F84 X1 Y1 Z1"
|
|
def do_encode(:position_write_zero, [:x]), do: "F84 X1"
|
|
def do_encode(:position_write_zero, [:y]), do: "F84 Y1"
|
|
def do_encode(:position_write_zero, [:z]), do: "F84 Z1"
|
|
|
|
def do_encode(:command_emergency_unlock, _), do: "F09"
|
|
def do_encode(:command_emergency_lock, _), do: "E"
|
|
def do_encode(), do: "R03"
|
|
|
|
@spec encode_floats([{Param.t(), float()}]) :: binary()
|
|
defp encode_floats(args) do
|
|
Enum.map(args, fn {param, value} ->
|
|
binary_float = :erlang.float_to_binary(value, decimals: 2)
|
|
String.upcase(to_string(param)) <> binary_float
|
|
end)
|
|
|> Enum.join(" ")
|
|
end
|
|
|
|
defp encode_axis_state([{axis, :idle}]),
|
|
do: String.upcase(to_string(axis)) <> "0"
|
|
|
|
defp encode_axis_state([{axis, :begin}]),
|
|
do: String.upcase(to_string(axis)) <> "1"
|
|
|
|
defp encode_axis_state([{axis, :accelerate}]),
|
|
do: String.upcase(to_string(axis)) <> "2"
|
|
|
|
defp encode_axis_state([{axis, :cruise}]),
|
|
do: String.upcase(to_string(axis)) <> "3"
|
|
|
|
defp encode_axis_state([{axis, :decelerate}]),
|
|
do: String.upcase(to_string(axis)) <> "4"
|
|
|
|
defp encode_axis_state([{axis, :stop}]),
|
|
do: String.upcase(to_string(axis)) <> "5"
|
|
|
|
defp encode_axis_state([{axis, :crawl}]),
|
|
do: String.upcase(to_string(axis)) <> "6"
|
|
|
|
defp encode_calibration_state([{axis, :idle}]),
|
|
do: String.upcase(to_string(axis)) <> "0"
|
|
|
|
defp encode_calibration_state([{axis, :home}]),
|
|
do: String.upcase(to_string(axis)) <> "1"
|
|
|
|
defp encode_calibration_state([{axis, :end}]),
|
|
do: String.upcase(to_string(axis)) <> "2"
|
|
|
|
defp encode_end_stops(xa: xa, xb: xb, ya: ya, yb: yb, za: za, zb: zb) do
|
|
"XA#{xa} XB#{xb} YA#{ya} YB#{yb} ZA#{za} ZB#{zb}"
|
|
end
|
|
|
|
defp encode_pv([{param, value}]) do
|
|
param_id = Param.encode(param)
|
|
binary_float = :erlang.float_to_binary(value, decimals: 2)
|
|
"P#{param_id} V#{binary_float}"
|
|
end
|
|
|
|
def encode_uxvywz([u_value, x_value, v_value, y_value, w_value, z_value]) do
|
|
u_int = to_string(u_value)
|
|
x_int = to_string(x_value)
|
|
v_int = to_string(v_value)
|
|
y_int = to_string(y_value)
|
|
w_int = to_string(w_value)
|
|
z_int = to_string(z_value)
|
|
"U#{u_int} X#{x_int} V#{v_int} Y#{y_int} W#{w_int} Z#{z_int}"
|
|
end
|
|
|
|
defp encode_error(error) do
|
|
case error do
|
|
:no_error -> "V0"
|
|
:emergency_lock -> "V1"
|
|
:timeout -> "V2"
|
|
:stall_detected -> "V3"
|
|
:calibration_error -> "V4"
|
|
:invalid_command -> "V14"
|
|
:no_config -> "V15"
|
|
:stall_detected_x -> "V31"
|
|
:stall_detected_y -> "V32"
|
|
:stall_detected_z -> "V33"
|
|
_ -> ""
|
|
end
|
|
end
|
|
|
|
defp encode_ints(args) do
|
|
Enum.map(args, fn {key, val} ->
|
|
String.upcase(to_string(key)) <> to_string(val)
|
|
end)
|
|
|> Enum.join(" ")
|
|
end
|
|
end
|