diff --git a/arduino_firmware/end_effector/CAN_FD_bridge.cpp b/arduino_firmware/end_effector/CAN_FD_bridge.cpp new file mode 100644 index 0000000..9363e4b --- /dev/null +++ b/arduino_firmware/end_effector/CAN_FD_bridge.cpp @@ -0,0 +1 @@ +#include \ No newline at end of file diff --git a/arduino_firmware/end_effector/CAN_FD_bridge.h b/arduino_firmware/end_effector/CAN_FD_bridge.h new file mode 100644 index 0000000..2def984 --- /dev/null +++ b/arduino_firmware/end_effector/CAN_FD_bridge.h @@ -0,0 +1,10 @@ +#ifndef CAN_FD_BRIDGE_H +#define CAN_FD_BRIDGE_H + +// Reconstructs CAN FD frame from an SPI message +void SPI_to_CAN_FD(); + +// Sends a CAN FD frame via an SPI message +void CAN_FD_to_SPI(); + +#endif CAN_FD_BRIDGE_H \ No newline at end of file diff --git a/arduino_firmware/end_effector/end_effector_actions.cpp b/arduino_firmware/end_effector/end_effector_actions.cpp new file mode 100644 index 0000000..bdf5e81 --- /dev/null +++ b/arduino_firmware/end_effector/end_effector_actions.cpp @@ -0,0 +1,16 @@ +#include + +// Interprets an SPI message and sends the corresponding action to the appropriate sensor +void handle_SPI_msg(uint8_t spi_peripheral_rx_buf[BUF_SIZE]) { + return void; +} + +// Interprets a CAN FD message and sends the corresponding action over SPI if permissible +void handle_CAN_FD_message(uint8_t spi_controller_tx_buf[BUF_SIZE]) { + return void; +} + +// Retrieves data from a requested sensor and sends it over SPI +void send_data_update_SPI_msg(uint8_t spi_peripheral_rx_buf[BUF_SIZE], uint8_t spi_peripheral_tx_buf[BUF_SIZE]) { + return void; +} \ No newline at end of file diff --git a/arduino_firmware/end_effector/end_effector_actions.h b/arduino_firmware/end_effector/end_effector_actions.h new file mode 100644 index 0000000..b5232cb --- /dev/null +++ b/arduino_firmware/end_effector/end_effector_actions.h @@ -0,0 +1,21 @@ +#ifndef END_EFFECTOR_ACTIONS_H +#define END_EFFECTOR_ACTIONS_H + +#include + +/* SPI message struct + - destination sensor + - sensor request + - sensor value +*/ + +// Interprets an SPI message and sends the corresponding action to the appropriate sensor or as a message over CAN FD if permissible +void handle_SPI_msg(uint8_t spi_peripheral_rx_buf[BUF_SIZE]); + +// Interprets a CAN FD message and sends the corresponding action over SPI if permissible +void handle_CAN_FD_message(uint8_t spi_controller_tx_buf[BUF_SIZE]); + +// Retrieves data from a requested sensor and sends it over SPI +void send_data_update_SPI_msg(uint8_t spi_peripheral_rx_buf[BUF_SIZE], uint8_t spi_peripheral_tx_buf[BUF_SIZE]); + +#endif END_EFFECTOR_ACTIONS_H \ No newline at end of file diff --git a/arduino_firmware/end_effector/end_effector_base.h b/arduino_firmware/end_effector/end_effector_base.h new file mode 100644 index 0000000..3cf5bd9 --- /dev/null +++ b/arduino_firmware/end_effector/end_effector_base.h @@ -0,0 +1,7 @@ +#ifndef END_EFFECTOR_BASE_H +#define END_EFFECTOR_BASE_H + +// Standard data buffer size = 32 Bytes +static const uint16_t BUF_SIZE = 32; + +#endif END_EFFECTOR_BASE_H \ No newline at end of file diff --git a/arduino_firmware/end_effector/end_effector_big_gripper.ino b/arduino_firmware/end_effector/end_effector_big_gripper.ino new file mode 100644 index 0000000..6c6d9b6 --- /dev/null +++ b/arduino_firmware/end_effector/end_effector_big_gripper.ino @@ -0,0 +1,62 @@ +#include +#include + +// This ESP is the peripheral, the bridge ESP is the controller +ESP32SPISlave peripheral; + +// Sending data buffer +uint8_t spi_peripheral_tx_buf[BUF_SIZE] = {0}; + +// Receiving data buffer +uint8_t spi_peripheral_rx_buf[BUF_SIZE] = {0}; + +void setup() { + + Serial.begin(115200); + + // Pin assignments can be customized + // Default pins are: VSPI (MISO=19, MOSI=23, SCLK=18, SS=5) + peripheral.begin(); // Uses default VSPI pins + + // Set up buffers + peripheral.set_buffers(spi_peripheral_tx_buf, spi_peripheral_rx_buf, BUF_SIZE); + + Serial.println("Big Gripper: SPI Peripheral Initialized"); + +} + +void loop() { + + // While no message has been recieved from the controller + while (!peripheral.wait(portMAX_DELAY)) { + ; + } + + // Once a message has been received from the controller + // Get the message size + size_t msg_size = peripheral.get_received_trans_len(); + + // Print the number of bytes received + Serial.print("Big Gripper: Received "); + Serial.print(msg_size); + Serial.print(" bytes: "); + + // Print the msg + for (uint16_t i = 0; i < msg_size; i++) { + Serial.print(spi_peripheral_rx_buf[i], HEX); + // Clear the buffer + spi_peripheral_rx_buf[i] = 0; + } + Serial.println(); + + // Interpret the message and perform appropriate action + handle_SPI_msg(spi_peripheral_rx_buf); + + // Get updated sensor values and send to the bridge module + // The controller will only pick this message up on the next message that it delivers + send_data_update_SPI_msg(spi_peripheral_rx_buf, spi_peripheral_tx_buf); + + // Clear message status + peripheral.pop(); + +} \ No newline at end of file diff --git a/arduino_firmware/end_effector/end_effector_bridge.ino b/arduino_firmware/end_effector/end_effector_bridge.ino new file mode 100644 index 0000000..6f7682e --- /dev/null +++ b/arduino_firmware/end_effector/end_effector_bridge.ino @@ -0,0 +1,67 @@ +#include +#include +#include + +// Defines pin that peripheral ESP is connected to +#define SS_PIN 6 + +// Sending data buffer +uint8_t spi_controller_tx_buf[BUF_SIZE] = {0}; + +// Receiving data buffer +uint8_t spi_controller_rx_buf[BUF_SIZE] = {0}; + +void setup() { + + Serial.begin(115200); + + // Initialize SPI bus + SPI.begin(); + + // Set peripheral SS pin as inactive output (active-low) + pinMode(SS_PIN, OUTPUT); + digitalWrite(SS_PIN, HIGH); + + + Serial.println("Bridge ESP: SPI Controller Initialized"); + +} + +void loop() { + + // Translate CAN FD frame to SPI frame to send + handle_CAN_FD_message(spi_controller_tx_buf); + + // Activate peripheral ESP connection for writing + digitalWrite(SS_PIN, LOW); + + // Settings: 1MHz, Most significant bit first, data mode + SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE0)); + + // Send all 32 bytes from buffer to the peripheral + for (int i = 0; i < BUF_SIZE; i++) { + // This sends a byte and receives a byte at the same time + spi_controller_rx_buf[i] = SPI.transfer(spi_controller_tx_buf[i]); + } + + // Stop transmission + SPI.endTransaction(); + + // Deactivate peripheral connection + digitalWrite(SS_PIN, HIGH); + + // Forward response message from peripheral onto CAN FD connection to Jetson + handle_SPI_msg(spi_controller_rx_buf); + + // Print data received from peripheral + Serial.print("Bridge ESP: Received bytes: "); + for (int i = 0; i < BUF_SIZE; i++) { + Serial.print(spi_controller_rx_buf[i], HEX); + Serial.print(" "); + } + Serial.println(); + + // Delay before writing to peripheral again + delay(1000); + +} \ No newline at end of file diff --git a/arduino_firmware/end_effector/end_effector_core_xy.ino b/arduino_firmware/end_effector/end_effector_core_xy.ino new file mode 100644 index 0000000..0e5f812 --- /dev/null +++ b/arduino_firmware/end_effector/end_effector_core_xy.ino @@ -0,0 +1,62 @@ +#include +#include + +// This ESP is the peripheral, the bridge ESP is the controller +ESP32SPISlave peripheral; + +// Sending data buffer +uint8_t spi_peripheral_tx_buf[BUF_SIZE] = {0}; + +// Receiving data buffer +uint8_t spi_peripheral_rx_buf[BUF_SIZE] = {0}; + +void setup() { + + Serial.begin(115200); + + // Pin assignments can be customized + // Default pins are: VSPI (MISO=19, MOSI=23, SCLK=18, SS=5) + peripheral.begin(); // Uses default VSPI pins + + // Set up buffers + peripheral.set_buffers(spi_peripheral_tx_buf, spi_peripheral_rx_buf, BUF_SIZE); + + Serial.println("Core X-Y: SPI Peripheral Initialized"); + +} + +void loop() { + + // While no message has been recieved from the controller + while (!peripheral.wait(portMAX_DELAY)) { + ; + } + + // Once a message has been received from the controller + // Get the message size + size_t msg_size = peripheral.get_received_trans_len(); + + // Print the number of bytes received + Serial.print("Core X-Y: Received "); + Serial.print(msg_size); + Serial.print(" bytes: "); + + // Print the msg + for (uint16_t i = 0; i < msg_size; i++) { + Serial.print(spi_peripheral_rx_buf[i], HEX); + // Clear the buffer + spi_peripheral_rx_buf[i] = 0; + } + Serial.println(); + + // Interpret the message and perform appropriate action + handle_SPI_msg(spi_peripheral_rx_buf); + + // Get updated sensor values and send to the bridge module + // The controller will only pick this message up on the next message that it delivers + send_data_update_SPI_msg(spi_peripheral_rx_buf, spi_peripheral_tx_buf); + + // Clear message status + peripheral.pop(); + +} \ No newline at end of file diff --git a/arduino_firmware/end_effector/moteus_driver/Moteus.h b/arduino_firmware/end_effector/moteus_driver/Moteus.h new file mode 100644 index 0000000..45a45a6 --- /dev/null +++ b/arduino_firmware/end_effector/moteus_driver/Moteus.h @@ -0,0 +1,746 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "moteus_protocol.h" + +namespace mm = mjbots::moteus; + +/// This is the primary interface to a moteus controller. One +/// instance of this class should be created per controller that is +/// commanded or monitored. +/// +/// The primary control functions each have 3 possible forms: +/// +/// 1. A "Make" variant which constructs a CanFdFrame to be used in a +/// later call to Transport::Cycle. +/// +/// 2. A "Set" variant which sends a command to the controller and +/// waits for a response in a blocking manner. +/// +/// 3. A "Begin" variant which sends a command and requires that the +/// user call Poll() regularly to check for a response, and then +/// retrieve that response from Moteus::last_result(). +class Moteus { + public: + using CanFdFrame = mm::CanFdFrame; + static constexpr int kDiagnosticTimeoutUs = 4000; + + using Query = mm::Query; + using PositionMode = mm::PositionMode; + using VFOCMode = mm::VFOCMode; + using CurrentMode = mm::CurrentMode; + using StayWithinMode = mm::StayWithinMode; + + static constexpr mm::Resolution kInt8 = mm::Resolution::kInt8; + static constexpr mm::Resolution kInt16 = mm::Resolution::kInt16; + static constexpr mm::Resolution kInt32 = mm::Resolution::kInt32; + static constexpr mm::Resolution kFloat = mm::Resolution::kFloat; + + struct Options { + // The ID of the servo to communicate with. + int8_t id = 1; + + // The source ID to use for the commanding node (i.e. the host or + // master). + int8_t source = 0; + + mm::Query::Format query_format; + + // Use the given prefix for all CAN IDs. + uint16_t can_prefix = 0x0000; + + // Disable BRS on outgoing frames. + bool disable_brs = true; + + // Request the configured set of registers as a query with every + // command. + bool default_query = true; + + uint16_t min_rcv_wait_us = 2000; + + Options() {} + }; + + Moteus(ACAN2517FD& can_bus, + const Options& options = {}) + : can_bus_(can_bus), + options_(options) { + mm::CanData can_data; + mm::WriteCanData query_write(&can_data); + mm::Query::Make(&query_write, options_.query_format); + + query_size_ = can_data.size; + query_data_ = reinterpret_cast(realloc(query_data_, query_size_)); + ::memcpy(query_data_, &can_data.data[0], query_size_); + } + + struct Result { + unsigned long timestamp = 0; + CanFdFrame frame; + mm::Query::Result values; + }; + + // The most recent result from any command. + const Result& last_result() const { return last_result_; } + + + ///////////////////////////////////////// + // Query + + CanFdFrame MakeQuery(const mm::Query::Format* format_override = nullptr) { + return MakeFrame(mm::EmptyMode(), {}, {}, + format_override == nullptr ? + &options_.query_format : format_override); + } + + bool SetQuery(const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeQuery(query_override)); + } + + + ///////////////////////////////////////// + // StopMode + + CanFdFrame MakeStop(const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::StopMode(), {}, {}, query_override); + } + + bool SetStop(const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeStop(query_override)); + } + + void BeginStop(const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeStop(query_override)); + } + + + ///////////////////////////////////////// + // BrakeMode + + + CanFdFrame MakeBrake(const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::BrakeMode(), {}, {}, query_override); + } + + bool SetBrake(const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeBrake(query_override)); + } + + void BeginBrake(const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeBrake(query_override)); + } + + + ///////////////////////////////////////// + // PositionMode + + CanFdFrame MakePosition(const mm::PositionMode::Command& cmd, + const mm::PositionMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::PositionMode(), + cmd, + (command_override == nullptr ? + mm::PositionMode::Format() : *command_override), + query_override); + } + + bool SetPosition(const mm::PositionMode::Command& cmd, + const mm::PositionMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakePosition(cmd, command_override, query_override)); + } + + void BeginPosition(const mm::PositionMode::Command& cmd, + const mm::PositionMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand( + MakePosition(cmd, command_override, query_override)); + } + + bool SetPositionWaitComplete(const mm::PositionMode::Command& cmd, + double period_s, + const mm::PositionMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + mm::Query::Format query_format = + query_override == nullptr ? options_.query_format : *query_override; + query_format.trajectory_complete = kInt8; + + // The query returned from a mode change will always report the + // previous state. Thus we need to have at least 2 responses + // before we have a valid trajectory complete flag. + int count = 2; + while (true) { + const bool got_result = SetPosition(cmd, command_override, &query_format); + if (got_result) { + count = count - 1; + if (count < 0) { count = 0; } + } + if (count == 0 && + got_result && + last_result_.values.trajectory_complete) { + return true; + } + + delay(static_cast(period_s * 1000)); + } + } + + + ///////////////////////////////////////// + // VFOCMode + + CanFdFrame MakeVFOC(const mm::VFOCMode::Command& cmd, + const mm::VFOCMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::VFOCMode(), + cmd, + (command_override == nullptr ? + mm::VFOCMode::Format() : *command_override), + query_override); + } + + bool SetVFOC(const mm::VFOCMode::Command& cmd, + const mm::VFOCMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeVFOC(cmd, command_override, query_override)); + } + + void BeginVFOC(const mm::VFOCMode::Command& cmd, + const mm::VFOCMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeVFOC(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // CurrentMode + + CanFdFrame MakeCurrent(const mm::CurrentMode::Command& cmd, + const mm::CurrentMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::CurrentMode(), + cmd, + (command_override == nullptr ? + mm::CurrentMode::Format() : *command_override), + query_override); + } + + bool SetCurrent(const mm::CurrentMode::Command& cmd, + const mm::CurrentMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeCurrent(cmd, command_override, query_override)); + } + + void BeginCurrent(const mm::CurrentMode::Command& cmd, + const mm::CurrentMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeCurrent(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // StayWithinMode + + CanFdFrame MakeStayWithin(const mm::StayWithinMode::Command& cmd, + const mm::StayWithinMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::StayWithinMode(), + cmd, + (command_override == nullptr ? + mm::StayWithinMode::Format() : *command_override), + query_override); + } + + bool SetStayWithin(const mm::StayWithinMode::Command& cmd, + const mm::StayWithinMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeStayWithin(cmd, command_override, query_override)); + } + + void BeginStayWithin(const mm::StayWithinMode::Command& cmd, + const mm::StayWithinMode::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeStayWithin(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // OutputNearest + + CanFdFrame MakeOutputNearest(const mm::OutputNearest::Command& cmd, + const mm::OutputNearest::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::OutputNearest(), + cmd, + (command_override == nullptr ? + mm::OutputNearest::Format() : *command_override), + query_override); + } + + bool SetOutputNearest(const mm::OutputNearest::Command& cmd, + const mm::OutputNearest::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeOutputNearest(cmd, command_override, query_override)); + } + + void BeginOutputNearest(const mm::OutputNearest::Command& cmd, + const mm::OutputNearest::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeOutputNearest(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // OutputExact + + CanFdFrame MakeOutputExact(const mm::OutputExact::Command& cmd, + const mm::OutputExact::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::OutputExact(), + cmd, + (command_override == nullptr ? + mm::OutputExact::Format() : *command_override), + query_override); + } + + bool SetOutputExact(const mm::OutputExact::Command& cmd, + const mm::OutputExact::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeOutputExact(cmd, command_override, query_override)); + } + + void BeginOutputExact(const mm::OutputExact::Command& cmd, + const mm::OutputExact::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeOutputExact(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // RequireReindex + + CanFdFrame MakeRequireReindex(const mm::RequireReindex::Command& cmd, + const mm::RequireReindex::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::RequireReindex(), + cmd, + (command_override == nullptr ? + mm::RequireReindex::Format() : *command_override), + query_override); + } + + bool SetRequireReindex(const mm::RequireReindex::Command& cmd, + const mm::RequireReindex::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeRequireReindex(cmd, command_override, query_override)); + } + + void BeginRequireReindex(const mm::RequireReindex::Command& cmd, + const mm::RequireReindex::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeRequireReindex(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // RecapturePositionVelocity + + CanFdFrame MakeRecapturePositionVelocity(const mm::RecapturePositionVelocity::Command& cmd, + const mm::RecapturePositionVelocity::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return MakeFrame(mm::RecapturePositionVelocity(), + cmd, + (command_override == nullptr ? + mm::RecapturePositionVelocity::Format() : *command_override), + query_override); + } + + bool SetRecapturePositionVelocity(const mm::RecapturePositionVelocity::Command& cmd, + const mm::RecapturePositionVelocity::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeRecapturePositionVelocity(cmd, command_override, query_override)); + } + + void BeginRecapturePositionVelocity(const mm::RecapturePositionVelocity::Command& cmd, + const mm::RecapturePositionVelocity::Format* command_override = nullptr, + const mm::Query::Format* query_override = nullptr) { + BeginSingleCommand(MakeRecapturePositionVelocity(cmd, command_override, query_override)); + } + + + ///////////////////////////////////////// + // Diagnostic channel operations + + enum DiagnosticReplyMode { + kExpectOK, + kExpectSingleLine, + }; + + String DiagnosticCommand(const String& message_in, + DiagnosticReplyMode reply_mode = kExpectOK) { + { + String message = message_in + "\n"; + while (message.length() > 0) { + const auto to_write = message.length() < 48u ? message.length() : 48u; + mm::DiagnosticWrite::Command write; + write.data = message.c_str(); + write.size = to_write; + + auto frame = DefaultFrame(kNoReply); + mm::WriteCanData write_frame(frame.data, &frame.size); + mm::DiagnosticWrite::Make(&write_frame, write, {}); + + BeginSingleCommand(frame); + message.remove(0, to_write); + } + } + + // Now we read until we get a complete response. + String response; + String current_line; + + const auto start = micros(); + auto end = start + kDiagnosticTimeoutUs; + + while (true) { + { + const auto now = micros(); + if (static_cast(now - end) > 0) { + return {}; + } + } + { + mm::DiagnosticRead::Command read; + auto frame = DefaultFrame(kReplyRequired); + mm::WriteCanData write_frame(frame.data, &frame.size); + mm::DiagnosticRead::Make(&write_frame, read, {}); + + BeginSingleCommand(frame); + } + + while (true) { + if (![&]() { + while (!Poll()) { + const auto now = micros(); + if (static_cast(now - end) > 0) { + return false; + } + } + return true; + }()) { + break; + } + + { + const auto parsed = mm::DiagnosticResponse::Parse( + last_result_.frame.data, last_result_.frame.size); + if (parsed.channel != 1) { + // This must not have been for us after all. + continue; + } + + if (parsed.size) { + // We got some data, so bump our timeout forward. + end = start + kDiagnosticTimeoutUs; + } + + // Sigh... older versions of Arduino have no ability to + // construct a string from a pointer and length. Guess we'll + // emulate it. + { + String this_data; + this_data.reserve(parsed.size); + for (int8_t i = 0; i < parsed.size; i++) { + this_data.concat(static_cast(parsed.data[i])); + } + + current_line.concat(this_data); + } + } + + auto find_newline = [&]() { + const int cr = current_line.indexOf('\r'); + const int nl = current_line.indexOf('\n'); + const int first_newline = + (cr < 0 ? nl : + nl < 0 ? cr : + nl < cr ? nl : cr); + return first_newline; + }; + int first_newline = -1; + while ((first_newline = find_newline()) != -1) { + String this_line = current_line.substring(0, first_newline); + if (reply_mode == kExpectSingleLine) { + return this_line; + } else if (this_line == "OK") { + return response; + } else { + response.concat(current_line.substring(0, first_newline + 1)); + current_line.remove(0, first_newline + 1); + } + } + + // We got a frame, so break out of our inner loop. + break; + } + } + } + + String SetDiagnosticRead(int channel = 1) { + { + mm::DiagnosticRead::Command read; + read.channel = channel; + auto frame = DefaultFrame(kReplyRequired); + mm::WriteCanData write_frame(frame.data, &frame.size); + mm::DiagnosticRead::Make(&write_frame, read, {}); + + BeginSingleCommand(frame); + } + + const auto start = micros(); + auto end = start + kDiagnosticTimeoutUs; + + while (true) { + if (![&]() { + while (!Poll()) { + const auto now = micros(); + if (static_cast(now - end) > 0) { + return false; + } + } + return true; + }()) { + return ""; + } + + const auto parsed = mm::DiagnosticResponse::Parse( + last_result_.frame.data, last_result_.frame.size); + if (parsed.channel != channel) { + continue; + } + + String result; + result.reserve(parsed.size); + for (int8_t i = 0; i < parsed.size; i++) { + result.concat(static_cast(parsed.data[i])); + } + return result; + } + } + + void SetDiagnosticFlush(int channel = 1) { + while (true) { + const auto response = SetDiagnosticRead(channel); + if (response.length() == 0) { return; } + } + } + + ///////////////////////////////////////// + // Non-command related methods + + /// Look for a response to a previous command. Return true if one + /// has been received. The parsed results can be seen in + /// Moteus::last_result() + bool Poll() { + const auto now = micros(); + + // Ensure any interrupts have been handled. + can_bus_.poll(); + + if (!can_bus_.available()) { return false; } + + CANFDMessage rx_msg; + can_bus_.receive(rx_msg); + + const int8_t source = (rx_msg.id >> 8) & 0x7f; + const int8_t destination = (rx_msg.id & 0x7f); + const uint16_t can_prefix = (rx_msg.id >> 16); + + if (source != options_.id || + destination != options_.source || + can_prefix != options_.can_prefix) { + return false; + } + + last_result_.timestamp = now; + + auto& cf = last_result_.frame; + cf.arbitration_id = rx_msg.id; + cf.destination = destination; + cf.source = source; + cf.size = rx_msg.len; + ::memcpy(&cf.data[0], &rx_msg.data[0], rx_msg.len); + cf.can_prefix = can_prefix; + + if (rx_msg.type == CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH) { + cf.brs = mm::CanFdFrame::kForceOn; + cf.fdcan_frame = mm::CanFdFrame::kForceOn; + } else if (rx_msg.type == CANFDMessage::CANFD_NO_BIT_RATE_SWITCH) { + cf.brs = mm::CanFdFrame::kForceOff; + cf.fdcan_frame = mm::CanFdFrame::kForceOn; + } else { + cf.brs = mm::CanFdFrame::kForceOff; + cf.fdcan_frame = mm::CanFdFrame::kForceOff; + } + + last_result_.values = + mm::Query::Parse(&cf.data[0], cf.size); + + return true; + } + + bool BeginSingleCommand(const mm::CanFdFrame& frame) { + CANFDMessage can_message; + can_message.id = frame.arbitration_id; + can_message.ext = true; + const bool desired_brs = + (frame.brs == CanFdFrame::kDefault ? !options_.disable_brs : + frame.brs == CanFdFrame::kForceOn ? true : false); + + if (frame.fdcan_frame == CanFdFrame::kDefault || + frame.fdcan_frame == CanFdFrame::kForceOn) { + if (desired_brs) { + can_message.type = CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH; + } else { + can_message.type = CANFDMessage::CANFD_NO_BIT_RATE_SWITCH; + } + } else { + can_message.type = CANFDMessage::CAN_DATA; + } + can_message.len = frame.size; + ::memcpy(&can_message.data[0], &frame.data[0], frame.size); + can_message.ext = true; + + PadCan(&can_message); + + // To work even when the ACAN2517FD doesn't have functioning + // interrupts, we will just poll it before and after attempting to + // send our message. This slows things down, but we're on an + // Arduino, so who cares? + can_bus_.poll(); + can_bus_.tryToSend(can_message); + can_bus_.poll(); + + return frame.reply_required; + } + + bool ExecuteSingleCommand(const mm::CanFdFrame& frame) { + const bool reply_required = BeginSingleCommand(frame); + + if (!reply_required) { return false; } + + auto start = micros(); + auto end = start + options_.min_rcv_wait_us; + bool got_a_response = false; + + while (true) { + const auto now = micros(); + const auto delta = static_cast(now - end); + if (delta > 0) { + // We timed out. + return false; + } + + if (Poll()) { + got_a_response = true; + } else if (got_a_response) { + // We both received a response, and have no more queued up. + return true; + } + } + } + + private: + enum ReplyMode { + kNoReply, + kReplyRequired, + }; + + CanFdFrame DefaultFrame(ReplyMode reply_mode = kReplyRequired) { + CanFdFrame result; + result.destination = options_.id; + result.reply_required = (reply_mode == kReplyRequired); + + result.arbitration_id = + (result.destination) | + (result.source << 8) | + (result.reply_required ? 0x8000 : 0x0000) | + (static_cast(options_.can_prefix) << 16); + result.bus = 0; + + return result; + } + + template + CanFdFrame MakeFrame(const CommandType&, + const typename CommandType::Command& cmd, + const typename CommandType::Format& fmt, + const mm::Query::Format* query_override = nullptr) { + auto result = DefaultFrame( + query_override != nullptr ? kReplyRequired : + options_.default_query ? kReplyRequired : kNoReply); + + mm::WriteCanData write_frame(result.data, &result.size); + CommandType::Make(&write_frame, cmd, fmt); + + if (query_override) { + mm::Query::Make(&write_frame, *query_override); + } else if (options_.default_query) { + ::memcpy(&result.data[result.size], + query_data_, + query_size_); + result.size += query_size_; + } + + return result; + } + + static int8_t RoundUpDlc(int8_t size) { + if (size <= 0) { return 0; } + if (size <= 1) { return 1; } + if (size <= 2) { return 2; } + if (size <= 3) { return 3; } + if (size <= 4) { return 4; } + if (size <= 5) { return 5; } + if (size <= 6) { return 6; } + if (size <= 7) { return 7; } + if (size <= 8) { return 8; } + if (size <= 12) { return 12; } + if (size <= 16) { return 16; } + if (size <= 20) { return 20; } + if (size <= 24) { return 24; } + if (size <= 32) { return 32; } + if (size <= 48) { return 48; } + if (size <= 64) { return 64; } + return 0; + } + + static void PadCan(CANFDMessage* msg) { + const auto new_size = RoundUpDlc(msg->len); + for (int8_t i = msg->len; i < new_size; i++) { + msg->data[i] = 0x50; + } + msg->len = new_size; + } + + ACAN2517FD& can_bus_; + const Options options_; + + Result last_result_; + char* query_data_ = nullptr; + size_t query_size_ = 0; +}; diff --git a/arduino_firmware/end_effector/moteus_driver/moteus_multiplex.h b/arduino_firmware/end_effector/moteus_driver/moteus_multiplex.h new file mode 100644 index 0000000..e58bd6e --- /dev/null +++ b/arduino_firmware/end_effector/moteus_driver/moteus_multiplex.h @@ -0,0 +1,666 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +namespace mjbots { +namespace moteus { + +/// Each value can be sent or received as one of the following. +enum Resolution : int8_t { + kInt8 = 0, + kInt16 = 1, + kInt32 = 2, + kFloat = 3, + kIgnore, +}; + +/// A vocabulary type for the basic data in a CAN-FD frame. +struct CanData { + uint8_t data[64] = {}; + uint8_t size = 0; +}; + +enum Multiplex : uint16_t { + kWriteBase = 0x00, + kWriteInt8 = 0x00, + kWriteInt16 = 0x04, + kWriteInt32 = 0x08, + kWriteFloat = 0x0c, + + kReadBase = 0x10, + kReadInt8 = 0x10, + kReadInt16 = 0x14, + kReadInt32 = 0x18, + kReadFloat = 0x1c, + + kReplyBase = 0x20, + kReplyInt8 = 0x20, + kReplyInt16 = 0x24, + kReplyInt32 = 0x28, + kReplyFloat = 0x2c, + + kWriteError = 0x30, + kReadError = 0x31, + + // # Tunneled Stream # + kClientToServer = 0x40, + kServerToClient = 0x41, + kClientPollServer = 0x42, + + kNop = 0x50, +}; + +namespace detail { +template +class numeric_limits { + public: +}; + +template <> +class numeric_limits { + public: + static int8_t max() { return 127; } + static int8_t min() { return -128; } +}; + +template <> +class numeric_limits { + public: + static int16_t max() { return 32767; } + static int16_t min() { return -32768; } +}; + +template <> +class numeric_limits { + public: + static int32_t max() { return 2147483647; } + static int32_t min() { return -2147483648; } +}; + +template <> +class numeric_limits { + public: +}; + +template +T max(T lhs, T rhs) { + return (lhs >= rhs) ? lhs : rhs; +} + +template +T min(T lhs, T rhs) { + return (lhs <= rhs) ? lhs : rhs; +} + +} + +namespace { +template +T Saturate(double value, double scale) { + // TODO: Implement without numeric_limits + if (!::isfinite(value)) { + return detail::numeric_limits::min(); + } + + const double scaled = value / scale; + const auto max = detail::numeric_limits::max(); + + const double double_max = static_cast(max); + // We purposefully limit to +- max, rather than to min. The minimum + // value for our two's complement types is reserved for NaN. + if (scaled < -double_max) { return -max; } + if (scaled > double_max) { return max; } + return static_cast(scaled); +} +} + +/// This class can be used to append values to the end of a CAN-FD +/// frame. +class WriteCanData { + public: + WriteCanData(CanData* frame) : data_(&frame->data[0]), size_(&frame->size) {} + WriteCanData(uint8_t* data, uint8_t* size) : data_(data), size_(size) {} + + uint8_t size() const { return *size_; } + + template + void Write(X value_in) { +#ifndef __ORDER_LITTLE_ENDIAN__ +#error "only little endian architectures supported" +#endif + + T value = static_cast(value_in); + if (sizeof(value) + *size_ > 64) { + abort(); + } + + ::memcpy(&data_[*size_], + reinterpret_cast(&value), + sizeof(value)); + *size_ += sizeof(value); + } + + void WriteVaruint(uint32_t value) { + while (true) { + auto this_byte = value & 0x7f; + value >>= 7; + this_byte |= ((value != 0) ? 0x80 : 0x00); + Write(this_byte); + + if (value == 0) { break; } + } + } + + void Write(const char* data, uint16_t size) { + if ((size + *size_) > 64) { + abort(); + } + ::memcpy(&data_[*size_], data, size); + *size_ += size; + } + + void WriteInt(int32_t value, Resolution res) { + switch (res) { + case Resolution::kInt8: { + Write(detail::max( + -127, detail::min(127, value))); + break; + } + case Resolution::kInt16: { + Write(detail::max( + -32767, detail::min(32767, value))); + break; + } + case Resolution::kInt32: { + Write(value); + break; + } + case Resolution::kFloat: { + Write(static_cast(value)); + break; + } + case Resolution::kIgnore: { + abort(); + } + } + } + + void WriteMapped( + double value, + double int8_scale, double int16_scale, double int32_scale, + Resolution res) { + switch (res) { + case Resolution::kInt8: { + Write(Saturate(value, int8_scale)); + break; + } + case Resolution::kInt16: { + Write(Saturate(value, int16_scale)); + break; + } + case Resolution::kInt32: { + Write(Saturate(value, int32_scale)); + break; + } + case Resolution::kFloat: { + Write(static_cast(value)); + break; + } + case Resolution::kIgnore: { + abort(); + } + } + } + + void WritePosition(double value, Resolution res) { + WriteMapped(value, 0.01, 0.0001, 0.00001, res); + } + + void WriteVelocity(double value, Resolution res) { + WriteMapped(value, 0.1, 0.00025, 0.00001, res); + } + + void WriteAccel(double value, Resolution res) { + WriteMapped(value, 0.05, 0.001, 0.00001, res); + } + + void WriteTorque(double value, Resolution res) { + WriteMapped(value, 0.5, 0.01, 0.001, res); + } + + void WritePwm(double value, Resolution res) { + WriteMapped(value, + 1.0 / 127.0, + 1.0 / 32767.0, + 1.0 / 2147483647.0, + res); + } + + void WriteVoltage(double value, Resolution res) { + WriteMapped(value, 0.5, 0.1, 0.001, res); + } + + void WriteTemperature(float value, Resolution res) { + WriteMapped(value, 1.0, 0.1, 0.001, res); + } + + void WriteTime(double value, Resolution res) { + WriteMapped(value, 0.01, 0.001, 0.000001, res); + } + + void WriteCurrent(double value, Resolution res) { + WriteMapped(value, 1.0, 0.1, 0.001, res); + } + + private: + uint8_t* const data_; + uint8_t* const size_; +}; + +/// Read typed values from a CAN frame. +class MultiplexParser { + public: + MultiplexParser(const CanData* frame) + : data_(&frame->data[0]), + size_(frame->size) {} + MultiplexParser(const uint8_t* data, uint8_t size) + : data_(data), + size_(size) {} + + uint16_t ReadVaruint() { + uint16_t result = 0; + uint16_t shift = 0; + + for (int8_t i = 0; i < 5; i++) { + if (remaining() == 0) { return result; } + + const auto this_byte = static_cast(Read()); + result |= (this_byte & 0x7f) << shift; + shift += 7; + + if ((this_byte & 0x80) == 0) { + return result; + } + } + return result; + } + + struct Result { + bool done = true; + uint16_t value = 0; + Resolution resolution = kIgnore; + + Result(bool done_in, uint16_t value_in, Resolution resolution_in) + : done(done_in), value(value_in), resolution(resolution_in) {} + + Result() {} + }; + + Result next() { + if (offset_ >= size_) { + // We are done. + return Result(true, 0, Resolution::kInt8); + } + + if (remaining_) { + remaining_--; + const auto this_register = current_register_++; + + // Do we actually have enough data? + if (offset_ + ResolutionSize(current_resolution_) > size_) { + return Result(true, 0, Resolution::kInt8); + } + + return Result(false, this_register, current_resolution_); + } + + // We need to look for another command. + while (offset_ < size_) { + const auto cmd = data_[offset_++]; + if (cmd == Multiplex::kNop) { continue; } + + // We are guaranteed to still need data. + if (offset_ >= size_) { + // Nope, we are out. + break; + } + + if (cmd >= 0x20 && cmd < 0x30) { + // This is a regular reply of some sort. + const auto id = (cmd >> 2) & 0x03; + current_resolution_ = [id]() { + switch (id) { + case 0: return Resolution::kInt8; + case 1: return Resolution::kInt16; + case 2: return Resolution::kInt32; + case 3: return Resolution::kFloat; + } + // we cannot reach this point + return Resolution::kInt8; + }(); + int8_t count = cmd & 0x03; + if (count == 0) { + count = data_[offset_++]; + + // We still need more data. + if (offset_ >= size_) { + break; + } + } + + if (count == 0) { + // Empty, guess we can ignore. + continue; + } + + current_register_ = ReadVaruint(); + remaining_ = count - 1; + + if (offset_ + ResolutionSize(current_resolution_) > size_) { + return Result(true, 0, Resolution::kInt8); + } + + return Result(false, current_register_++, current_resolution_); + } + + // For anything else, we'll just assume it is an error of some + // sort and stop parsing. + offset_ = size_; + break; + } + return Result(true, 0, Resolution::kInt8); + } + + template + T Read() { + if (offset_ + sizeof(T) > size_) { + abort(); + } + + T result = {}; + ::memcpy(&result, &data_[offset_], sizeof(T)); + offset_ += sizeof(T); + return result; + } + + template + double Nanify(T value) { + if (value == detail::numeric_limits::min()) { + return NaN; + } + return value; + } + + double ReadMapped(Resolution res, + double int8_scale, + double int16_scale, + double int32_scale) { + switch (res) { + case Resolution::kInt8: { + return Nanify(Read()) * int8_scale; + } + case Resolution::kInt16: { + return Nanify(Read()) * int16_scale; + } + case Resolution::kInt32: { + return Nanify(Read()) * int32_scale; + } + case Resolution::kFloat: { + return Read(); + } + default: { + break; + } + } + abort(); + } + + static constexpr int8_t kInt = 0; + static constexpr int8_t kPosition = 1; + static constexpr int8_t kVelocity = 2; + static constexpr int8_t kTorque = 3; + static constexpr int8_t kPwm = 4; + static constexpr int8_t kVoltage = 5; + static constexpr int8_t kTemperature = 6; + static constexpr int8_t kTime = 7; + static constexpr int8_t kCurrent = 8; + static constexpr int8_t kTheta = 9; + + double ReadConcrete(Resolution res, int8_t concrete_type) { +#ifndef ARDUINO + static constexpr double kMappingValues[] = { +#else + static constexpr float PROGMEM kMappingValues[] = { +#endif + 1.0, 1.0, 1.0, // kInt + 0.01, 0.0001, 0.00001, // kPosition + 0.1, 0.00025, 0.00001, // kVelocity + 0.5, 0.01, 0.001, // kTorque + 1.0 / 127.0, 1.0 / 32767.0, 1.0 / 2147483647.0, // kPwm + 0.5, 0.1, 0.001, // kVoltage + 1.0, 0.1, 0.001, // kTemperature + 0.01, 0.001, 0.000001, // kTime + 1.0, 0.1, 0.001, // kCurrent + 1.0 / 127.0 * M_PI, 1.0 / 32767.0 * M_PI, 1.0 / 2147483647.0 * M_PI, // kTheta + }; + +#ifndef ARDUINO + const double int8_scale = kMappingValues[concrete_type * 3 + 0]; + const double int16_scale = kMappingValues[concrete_type * 3 + 1]; + const double int32_scale = kMappingValues[concrete_type * 3 + 2]; +#else + const float int8_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 0); + const float int16_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 1); + const float int32_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 2); +#endif + + switch (res) { + case Resolution::kInt8: { + return Nanify(Read()) * int8_scale; + } + case Resolution::kInt16: { + return Nanify(Read()) * int16_scale; + } + case Resolution::kInt32: { + return Nanify(Read()) * int32_scale; + } + case Resolution::kFloat: { + return Read(); + } + default: { + break; + } + } + + abort(); + } + + int ReadInt(Resolution res) { + return static_cast(ReadConcrete(res, kInt)); + } + + double ReadPosition(Resolution res) { + return ReadConcrete(res, kPosition); + } + + double ReadVelocity(Resolution res) { + return ReadConcrete(res, kVelocity); + } + + double ReadTorque(Resolution res) { + return ReadConcrete(res, kTorque); + } + + double ReadPwm(Resolution res) { + return ReadConcrete(res, kPwm); + } + + double ReadVoltage(Resolution res) { + return ReadConcrete(res, kVoltage); + } + + double ReadTemperature(Resolution res) { + return ReadConcrete(res, kTemperature); + } + + double ReadTime(Resolution res) { + return ReadConcrete(res, kTime); + } + + double ReadCurrent(Resolution res) { + return ReadConcrete(res, kCurrent); + } + + void Ignore(Resolution res) { + offset_ += ResolutionSize(res); + } + + void ReadRaw(uint8_t* output, uint16_t size) { + if ((offset_ + size) > size_) { ::abort(); } + ::memcpy(output, &data_[offset_], size); + offset_ += size; + } + + uint16_t remaining() const { + return size_ - offset_; + } + + static int8_t ResolutionSize(Resolution res) { + switch (res) { + case Resolution::kInt8: return 1; + case Resolution::kInt16: return 2; + case Resolution::kInt32: return 4; + case Resolution::kFloat: return 4; + default: { break; } + } + return 1; + } + + private: + const uint8_t* const data_; + const uint8_t size_; + uint16_t offset_ = 0; + + int8_t remaining_ = 0; + Resolution current_resolution_ = Resolution::kIgnore; + uint16_t current_register_ = 0; +}; + +/// Determines how to group registers when encoding them to minimize +/// the required bytes. +class WriteCombiner { + public: + WriteCombiner(WriteCanData* frame, + int8_t base_command, + uint16_t start_register, + const Resolution* resolutions, + uint16_t resolutions_size) + : frame_(frame), + base_command_(base_command), + start_register_(start_register), + resolutions_(resolutions), + resolutions_size_(resolutions_size) {} + + ~WriteCombiner() { + if (offset_ != resolutions_size_) { + ::abort(); + } + } + + uint8_t reply_size() const { return reply_size_; } + + bool MaybeWrite() { + const auto this_offset = offset_; + offset_++; + + if (current_resolution_ == resolutions_[this_offset]) { + // We don't need to write any register operations here, and the + // value should go out only if requested. + return current_resolution_ != Resolution::kIgnore; + } + // We need to do some kind of framing. See how far ahead the new + // resolution goes. + const auto new_resolution = resolutions_[this_offset]; + current_resolution_ = new_resolution; + + // We are now in a new block of ignores. + if (new_resolution == Resolution::kIgnore) { + return false; + } + + int16_t count = 1; + for (uint16_t i = this_offset + 1; + i < resolutions_size_ && resolutions_[i] == new_resolution; + i++) { + count++; + } + + int8_t write_command = base_command_ + [&]() { + switch (new_resolution) { + case Resolution::kInt8: return 0x00; + case Resolution::kInt16: return 0x04; + case Resolution::kInt32: return 0x08; + case Resolution::kFloat: return 0x0c; + case Resolution::kIgnore: { + abort(); + } + } + return 0x00; + }(); + + const auto start_size = frame_->size(); + if (count <= 3) { + // Use the shorthand formulation. + frame_->Write(write_command + count); + } else { + // Nope, the long form. + frame_->Write(write_command); + frame_->Write(count); + } + frame_->WriteVaruint(start_register_ + this_offset); + const auto end_size = frame_->size(); + + reply_size_ += (end_size - start_size); + reply_size_ += count * MultiplexParser::ResolutionSize(new_resolution); + + return true; + } + + private: + WriteCanData* const frame_; + int8_t base_command_ = 0; + uint16_t start_register_ = 0; + const Resolution* const resolutions_; + uint16_t resolutions_size_ = 0; + + Resolution current_resolution_ = Resolution::kIgnore; + uint16_t offset_ = 0; + uint8_t reply_size_ = 0; +}; + +} +} diff --git a/arduino_firmware/end_effector/moteus_driver/moteus_protocol.h b/arduino_firmware/end_effector/moteus_driver/moteus_protocol.h new file mode 100644 index 0000000..eb2d6a7 --- /dev/null +++ b/arduino_firmware/end_effector/moteus_driver/moteus_protocol.h @@ -0,0 +1,1259 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file +/// +/// This file contains primitives used to encode and decode CAN-FD +/// messages for the mjbots moteus brushless controller. While C++, +/// it is designed to be usable in "minimal-C++" environments like +/// Arduino, in addition to fully standards conforming environments. + +#pragma once + +#include + +#ifndef ARDUINO + +#include +#include +#define NaN std::numeric_limits::quiet_NaN(); + +#else + +#define NaN (0.0 / 0.0) + +#endif + + +#include "moteus_multiplex.h" + +namespace mjbots { +namespace moteus { + +/// This is a single CAN-FD frame, its headers, and other associated +/// metadata, like which bus the message was sent or received from in +/// multi-bus systems. +struct CanFdFrame { + /////////////////////////////////////////// + /// First are the raw data from the bus. + + // Non-zero if this transport supports multiple busses. + int8_t bus = 0; + + uint32_t arbitration_id = 0; + uint8_t data[64] = {}; + uint8_t size = 0; + + enum Toggle { + kDefault, + kForceOff, + kForceOn, + }; + + Toggle brs = kDefault; + Toggle fdcan_frame = kDefault; + + ////////////////////////////////////////// + /// Next are parsed items for moteus frames. These are not required + /// to be set when sending a frame to a transport, but they will be + /// filled in on receipt. Higher level layers than the transport + /// layer may use them to populate the bus-level data. + + // If true, then the ID used is not calculated from destination and + // source, but is instead determined directly from arbitration_id. + bool raw = false; + + int8_t destination = 0; + int8_t source = 0; + uint16_t can_prefix = 0x0000; // A 13 bit CAN prefix + + //////////////////////////////////////// + /// Finally other hinting data. + + // Whether this frame is expected to elicit a response. + bool reply_required = false; + + // If this frame does elicit a response, how large is it expected to + // be. + uint8_t expected_reply_size = 0; +}; + + +/// The expected version associated with register 0x102. If it +/// differs from this, then semantics of one or more registers may +/// have changed. +enum { + kCurrentRegisterMapVersion = 5, +}; + +enum Register : uint16_t { + kMode = 0x000, + kPosition = 0x001, + kVelocity = 0x002, + kTorque = 0x003, + kQCurrent = 0x004, + kDCurrent = 0x005, + kAbsPosition = 0x006, + + kMotorTemperature = 0x00a, + kTrajectoryComplete = 0x00b, + kHomeState = 0x00c, + kVoltage = 0x00d, + kTemperature = 0x00e, + kFault = 0x00f, + + kPwmPhaseA = 0x010, + kPwmPhaseB = 0x011, + kPwmPhaseC = 0x012, + + kVoltagePhaseA = 0x014, + kVoltagePhaseB = 0x015, + kVoltagePhaseC = 0x016, + + kVFocTheta = 0x018, + kVFocVoltage = 0x019, + kVoltageDqD = 0x01a, + kVoltageDqQ = 0x01b, + + kCommandQCurrent = 0x01c, + kCommandDCurrent = 0x01d, + + kCommandPosition = 0x020, + kCommandVelocity = 0x021, + kCommandFeedforwardTorque = 0x022, + kCommandKpScale = 0x023, + kCommandKdScale = 0x024, + kCommandPositionMaxTorque = 0x025, + kCommandStopPosition = 0x026, + kCommandTimeout = 0x027, + + kPositionKp = 0x030, + kPositionKi = 0x031, + kPositionKd = 0x032, + kPositionFeedforward = 0x033, + kPositionCommand = 0x034, + + kControlPosition = 0x038, + kControlVelocity = 0x039, + kControlTorque = 0x03a, + kControlPositionError = 0x03b, + kControlVelocityError = 0x03c, + kControlTorqueError = 0x03d, + + kCommandStayWithinLowerBound = 0x040, + kCommandStayWithinUpperBound = 0x041, + kCommandStayWithinFeedforwardTorque = 0x042, + kCommandStayWithinKpScale = 0x043, + kCommandStayWithinKdScale = 0x044, + kCommandStayWithinPositionMaxTorque = 0x045, + kCommandStayWithinTimeout = 0x046, + + kEncoder0Position = 0x050, + kEncoder0Velocity = 0x051, + kEncoder1Position = 0x052, + kEncoder1Velocity = 0x053, + kEncoder2Position = 0x054, + kEncoder2Velocity = 0x055, + + kEncoderValidity = 0x058, + + kAux1GpioCommand = 0x05c, + kAux2GpioCommand = 0x05d, + kAux1GpioStatus = 0x05e, + kAux2GpioStatus = 0x05f, + + kAux1AnalogIn1 = 0x060, + kAux1AnalogIn2 = 0x061, + kAux1AnalogIn3 = 0x062, + kAux1AnalogIn4 = 0x063, + kAux1AnalogIn5 = 0x064, + + kAux2AnalogIn1 = 0x068, + kAux2AnalogIn2 = 0x069, + kAux2AnalogIn3 = 0x06a, + kAux2AnalogIn4 = 0x06b, + kAux2AnalogIn5 = 0x06c, + + kMillisecondCounter = 0x070, + kClockTrim = 0x071, + + kRegisterMapVersion = 0x102, + kSerialNumber = 0x120, + kSerialNumber1 = 0x120, + kSerialNumber2 = 0x121, + kSerialNumber3 = 0x122, + + kRezero = 0x130, + kSetOutputNearest = 0x130, + kSetOutputExact = 0x131, + kRequireReindex = 0x132, + kRecapturePositionVelocity = 0x133, + + kDriverFault1 = 0x140, + kDriverFault2 = 0x141, +}; + +enum class Mode { + kStopped = 0, + kFault = 1, + kEnabling = 2, + kCalibrating = 3, + kCalibrationComplete = 4, + kPwm = 5, + kVoltage = 6, + kVoltageFoc = 7, + kVoltageDq = 8, + kCurrent = 9, + kPosition = 10, + kPositionTimeout = 11, + kZeroVelocity = 12, + kStayWithin = 13, + kMeasureInd = 14, + kBrake = 15, + kNumModes, +}; + +enum class HomeState { + kRelative = 0, + kRotor = 1, + kOutput = 2, +}; + + +//////////////////////////////////////////////////////////////// +// Each command that can be issued is represented by a structure below +// with a few different sub-structs. Possibilities are: +// +// 'Command' items that are serialized in an outgoing message +// 'Format' the corresponding resolution for each item in 'Command' +// 'Result' the deserialized version of a response +// +// Additionally, there are two static methods possible: +// +// 'Make' - take a Command and Format, and serialize it +// 'Parse' - deserialize CAN data into a 'Result' structure + +struct EmptyMode { + struct Command {}; + struct Format {}; + static uint8_t Make(WriteCanData*, const Command&, const Format&) { + return 0; + } +}; + +struct Query { + struct ItemValue { + int16_t register_number = detail::numeric_limits::max(); + double value = 0.0; + }; + +#ifndef ARDUINO + static constexpr int16_t kMaxExtra = 16; +#else + static constexpr int16_t kMaxExtra = 8; +#endif + + struct Result { + Mode mode = Mode::kStopped; + double position = NaN; + double velocity = NaN; + double torque = NaN; + double q_current = NaN; + double d_current = NaN; + double abs_position = NaN; + double motor_temperature = NaN; + bool trajectory_complete = false; + HomeState home_state = HomeState::kRelative; + double voltage = NaN; + double temperature = NaN; + int8_t fault = 0; + + int8_t aux1_gpio = 0; + int8_t aux2_gpio = 0; + + // Before gcc-12, initializating non-POD array types can be + // painful if done in the idiomatic way with ={} inline. Instead + // we do it in the constructor. + // + // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=92385 + ItemValue extra[kMaxExtra]; + + Result() : extra() {} + }; + + struct ItemFormat { + int16_t register_number = detail::numeric_limits::max(); + Resolution resolution = moteus::kIgnore; + }; + + struct Format { + Resolution mode = kInt8; + Resolution position = kFloat; + Resolution velocity = kFloat; + Resolution torque = kFloat; + Resolution q_current = kIgnore; + Resolution d_current = kIgnore; + Resolution abs_position = kIgnore; + Resolution motor_temperature = kIgnore; + Resolution trajectory_complete = kIgnore; + Resolution home_state = kIgnore; + Resolution voltage = kInt8; + Resolution temperature = kInt8; + Resolution fault = kInt8; + + Resolution aux1_gpio = kIgnore; + Resolution aux2_gpio = kIgnore; + + // Any values here must be sorted by register number. + ItemFormat extra[kMaxExtra]; + + // gcc bug 92385 again + Format() : extra() {} + }; + + static uint8_t Make(WriteCanData* frame, const Format& format) { + uint8_t reply_size = 0; + + { + const Resolution kResolutions[] = { + format.mode, + format.position, + format.velocity, + format.torque, + format.q_current, + format.d_current, + format.abs_position, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kMode, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const Resolution kResolutions[] = { + format.motor_temperature, + format.trajectory_complete, + format.home_state, + format.voltage, + format.temperature, + format.fault, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kMotorTemperature, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const Resolution kResolutions[] = { + format.aux1_gpio, + format.aux2_gpio, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kAux1GpioStatus, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const int16_t size = [&]() { + for (int16_t i = 0; i < kMaxExtra; i++) { + if (format.extra[i].register_number == + detail::numeric_limits::max()) { + return i; + } + } + return kMaxExtra; + }(); + + if (size == 0) { return reply_size; } + const int16_t min_register_number = format.extra[0].register_number; + const int16_t max_register_number = format.extra[size - 1].register_number; + + const uint16_t required_registers = + max_register_number - min_register_number + 1; + + // An arbitrary limit to constrain the amount of stack we use + // below. + if (required_registers > 512) { ::abort(); } + +#ifndef ARDUINO + std::vector resolutions(required_registers); +#else + Resolution resolutions[required_registers]; +#endif + ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); + + for (int16_t this_register = min_register_number, index = 0; + this_register <= max_register_number; + this_register++) { + if (format.extra[index].register_number == this_register) { + resolutions[this_register - min_register_number] = + format.extra[index].resolution; + index++; + } else { + resolutions[this_register - min_register_number] = kIgnore; + } + } + WriteCombiner combiner( + frame, 0x10, min_register_number, + &resolutions[0], required_registers); + for (uint16_t i = 0; i < required_registers; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + return reply_size; + } + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + + return Parse(&parser); + } + + static Result Parse(const CanData* frame) { + MultiplexParser parser(frame); + + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + + int16_t extra_index = 0; + + while (true) { + const auto current = parser->next(); + if (current.done) { return result; } + + const auto res = current.resolution; + + switch (static_cast(current.value)) { + case Register::kMode: { + result.mode = static_cast(parser->ReadInt(res)); + break; + } + case Register::kPosition: { + result.position = parser->ReadPosition(res); + break; + } + case Register::kVelocity: { + result.velocity = parser->ReadVelocity(res); + break; + } + case Register::kTorque: { + result.torque = parser->ReadTorque(res); + break; + } + case Register::kQCurrent: { + result.q_current = parser->ReadCurrent(res); + break; + } + case Register::kDCurrent: { + result.d_current = parser->ReadCurrent(res); + break; + } + case Register::kAbsPosition: { + result.abs_position = parser->ReadPosition(res); + break; + } + case Register::kMotorTemperature: { + result.motor_temperature = parser->ReadTemperature(res); + break; + } + case Register::kTrajectoryComplete: { + result.trajectory_complete = parser->ReadInt(res) != 0; + break; + } + case Register::kHomeState: { + result.home_state = static_cast(parser->ReadInt(res)); + break; + } + case Register::kVoltage: { + result.voltage = parser->ReadVoltage(res); + break; + } + case Register::kTemperature: { + result.temperature = parser->ReadTemperature(res); + break; + } + case Register::kFault: { + result.fault = parser->ReadInt(res); + break; + } + case Register::kAux1GpioStatus: { + result.aux1_gpio = parser->ReadInt(res); + break; + } + case Register::kAux2GpioStatus: { + result.aux2_gpio = parser->ReadInt(res); + break; + } + default: { + if (extra_index < kMaxExtra) { + result.extra[extra_index].register_number = current.value; + result.extra[extra_index].value = + ParseGeneric(parser, current.value, res); + extra_index++; + } else { + // Ignore this value. + parser->ReadInt(res); + } + break; + } + } + } + } + + static double ParseGeneric(MultiplexParser* parser, + int16_t register_number, + Resolution resolution) { + const auto res = resolution; + + using R = Register; + using MP = MultiplexParser; + + struct RegisterDefinition { + uint16_t register_number; + uint8_t block_size; + int8_t concrete; + }; +#ifndef ARDUINO + static constexpr RegisterDefinition kRegisterDefinitions[] = { +#else + static constexpr RegisterDefinition PROGMEM kRegisterDefinitions[] = { +#endif + { R::kMode, 1, MP::kInt, }, + { R::kPosition, 1, MP::kPosition, }, + { R::kVelocity, 1, MP::kVelocity, }, + { R::kTorque, 1, MP::kTorque, }, + { R::kQCurrent, 2, MP::kCurrent, }, + // { R::kDCurrent, 1, MP::kCurrent, }, + { R::kAbsPosition, 1, MP::kPosition, }, + { R::kMotorTemperature, 1, MP::kTemperature, }, + { R::kTrajectoryComplete, 2, MP::kInt, }, + // { R::kHomeState, 1, MP::kInt, }, + { R::kVoltage, 1, MP::kVoltage, }, + { R::kTemperature, 1, MP::kTemperature, }, + { R::kFault, 1, MP::kInt, }, + + { R::kPwmPhaseA, 3, MP::kPwm, }, + // { R::kPwmPhaseB, 1, MP::kPwm, }, + // { R::kPwmPhaseC, 1, MP::kPwm, }, + + { R::kVoltagePhaseA, 3, MP::kVoltage, }, + // { R::kVoltagePhaseB, 1, MP::kVoltage, }, + // { R::kVoltagePhaseC, 1, MP::kVoltage, }, + + { R::kVFocTheta, 1, MP::kTheta, }, + { R::kVFocVoltage, 3, MP::kVoltage, }, + // { R::kVoltageDqD, 1, MP::kVoltage, }, + // { R::kVoltageDqQ, 1, MP::kVoltage, }, + + { R::kCommandQCurrent, 2, MP::kCurrent, }, + // { R::kCommandDCurrent, 1, MP::kCurrent, }, + + { R::kCommandPosition, 1, MP::kPosition, }, + { R::kCommandVelocity, 1, MP::kVelocity, }, + { R::kCommandFeedforwardTorque, 1, MP::kTorque, }, + { R::kCommandKpScale, 2, MP::kPwm, }, + // { R::kCommandKdScale, 1, MP::kPwm, }, + { R::kCommandPositionMaxTorque, 1, MP::kTorque, }, + { R::kCommandStopPosition, 1, MP::kPosition, }, + { R::kCommandTimeout, 1, MP::kTime, }, + + { R::kPositionKp, 5, MP::kTorque, }, + // { R::kPositionKi, 1, MP::kTorque, }, + // { R::kPositionKd, 1, MP::kTorque, }, + // { R::kPositionFeedforward, 1, MP::kTorque, }, + // { R::kPositionCommand, 1, MP::kTorque, }, + + { R::kControlPosition, 1, MP::kPosition, }, + { R::kControlVelocity, 1, MP::kVelocity, }, + { R::kControlTorque, 1, MP::kTorque, }, + { R::kControlPositionError, 1, MP::kPosition, }, + { R::kControlVelocityError, 1, MP::kVelocity, }, + { R::kControlTorqueError, 1, MP::kTorque, }, + + { R::kCommandStayWithinLowerBound, 2, MP::kPosition, }, + // { R::kCommandStayWithinUpperBound, 1, MP::kPosition, }, + { R::kCommandStayWithinFeedforwardTorque, 1, MP::kTorque, }, + { R::kCommandStayWithinKpScale, 2, MP::kPwm, }, + // { R::kCommandStayWithinKdScale, 1, MP::kPwm, }, + { R::kCommandStayWithinPositionMaxTorque, 1, MP::kTorque, }, + { R::kCommandStayWithinTimeout, 1, MP::kTime, }, + + { R::kEncoder0Position, 1, MP::kPosition, }, + { R::kEncoder0Velocity, 1, MP::kVelocity, }, + { R::kEncoder1Position, 1, MP::kPosition, }, + { R::kEncoder1Velocity, 1, MP::kVelocity, }, + { R::kEncoder2Position, 1, MP::kPosition, }, + { R::kEncoder2Velocity, 1, MP::kVelocity, }, + + { R::kEncoderValidity, 1, MP::kInt, }, + + { R::kAux1GpioCommand, 4, MP::kInt, }, + // { R::kAux2GpioCommand, 1, MP::kInt, }, + // { R::kAux1GpioStatus, 1, MP::kInt, }, + // { R::kAux2GpioStatus, 1, MP::kInt, }, + + { R::kAux1AnalogIn1, 5, MP::kPwm, }, + // { R::kAux1AnalogIn2, 1, MP::kPwm, }, + // { R::kAux1AnalogIn3, 1, MP::kPwm, }, + // { R::kAux1AnalogIn4, 1, MP::kPwm, }, + // { R::kAux1AnalogIn5, 1, MP::kPwm, }, + + { R::kAux2AnalogIn1, 5, MP::kPwm, }, + // { R::kAux2AnalogIn2, 1, MP::kPwm, }, + // { R::kAux2AnalogIn3, 1, MP::kPwm, }, + // { R::kAux2AnalogIn4, 1, MP::kPwm, }, + // { R::kAux2AnalogIn5, 1, MP::kPwm, }, + + { R::kMillisecondCounter, 2, MP::kInt, }, + // { R::kClockTrim, 1, MP::kInt, }, + + { R::kRegisterMapVersion, 1, MP::kInt, }, + { R::kSerialNumber1, 3, MP::kInt, }, + // { R::kSerialNumber2, 1, MP::kInt, }, + // { R::kSerialNumber3, 1, MP::kInt, }, + + { R::kSetOutputNearest, 3, MP::kInt, }, + // { R::kSetOutputExact, 1, MP::kInt, }, + // { R::kRequireReindex, 1, MP::kInt, }, + // { R::kRecapturePositionVelocity, 1, MP::kInt, } + + { R::kDriverFault1, 2, MP::kInt, }, + // { R::kDriverFault2, 1, MP::kInt, }, + + }; + for (uint16_t i = 0; + i < sizeof(kRegisterDefinitions) / sizeof (*kRegisterDefinitions); + i ++) { + +#ifndef ARDUINO + const int16_t start_reg = kRegisterDefinitions[i].register_number; + const uint8_t block_size = kRegisterDefinitions[i].block_size; + const int8_t concrete_type = kRegisterDefinitions[i].concrete; +#else + const int16_t start_reg = pgm_read_word_near(&kRegisterDefinitions[i].register_number); + const uint8_t block_size = pgm_read_byte_near(&kRegisterDefinitions[i].block_size); + const int8_t concrete_type = pgm_read_byte_near(&kRegisterDefinitions[i].concrete); +#endif + if (register_number >= start_reg && + register_number < (start_reg + block_size)) { + return parser->ReadConcrete(res, concrete_type); + } + } + return parser->ReadInt(res); + } +}; + +struct GenericQuery { + struct Command {}; + + struct ItemValue { + int16_t register_number = -1; + double value = 0.0; + }; + + // A CAN-FD frame can only have 64 bytes, so we definitely can't + // have more than 64 items in a single one ever. + static constexpr int16_t kMaxItems = 64; + + struct Result { + ItemValue values[kMaxItems] = {}; + }; + + struct ItemFormat { + int16_t register_number = detail::numeric_limits::max(); + Resolution resolution = kIgnore; + }; + + struct Format { + // These values must be sorted by register number. + ItemFormat values[kMaxItems] = {}; + }; + + static int ItemFormatSort(const void* lhs_in, const void* rhs_in) { + const auto* lhs = reinterpret_cast(lhs_in); + const auto* rhs = reinterpret_cast(rhs_in); + + return lhs->register_number - rhs->register_number; + } + + static uint8_t Make(WriteCanData* frame, const Command&, const Format& format) { + const int16_t size = [&]() { + for (int16_t i = 0; i < kMaxItems; i++) { + if (format.values[i].register_number == + detail::numeric_limits::max()) { + return i; + } + } + return kMaxItems; + }(); + + if (size == 0) { return 0; } + const int16_t min_register_number = format.values[0].register_number; + const int16_t max_register_number = format.values[size - 1].register_number; + + const uint16_t required_registers = max_register_number - min_register_number + 1; + + // An arbitrary limit to constrain the amount of stack we use + // below. + if (required_registers > 512) { ::abort(); } + +#ifndef ARDUINO + std::vector resolutions(required_registers); +#else + Resolution resolutions[required_registers]; +#endif + ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); + + for (int16_t this_register = min_register_number, index = 0; + this_register <= max_register_number; + this_register++) { + if (format.values[index].register_number == this_register) { + resolutions[this_register - min_register_number] = + format.values[index].resolution; + index++; + } else { + resolutions[this_register - min_register_number] = kIgnore; + } + } + WriteCombiner combiner( + frame, 0x10, min_register_number, + &resolutions[0], required_registers); + for (uint16_t i = 0; i < required_registers; i++) { + combiner.MaybeWrite(); + } + + return combiner.reply_size(); + } + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + + return Parse(&parser); + } + + static Result Parse(const CanData* frame) { + MultiplexParser parser(frame); + + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + + int16_t index = 0; + + while (true) { + if (index >= kMaxItems) { return result; } + + const auto current = parser->next(); + if (current.done) { return result; } + + result.values[index].register_number = current.value; + result.values[index].value = + Query::ParseGeneric(parser, current.value, current.resolution); + + index++; + } + } +}; + +struct PositionMode { + struct Command { + double position = 0.0; + double velocity = 0.0; + double feedforward_torque = 0.0; + double kp_scale = 1.0; + double kd_scale = 1.0; + double maximum_torque = NaN; + double stop_position = NaN; + double watchdog_timeout = NaN; + double velocity_limit = NaN; + double accel_limit = NaN; + double fixed_voltage_override = NaN; + }; + + struct Format { + Resolution position = kFloat; + Resolution velocity = kFloat; + Resolution feedforward_torque = kIgnore; + Resolution kp_scale = kIgnore; + Resolution kd_scale = kIgnore; + Resolution maximum_torque = kIgnore; + Resolution stop_position = kIgnore; + Resolution watchdog_timeout = kIgnore; + Resolution velocity_limit = kIgnore; + Resolution accel_limit = kIgnore; + Resolution fixed_voltage_override = kIgnore; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kPosition); + + // Now we use some heuristics to try and group consecutive registers + // of the same resolution together into larger writes. + const Resolution kResolutions[] = { + format.position, + format.velocity, + format.feedforward_torque, + format.kp_scale, + format.kd_scale, + format.maximum_torque, + format.stop_position, + format.watchdog_timeout, + format.velocity_limit, + format.accel_limit, + format.fixed_voltage_override, + }; + WriteCombiner combiner( + frame, 0x00, + Register::kCommandPosition, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePosition(command.position, format.position); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.velocity, format.velocity); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.feedforward_torque, format.feedforward_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kp_scale, format.kp_scale); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kd_scale, format.kd_scale); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.maximum_torque, format.maximum_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePosition(command.stop_position, format.stop_position); + } + if (combiner.MaybeWrite()) { + frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.velocity_limit, format.velocity_limit); + } + if (combiner.MaybeWrite()) { + frame->WriteAccel(command.accel_limit, format.accel_limit); + } + if (combiner.MaybeWrite()) { + frame->WriteVoltage(command.fixed_voltage_override, + format.fixed_voltage_override); + } + return 0; + } +}; + +/// The voltage-mode FOC command. +struct VFOCMode { + struct Command { + double theta_rad = 0.0; + double voltage = 0.0; + double theta_rad_rate = 0.0; + }; + + struct Format { + Resolution theta_rad = kFloat; + Resolution voltage = kFloat; + Resolution theta_rad_rate = kFloat; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kVoltageFoc); + + const Resolution kResolutions[] = { + format.theta_rad, + format.voltage, + kIgnore, + kIgnore, + kIgnore, + kIgnore, + format.theta_rad_rate, + }; + WriteCombiner combiner( + frame, 0x00, + Register::kVFocTheta, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePwm(command.theta_rad / M_PI, format.theta_rad); + } + if (combiner.MaybeWrite()) { + frame->WriteVoltage(command.voltage, format.voltage); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.theta_rad_rate / M_PI, format.theta_rad_rate); + } + + return 0; + } +}; + +/// DQ phase current command. +struct CurrentMode { + struct Command { + double d_A = 0.0; + double q_A = 0.0; + }; + + struct Format { + Resolution d_A = kFloat; + Resolution q_A = kFloat; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kCurrent); + + const Resolution kResolutions[] = { + format.d_A, + format.q_A, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kCommandQCurrent, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WriteCurrent(command.q_A, format.q_A); + } + if (combiner.MaybeWrite()) { + frame->WriteCurrent(command.d_A, format.d_A); + } + + return 0; + } +}; + +struct StayWithinMode { + struct Command { + double lower_bound = NaN; + double upper_bound = NaN; + double feedforward_torque = 0.0; + double kp_scale = 1.0; + double kd_scale = 1.0; + double maximum_torque = 0.0; + double watchdog_timeout = NaN; + }; + + struct Format { + Resolution lower_bound = kFloat; + Resolution upper_bound = kFloat; + Resolution feedforward_torque = kIgnore; + Resolution kp_scale = kIgnore; + Resolution kd_scale = kIgnore; + Resolution maximum_torque = kIgnore; + Resolution watchdog_timeout = kIgnore; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kStayWithin); + + const Resolution kResolutions[] = { + format.lower_bound, + format.upper_bound, + format.feedforward_torque, + format.kp_scale, + format.kd_scale, + format.maximum_torque, + format.watchdog_timeout, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kCommandStayWithinLowerBound, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePosition(command.lower_bound, format.lower_bound); + } + if (combiner.MaybeWrite()) { + frame->WritePosition(command.upper_bound, format.upper_bound); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.feedforward_torque, + format.feedforward_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kp_scale, format.kp_scale); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kd_scale, format.kd_scale); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.maximum_torque, format.maximum_torque); + } + if (combiner.MaybeWrite()) { + frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); + } + return 0; + } +}; + +struct BrakeMode { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, + const Command&, + const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kBrake); + return 0; + } +}; + +struct StopMode { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, + const Command&, + const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kStopped); + return 0; + } +}; + +struct GpioWrite { + struct Command { + int8_t aux1 = 0; + int8_t aux2 = 0; + }; + + struct Format { + Resolution aux1 = kInt8; + Resolution aux2 = kInt8; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + const Resolution kResolutions[] = { + format.aux1, + format.aux2, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kAux1GpioCommand, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WriteInt(command.aux1, format.aux1); + } + if (combiner.MaybeWrite()) { + frame->WriteInt(command.aux2, format.aux2); + } + return 0; + } +}; + +struct OutputNearest { + struct Command { + double position = 0.0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteFloat | 0x01); + frame->WriteVaruint(Register::kSetOutputNearest); + frame->Write(command.position); + return 0; + } +}; + +struct OutputExact { + struct Command { + double position = 0.0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteFloat | 0x01); + frame->WriteVaruint(Register::kSetOutputExact); + frame->Write(command.position); + return 0; + } +}; + +struct RequireReindex { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRequireReindex); + frame->Write(1); + return 0; + } +}; + +struct RecapturePositionVelocity { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRecapturePositionVelocity); + frame->Write(1); + return 0; + } +}; + +struct DiagnosticWrite { + struct Command { + int8_t channel = 1; + const char* data = nullptr; + int8_t size = 0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kClientToServer); + frame->Write(command.channel); + frame->Write(command.size); + frame->Write(command.data, command.size); + return 0; + } +}; + +struct DiagnosticRead { + struct Command { + int8_t channel = 1; + int8_t max_length = 48; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kClientPollServer); + frame->Write(command.channel); + frame->Write(command.max_length); + return command.max_length + 3; + } +}; + +struct DiagnosticResponse { + struct Result { + int8_t channel = 1; + uint8_t data[64] = {}; + int8_t size = 0; + }; + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + result.channel = -1; + + if (parser->remaining() < 3) { return result; } + + const auto action = parser->Read(); + if (action != Multiplex::kServerToClient) { return result; } + const auto channel = parser->Read(); + + const uint16_t size = parser->ReadVaruint(); + if (parser->remaining() < size) { return result; } + + result.channel = channel; + result.size = size; + parser->ReadRaw(result.data, size); + + return result; + } +}; + +struct ClockTrim { + struct Command { + int32_t trim = 0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteInt32 | 0x01); + frame->WriteVaruint(Register::kClockTrim); + frame->Write(command.trim); + return 0; + } +}; + +} +}