diff --git a/README.md b/README.md index cdeb6fe..26395d2 100644 --- a/README.md +++ b/README.md @@ -2,17 +2,15 @@ ## Summary ## -This is a C++ library that provides a convenient API for controlling and monitoring an mjbots moteus brushless servo controller. It uses the ACAN2517FD library to perform CAN-FD communications, but could be easily modified to work with other libraries. +This is a C++ library that provides a convenient API for controlling and monitoring an mjbots moteus brushless servo controller. It uses the ACAN4 library to perform CAN-FD communications, but could be easily modified to work with other libraries. ## Getting started ## Note that this library can only communicate and operate a moteus controller which has already been calibrated. Calibration currently can not be performed via an Arduino and requires `moteus_tool` executing on a desktop computer with some CAN-FD adapter. See the moteus [getting started guide](https://github.com/mjbots/moteus/blob/main/docs/getting_started.md#calibration) for details. The only controllers sold by mjbots.com which are pre-calibrated are those included in development kits. ### Hardware ### +For Teensy 4.x support, you will need a 3.3v compatible CAN transceiver such as the Adafruit TJA1051T/3. Only CAN3 (pins 30/31) support FD mode. Connected the CRX to RX and CTX to TX on the transceiver. From the transceiver to the Moteus CAN-H, CAN-L, and GND should be connected in standard way. The CAN-FD bus needs to be connected to moteus, typically this would be at least the CANL and CANH wires, and likely the ground as well. For more than 2 or 3 controllers, separate 120 ohm termination resistors will be required on each end of the CAN bus. Some Arduino CAN-FD shields, like the CANBed FD, have one termination resistor built in. -You will need to have hardware that is compatible with the [ACAN2517FD library](https://github.com/pierremolinaro/acan2517FD), this will be something that has the MCP2517FD or MCP2518FD CAN-FD controller onboard. - -The CAN-FD bus needs to be connected to moteus, typically this would be at least the CANL and CANH wires, and likely the ground as well. For more than 2 or 3 controllers, separate 120 ohm termination resistors will be required on each end of the CAN bus. Some Arduino CAN-FD shields, like the CANBed FD, have one termination resistor built in. ### Software ### @@ -25,7 +23,7 @@ If you are using version 1.6.x or later of the Arduino software (IDE) you can us If this does not work, you can manually install the library: -1. Download the [latest release archive from GitHub](https://github.com/mjbots/moteus-arduino/releases) and decompress it +1. Download the [latest release archive from GitHub](https://github.com/kylevernyi/moteus-teensy) and decompress it 2. Rename the folder "moteus-arduino" to "moteus" 3. Drag the "moteus" folder into the "libraries" directory inside your Arduino sketchbook directory. You can view your sketchbook location by opening the "File" menu and selecting "Preferences" in the Arduino IDE. If there is not already a "libraries" folder in that location, you should make the folder yourself. 4. After installing the library, restart the Arduino IDE. diff --git a/examples/BasicControl/BasicControl.ino b/examples/BasicControl/BasicControl.ino index 689b3e7..91c8e53 100644 --- a/examples/BasicControl/BasicControl.ino +++ b/examples/BasicControl/BasicControl.ino @@ -1,90 +1,46 @@ -//—————————————————————————————————————————————————————————————————————————————— -// Demonstration of control and monitoring of 2 moteus controllers -// running on a CANBed FD from longan labs. -// * https://mjbots.com/products/moteus-r4-11 -// * https://www.longan-labs.cc/1030009.html -// -// Controller ID 1 is moved through a sine wave pattern, while -// controller ID 2 just has a brake command sent. -// —————————————————————————————————————————————————————————————————————————————— - -#include -#include - -//—————————————————————————————————————————————————————————————————————————————— -// The following pins are selected for the CANBed FD board. -//—————————————————————————————————————————————————————————————————————————————— - -static const byte MCP2517_SCK = 9 ; // SCK input of MCP2517 -static const byte MCP2517_SDI = 10 ; // SDI input of MCP2517 -static const byte MCP2517_SDO = 11 ; // SDO output of MCP2517 - -static const byte MCP2517_CS = 17 ; // CS input of MCP2517 -static const byte MCP2517_INT = 7 ; // INT output of MCP2517 +#ifndef __IMXRT1062__ + #error "This sketch should be compiled for Teensy 4.x" +#endif -static uint32_t gNextSendMillis = 0; - -//—————————————————————————————————————————————————————————————————————————————— -// ACAN2517FD Driver object -//—————————————————————————————————————————————————————————————————————————————— -ACAN2517FD can (MCP2517_CS, SPI, MCP2517_INT) ; +#include +#include "Moteus.h" -Moteus moteus1(can, []() { +Moteus moteus1( +[]() { // lambda function which initializes options Moteus::Options options; options.id = 1; return options; -}()); -Moteus moteus2(can, []() { - Moteus::Options options; - options.id = 2; - return options; -}()); +} +()); Moteus::PositionMode::Command position_cmd; -void setup() { - pinMode (LED_BUILTIN, OUTPUT); - - // Let the world know we have begun! +void setup () { + // Let the world know we have begun! Serial.begin(115200); while (!Serial) {} Serial.println(F("started")); - SPI.begin(); - - // This operates the CAN-FD bus at 1Mbit for both the arbitration - // and data rate. Most arduino shields cannot operate at 5Mbps - // correctly, so the moteus Arduino library permanently disables - // BRS. - ACAN2517FDSettings settings( - ACAN2517FDSettings::OSC_20MHz, 1000ll * 1000ll, DataBitRateFactor::x1); + ACAN_T4FD_Settings settings (1000000, DataBitRateFactor::x1) ; - // The atmega32u4 on the CANbed has only a tiny amount of memory. - // The ACAN2517FD driver needs custom settings so as to not exhaust - // all of SRAM just with its buffers. - settings.mArbitrationSJW = 2; - settings.mDriverTransmitFIFOSize = 1; - settings.mDriverReceiveFIFOSize = 2; - - const uint32_t errorCode = can.begin(settings, [] { can.isr(); }); - - while (errorCode != 0) { - Serial.print(F("CAN error 0x")); - Serial.println(errorCode, HEX); - delay(1000); + const uint32_t errorCode = ACAN_T4::can3.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("can3 ok") ; + }else{ + Serial.print ("Error can3: 0x") ; + Serial.println (errorCode, HEX) ; } - // To clear any faults the controllers may have, we start by sending - // a stop command to each. moteus1.SetStop(); - moteus2.SetStop(); Serial.println(F("all stopped")); } + +static uint32_t gNextSendMillis = 0; uint16_t gLoopCount = 0; -void loop() { +void loop () { // We intend to send control frames every 20ms. const auto time = millis(); if (gNextSendMillis >= time) { return; } @@ -94,15 +50,12 @@ void loop() { Moteus::PositionMode::Command cmd; cmd.position = NaN; - cmd.velocity = 0.2 * ::sin(time / 1000.0); - + cmd.velocity = 5*::sin(time / 1000.0); moteus1.SetPosition(cmd); - moteus2.SetBrake(); if (gLoopCount % 5 != 0) { return; } // Only print our status every 5th cycle, so every 1s. - Serial.print(F("time ")); Serial.print(gNextSendMillis); @@ -115,7 +68,6 @@ void loop() { }; print_moteus(moteus1.last_result().values); - Serial.print(F(" / ")); - print_moteus(moteus2.last_result().values); Serial.println(); + } diff --git a/examples/DiagnosticProtocol/DiagnosticProtocol.ino b/examples/DiagnosticProtocol/DiagnosticProtocol.ino deleted file mode 100644 index e4e81be..0000000 --- a/examples/DiagnosticProtocol/DiagnosticProtocol.ino +++ /dev/null @@ -1,147 +0,0 @@ -//—————————————————————————————————————————————————————————————————————————————— -// Shows how to use the moteus diagnostic protocol to set and read -// configurable values on a CANBed FD from longan labs. -// * https://mjbots.com/products/moteus-r4-11 -// * https://www.longan-labs.cc/1030009.html -// —————————————————————————————————————————————————————————————————————————————— - -#include -#include - -//—————————————————————————————————————————————————————————————————————————————— -// The following pins are selected for the CANBed FD board. -//—————————————————————————————————————————————————————————————————————————————— - -static const byte MCP2517_SCK = 9 ; // SCK input of MCP2517 -static const byte MCP2517_SDI = 10 ; // SDI input of MCP2517 -static const byte MCP2517_SDO = 11 ; // SDO output of MCP2517 - -static const byte MCP2517_CS = 17 ; // CS input of MCP2517 -static const byte MCP2517_INT = 7 ; // INT output of MCP2517 - -static uint32_t gNextSendMillis = 0; - -//—————————————————————————————————————————————————————————————————————————————— -// ACAN2517FD Driver object -//—————————————————————————————————————————————————————————————————————————————— - -ACAN2517FD can (MCP2517_CS, SPI, MCP2517_INT) ; - -Moteus moteus1(can, []() { - Moteus::Options options; - options.id = 1; - // By default, only position and velocity are queried. Additional - // fields can be requested by changing their resolution in the - // options structure. - options.query_format.torque = Moteus::kFloat; - return options; -}()); - - -Moteus::PositionMode::Command position_cmd; -Moteus::PositionMode::Format position_fmt; - - -void setup() { - pinMode (LED_BUILTIN, OUTPUT); - - // Let the world know we have begun! - Serial.begin(115200); - while (!Serial) {} - Serial.println(F("started")); - - SPI.begin(); - - // This operates the CAN-FD bus at 1Mbit for both the arbitration - // and data rate. Most arduino shields cannot operate at 5Mbps - // correctly, so the moteus Arduino library permanently disables - // BRS. - ACAN2517FDSettings settings( - ACAN2517FDSettings::OSC_20MHz, 1000ll * 1000ll, DataBitRateFactor::x1); - - // The atmega32u4 on the CANbed has only a tiny amount of memory. - // The ACAN2517FD driver needs custom settings so as to not exhaust - // all of SRAM just with its buffers. - settings.mArbitrationSJW = 2; - settings.mDriverTransmitFIFOSize = 1; - settings.mDriverReceiveFIFOSize = 2; - - const uint32_t errorCode = can.begin(settings, [] { can.isr(); }); - - while (errorCode != 0) { - Serial.print(F("CAN error 0x")); - Serial.println(errorCode, HEX); - delay(1000); - } - - // First we'll clear faults. - moteus1.SetStop(); - Serial.println(F("all stopped")); - - // Now we will use the diagnostic protocol to ensure some - // configurable parameters are set to our liking. - - // First, in case this motor has been opened with tview, we will try - // to stop any unsolicited data that may be occurring. - moteus1.DiagnosticCommand(F("tel stop")); - moteus1.SetDiagnosticFlush(); - - // Now we can send some commands to configure things. - moteus1.DiagnosticCommand(F("conf set servo.pid_position.kp 2.0")); - moteus1.DiagnosticCommand(F("conf set servo.pid_position.kd 0.1")); - - // Finally, verify that our config was set properly. - const auto current_kp = - moteus1.DiagnosticCommand(F("conf get servo.pid_position.kp"), - Moteus::kExpectSingleLine); - const auto current_kd = - moteus1.DiagnosticCommand(F("conf get servo.pid_position.kd"), - Moteus::kExpectSingleLine); - - Serial.print("Current config: kp="); - Serial.print(current_kp); - Serial.print(" kd="); - Serial.print(current_kd); - Serial.println(); - - position_fmt.velocity_limit = Moteus::kFloat; - position_fmt.accel_limit = Moteus::kFloat; -} - -uint16_t gLoopCount = 0; - -void loop() { - // We intend to send control frames every 20ms. - const auto time = millis(); - if (gNextSendMillis >= time) { return; } - - gNextSendMillis += 20; - gLoopCount++; - - Moteus::PositionMode::Command cmd; - // Oscillate between position 0.5 and position 0.1 every 2s. - cmd.position = (gNextSendMillis / 2000) % 2 ? 0.5 : 0.1; - cmd.velocity = 0.0; - cmd.velocity_limit = 2.0; - cmd.accel_limit = 3.0; - - moteus1.SetPosition(cmd, &position_fmt); - - if (gLoopCount % 5 != 0) { return; } - - // Only print our status every 5th cycle, so every 1s. - - Serial.print(F("time ")); - Serial.print(gNextSendMillis); - - const auto& v = moteus1.last_result().values; - Serial.print(F(" mode=")); - Serial.print(static_cast(v.mode)); - Serial.print(F(" pos=")); - Serial.print(v.position); - Serial.print(F(" vel=")); - Serial.print(v.velocity); - Serial.print(F(" torque=")); - Serial.print(v.torque); - Serial.println(); -} diff --git a/examples/WaitComplete/WaitComplete.ino b/examples/WaitComplete/WaitComplete.ino deleted file mode 100644 index 0a253a1..0000000 --- a/examples/WaitComplete/WaitComplete.ino +++ /dev/null @@ -1,109 +0,0 @@ -//—————————————————————————————————————————————————————————————————————————————— - -// Demonstrates how to use SetPositionWaitComplete to wait until the -// exact time that a trajectory motion is completed. Intended to -// execute on a CANBed FD from longan labs. -// * https://mjbots.com/products/moteus-r4-11 -// * https://www.longan-labs.cc/1030009.html -// —————————————————————————————————————————————————————————————————————————————— - -#include -#include - -//—————————————————————————————————————————————————————————————————————————————— -// The following pins are selected for the CANBed FD board. -//—————————————————————————————————————————————————————————————————————————————— - -static const byte MCP2517_SCK = 9 ; // SCK input of MCP2517 -static const byte MCP2517_SDI = 10 ; // SDI input of MCP2517 -static const byte MCP2517_SDO = 11 ; // SDO output of MCP2517 - -static const byte MCP2517_CS = 17 ; // CS input of MCP2517 -static const byte MCP2517_INT = 7 ; // INT output of MCP2517 - -//—————————————————————————————————————————————————————————————————————————————— -// ACAN2517FD Driver object -//—————————————————————————————————————————————————————————————————————————————— - -ACAN2517FD can (MCP2517_CS, SPI, MCP2517_INT) ; - -Moteus moteus1(can, []() { - Moteus::Options options; - options.id = 1; - return options; -}()); - - -Moteus::PositionMode::Command position_cmd; -Moteus::PositionMode::Format position_fmt; - - -void setup() { - pinMode (LED_BUILTIN, OUTPUT); - - // Let the world know we have begun! - Serial.begin(115200); - while (!Serial) {} - Serial.println(F("started")); - - SPI.begin(); - - // This operates the CAN-FD bus at 1Mbit for both the arbitration - // and data rate. Most arduino shields cannot operate at 5Mbps - // correctly, so the moteus Arduino library permanently disables - // BRS. - ACAN2517FDSettings settings( - ACAN2517FDSettings::OSC_20MHz, 1000ll * 1000ll, DataBitRateFactor::x1); - - // The atmega32u4 on the CANbed has only a tiny amount of memory. - // The ACAN2517FD driver needs custom settings so as to not exhaust - // all of SRAM just with its buffers. - settings.mArbitrationSJW = 2; - settings.mDriverTransmitFIFOSize = 1; - settings.mDriverReceiveFIFOSize = 2; - - const uint32_t errorCode = can.begin(settings, [] { can.isr(); }); - - while (errorCode != 0) { - Serial.print(F("CAN error 0x")); - Serial.println(errorCode, HEX); - delay(1000); - } - - // First we'll clear faults. - moteus1.SetStop(); - Serial.println(F("all stopped")); - - position_fmt.velocity_limit = Moteus::kFloat; - position_fmt.accel_limit = Moteus::kFloat; - - position_cmd.velocity_limit = 2.0; - position_cmd.accel_limit = 3.0; -} - -void loop() { - auto print_state = [&]() { - const auto& v = moteus1.last_result().values; - Serial.print(F(" mode=")); - Serial.print(static_cast(v.mode)); - Serial.print(F(" pos=")); - Serial.print(v.position); - Serial.print(F(" vel=")); - Serial.print(v.velocity); - Serial.print(F(" torque=")); - Serial.print(v.torque); - Serial.println(); - }; - - position_cmd.position = 0.1; - moteus1.SetPositionWaitComplete(position_cmd, 0.02, &position_fmt); - - Serial.print(F("sent 0.1 ")); - print_state(); - - position_cmd.position = 0.5; - moteus1.SetPositionWaitComplete(position_cmd, 0.02, &position_fmt); - - Serial.print(F("sent 0.5 ")); - print_state(); -} diff --git a/src/Moteus.h b/src/Moteus.h index 45a45a6..6f7b266 100644 --- a/src/Moteus.h +++ b/src/Moteus.h @@ -74,10 +74,14 @@ class Moteus { Options() {} }; - Moteus(ACAN2517FD& can_bus, - const Options& options = {}) - : can_bus_(can_bus), - options_(options) { + Options options_; + + Moteus() + { + } + + void Initialize() + { mm::CanData can_data; mm::WriteCanData query_write(&can_data); mm::Query::Make(&query_write, options_.query_format); @@ -96,10 +100,8 @@ class Moteus { // 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 ? @@ -558,13 +560,8 @@ class Moteus { 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); + if (!ACAN_T4::can3.receiveFD(rx_msg)) { return false;} const int8_t source = (rx_msg.id >> 8) & 0x7f; const int8_t destination = (rx_msg.id & 0x7f); @@ -626,21 +623,15 @@ class Moteus { 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(); + + int32_t send_status = ACAN_T4::can3.tryToSendReturnStatusFD(can_message); return frame.reply_required; } bool ExecuteSingleCommand(const mm::CanFdFrame& frame) { const bool reply_required = BeginSingleCommand(frame); - + if (!reply_required) { return false; } auto start = micros(); @@ -737,8 +728,6 @@ class Moteus { msg->len = new_size; } - ACAN2517FD& can_bus_; - const Options options_; Result last_result_; char* query_data_ = nullptr; diff --git a/src/moteus_multiplex.h b/src/moteus_multiplex.h index e58bd6e..97a99da 100644 --- a/src/moteus_multiplex.h +++ b/src/moteus_multiplex.h @@ -18,6 +18,8 @@ #include #include + + #ifdef min #undef min #endif