///|
/// Control export: joint-to-actuator bindings, `ros2_control` configuration,
/// a firmware pin map, and consistency checks.
///|
/// Maps one commandable robot joint onto an actuator, bus channel, and physical
/// pins, with direction, gear ratio, soft limits, and homing/limit inputs.
pub(all) struct ControlBinding {
/// Joint name as it appears in the compiled robot.
joint : String
/// Actuator component name that drives the joint.
actuator : String
/// Bus or interface channel, e.g. `"can0:1"` or `"step_dir:0"`.
channel : String
/// Free-form pin assignment, e.g. `"STEP=PA0; DIR=PA1; EN=PA2"`.
mcu_pins : String
/// `"normal"` or `"inverted"`.
direction : String
/// Motor revolutions per joint revolution.
gear_ratio : Double
soft_lower : Double?
soft_upper : Double?
/// Input that homes this joint, e.g. `"limit_home"`.
homing_input : String?
/// Input that ends travel at the far end, e.g. `"limit_closed"`.
limit_input : String?
}
///|
pub fn ControlBinding::new(
joint : String,
actuator~ : String = "",
channel~ : String = "",
mcu_pins~ : String = "",
direction~ : String = "normal",
gear_ratio~ : Double = 1.0,
soft_lower~ : Double? = None,
soft_upper~ : Double? = None,
homing_input~ : String? = None,
limit_input~ : String? = None,
) -> ControlBinding {
{
joint,
actuator,
channel,
mcu_pins,
direction,
gear_ratio,
soft_lower,
soft_upper,
homing_input,
limit_input,
}
}
///|
/// All control bindings declared anywhere in this component tree.
pub fn Component::control_bindings(self : Component) -> Array[ControlBinding] {
let bindings : Array[ControlBinding] = []
self.collect_control_bindings(bindings)
bindings
}
///|
fn Component::collect_control_bindings(
self : Component,
bindings : Array[ControlBinding],
) -> Unit {
for binding in self.control {
bindings.push(binding)
}
for att in self.attachments {
att.child.collect_control_bindings(bindings)
}
}
///|
/// Adds a joint-to-actuator binding.
pub fn Component::add_control_binding(
self : Component,
binding : ControlBinding,
) -> Component {
self.control.push(binding)
self
}
///|
fn yaml_quote(value : String) -> String {
"\"" +
value.replace(old="\\", new="\\\\").replace(old="\"", new="\\\"") +
"\""
}
///|
fn yaml_number(value : Double?) -> String {
match value {
Some(number) => number.to_string()
None => "null"
}
}
///|
fn yaml_string(value : String?) -> String {
match value {
Some(text) => yaml_quote(text)
None => "null"
}
}
///|
/// Renders a `ros2_control` configuration mapping every declared binding.
///
/// The controller manager exposes a joint-state broadcaster and a joint
/// trajectory controller over the bound joints. The hardware section carries
/// the actuator, channel, pins, direction, gear ratio, soft limits, homing and
/// limit inputs for each joint, plus the sensor interfaces.
pub fn Component::to_ros2_control_yaml(
self : Component,
controller_name~ : String = "joint_trajectory_controller",
update_rate_hz~ : Double = 100.0,
) -> String {
let bindings = self.control_bindings()
let sensors = self.power_budget().sensors
let mut out = "# Generated by gmlewis/cad (ros2_control)\n"
out = out + "controller_manager:\n ros__parameters:\n"
out = out + " update_rate: " + update_rate_hz.to_string() + "\n\n"
out = out + " joint_state_broadcaster:\n"
out = out + " type: joint_state_broadcaster/JointStateBroadcaster\n\n"
out = out +
" " +
controller_name +
":\n type: joint_trajectory_controller/JointTrajectoryController\n\n"
out = out + controller_name + ":\n ros__parameters:\n joints:\n"
if bindings.length() == 0 {
out = out + " []\n"
} else {
for binding in bindings {
out = out + " - " + yaml_quote(binding.joint) + "\n"
}
}
out = out + " command_interfaces:\n - position\n"
out = out + " state_interfaces:\n - position\n - velocity\n"
out = out + " allow_partial_joints_goal: false\n\n"
out = out + "moonbit_robot_hardware:\n ros__parameters:\n joints:\n"
if bindings.length() == 0 {
out = out + " []\n"
} else {
for binding in bindings {
out = out + " - " + yaml_quote(binding.joint) + "\n"
}
}
for binding in bindings {
out = out + " " + yaml_quote(binding.joint) + ":\n"
out = out + " actuator: " + yaml_quote(binding.actuator) + "\n"
out = out + " channel: " + yaml_quote(binding.channel) + "\n"
out = out + " mcu_pins: " + yaml_quote(binding.mcu_pins) + "\n"
out = out + " direction: " + yaml_quote(binding.direction) + "\n"
out = out + " gear_ratio: " + binding.gear_ratio.to_string() + "\n"
out = out + " soft_lower: " + yaml_number(binding.soft_lower) + "\n"
out = out + " soft_upper: " + yaml_number(binding.soft_upper) + "\n"
out = out +
" homing_input: " +
yaml_string(binding.homing_input) +
"\n"
out = out + " limit_input: " + yaml_string(binding.limit_input) + "\n"
}
out = out + " sensors:\n"
if sensors.length() == 0 {
out = out + " {}\n"
} else {
for line in sensors {
let name = leaf_name(line.path)
out = out + " " + yaml_quote(name) + ":\n"
out = out + " model: " + yaml_quote(line.spec.model) + "\n"
out = out + " interface: " + yaml_quote(line.spec.interface) + "\n"
out = out + " voltage_v: " + line.spec.voltage_v.to_string() + "\n"
out = out +
" current_ma: " +
line.spec.current_ma.to_string() +
"\n"
}
}
out
}
///|
fn csv_number_or_empty(value : Double?) -> String {
match value {
Some(number) => number.to_string()
None => "\"\""
}
}
///|
/// Renders the firmware pin map as CSV: one row per binding.
pub fn Component::to_pin_map_csv(self : Component) -> String {
let header : Array[String] = [
"joint", "actuator", "channel", "mcu_pins", "direction", "gear_ratio", "soft_lower",
"soft_upper", "homing_input", "limit_input",
]
let rows : Array[String] = [csv_row(header.map(fn(v) { csv_field(v) }))]
for binding in self.control_bindings() {
rows.push(
csv_row([
csv_field(binding.joint),
csv_field(binding.actuator),
csv_field(binding.channel),
csv_field(binding.mcu_pins),
csv_field(binding.direction),
binding.gear_ratio.to_string(),
csv_number_or_empty(binding.soft_lower),
csv_number_or_empty(binding.soft_upper),
csv_field(binding.homing_input.unwrap_or("")),
csv_field(binding.limit_input.unwrap_or("")),
]),
)
}
rows.fold(init="", fn(acc, row) { acc + row + "\n" })
}
///|
fn Component::collect_port_names(
self : Component,
names : Array[String],
) -> Unit {
for port in self.ports {
names.push(port.name)
}
for att in self.attachments {
att.child.collect_port_names(names)
}
}
///|
/// Checks the control map against the compiled robot: unknown joints or
/// actuators, unbound commandable joints, bad directions/gear ratios/limits,
/// and homing or limit inputs that are not declared ports.
///
/// Returns no warnings when the tree declares no bindings, so examples without
/// a control map are not forced to provide one.
pub fn Component::control_checks(self : Component) -> Array[String] {
let warnings : Array[String] = []
let bindings = self.control_bindings()
if bindings.length() == 0 {
return warnings
}
let robot = self.to_robot()
let component_names : Array[String] = []
self.collect_component_names(component_names)
let port_names : Array[String] = []
self.collect_port_names(port_names)
let seen : Array[String] = []
for binding in bindings {
if array_has(seen, binding.joint) {
warnings.push(
"duplicate control binding for joint '" + binding.joint + "'",
)
} else {
seen.push(binding.joint)
}
if robot.get_joint(binding.joint) is None {
warnings.push(
"control binding references unknown joint '" + binding.joint + "'",
)
}
if binding.actuator == "" {
warnings.push(
"control binding for joint '" + binding.joint + "' has no actuator",
)
} else if !array_has(component_names, binding.actuator) {
warnings.push(
"control binding for joint '" +
binding.joint +
"' references unknown actuator '" +
binding.actuator +
"'",
)
}
if binding.direction != "normal" && binding.direction != "inverted" {
warnings.push(
"control binding for joint '" +
binding.joint +
"' has direction '" +
binding.direction +
"'",
)
}
if binding.gear_ratio <= 0.0 {
warnings.push(
"control binding for joint '" +
binding.joint +
"' has gear ratio " +
binding.gear_ratio.to_string(),
)
}
match binding.soft_lower {
Some(lower) =>
match binding.soft_upper {
Some(upper) =>
if lower > upper {
warnings.push(
"control binding for joint '" +
binding.joint +
"' has soft lower > upper",
)
}
None => ()
}
None => ()
}
match binding.homing_input {
Some(input) =>
if !array_has(port_names, input) {
warnings.push(
"control binding for joint '" +
binding.joint +
"' references unknown homing input '" +
input +
"'",
)
}
None => ()
}
match binding.limit_input {
Some(input) =>
if !array_has(port_names, input) {
warnings.push(
"control binding for joint '" +
binding.joint +
"' references unknown limit input '" +
input +
"'",
)
}
None => ()
}
}
for joint in robot.joints {
let commandable = match joint.joint_type {
Fixed | Floating | Planar => false
Revolute | Continuous | Prismatic => joint.mimic is None
}
if commandable && !array_has(seen, joint.name) {
warnings.push("robot joint '" + joint.name + "' has no control binding")
}
}
warnings
}
///|
fn c_string(value : String) -> String {
"\"" +
value
.replace(old="\\", new="\\\\")
.replace(old="\"", new="\\\"")
.replace(old="\n", new="\\n") +
"\""
}
///|
fn c_float(value : Double) -> String {
let text = value.to_string()
if text.contains(".") || text.contains("e") || text.contains("E") {
text + "f"
} else {
text + ".0f"
}
}
///|
fn c_optional_string(value : String?) -> String {
match value {
Some(text) => c_string(text)
None => "NULL"
}
}
///|
/// A C header for firmware: joint-to-actuator bindings and sensor interfaces.
///
/// The tables are derived from `ControlBinding`s and `SensorSpec`s in the tree.
pub fn Component::firmware_header(self : Component) -> String {
let bindings = self.control_bindings()
let sensors = self.power_budget().sensors
let mut out = "// Generated by gmlewis/cad for " +
self.display_name() +
"\n#pragma once\n\n#include \n#include \n\n"
out = out + "#define MB_JOINT_COUNT " + bindings.length().to_string() + "\n"
out = out + "#define MB_SENSOR_COUNT " + sensors.length().to_string() + "\n\n"
out = out + "typedef struct {\n"
out = out + " const char *joint;\n"
out = out + " const char *actuator;\n"
out = out + " const char *channel;\n"
out = out + " const char *pins;\n"
out = out + " const char *direction;\n"
out = out + " float gear_ratio;\n"
out = out + " uint8_t has_soft_limits;\n"
out = out + " float soft_lower;\n"
out = out + " float soft_upper;\n"
out = out + " const char *homing_input;\n"
out = out + " const char *limit_input;\n"
out = out + "} mb_joint_binding;\n\n"
out = out +
"static const mb_joint_binding mb_joint_bindings[MB_JOINT_COUNT] = {\n"
for binding in bindings {
let has_limits = match binding.soft_lower {
Some(_) =>
match binding.soft_upper {
Some(_) => true
None => false
}
None => false
}
let lower = match binding.soft_lower {
Some(value) => c_float(value)
None => "0.0f"
}
let upper = match binding.soft_upper {
Some(value) => c_float(value)
None => "0.0f"
}
out = out +
" {" +
c_string(binding.joint) +
", " +
c_string(binding.actuator) +
", " +
c_string(binding.channel) +
", " +
c_string(binding.mcu_pins) +
", " +
c_string(binding.direction) +
", " +
c_float(binding.gear_ratio) +
", " +
(if has_limits { "1" } else { "0" }) +
", " +
lower +
", " +
upper +
", " +
c_optional_string(binding.homing_input) +
", " +
c_optional_string(binding.limit_input) +
"},\n"
}
out = out + "};\n\n"
out = out + "typedef struct {\n"
out = out + " const char *name;\n"
out = out + " const char *model;\n"
out = out + " const char *interface;\n"
out = out + " float voltage_v;\n"
out = out + " float current_ma;\n"
out = out + "} mb_sensor;\n\n"
out = out + "static const mb_sensor mb_sensors[MB_SENSOR_COUNT] = {\n"
for line in sensors {
out = out +
" {" +
c_string(leaf_name(line.path)) +
", " +
c_string(line.spec.model) +
", " +
c_string(line.spec.interface) +
", " +
c_float(line.spec.voltage_v) +
", " +
c_float(line.spec.current_ma) +
"},\n"
}
out = out + "};\n"
out
}