276 lines
7.9 KiB
C++
276 lines
7.9 KiB
C++
/*
|
|
* command_manager_example.ino
|
|
* Example sketch demonstrating DShotCommandManager usage
|
|
* Author: Wastl Kraus
|
|
* Date: 2025-09-04
|
|
* License: MIT
|
|
*
|
|
* Modified by Especiallist to support continuous throttle sending.
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <DShotRMT.h>
|
|
#include <DShotCommandManager.h>
|
|
|
|
// USB serial port settings
|
|
static constexpr auto &USB_SERIAL = Serial0;
|
|
static constexpr auto USB_SERIAL_BAUD = 115200;
|
|
|
|
// Motor configuration
|
|
static constexpr auto MOTOR01_PIN = 17;
|
|
static constexpr auto IS_BIDIRECTIONAL = false;
|
|
|
|
// Motor magnet count for RPM calculation
|
|
static constexpr auto MOTOR01_MAGNET_COUNT = 14;
|
|
|
|
// Create motor and command manager instances
|
|
DShotRMT motor01(MOTOR01_PIN, DSHOT300, IS_BIDIRECTIONAL);
|
|
DShotCommandManager commandManager(motor01);
|
|
|
|
// Global variable to store the desired continuous throttle value
|
|
static volatile uint16_t throttle_now = NULL;
|
|
|
|
//
|
|
void setup()
|
|
{
|
|
// Start USB Serial Port
|
|
USB_SERIAL.begin(USB_SERIAL_BAUD);
|
|
|
|
USB_SERIAL.println("=== DShotRMT Command Manager Example ===");
|
|
|
|
// Initialize DShot
|
|
if (motor01.begin() != 0)
|
|
{
|
|
USB_SERIAL.println("ERROR: Failed to initialize DShotRMT!");
|
|
while (1)
|
|
delay(1000);
|
|
}
|
|
|
|
// Init Command Manager
|
|
if (!commandManager.begin())
|
|
{
|
|
USB_SERIAL.println("ERROR: Failed to initialize DShotCommandManager!");
|
|
while (1)
|
|
delay(1000);
|
|
}
|
|
|
|
USB_SERIAL.println("Initialization successful!");
|
|
|
|
// Print Menu
|
|
printMenu();
|
|
}
|
|
|
|
//
|
|
void loop()
|
|
{
|
|
// Time Measurement
|
|
static uint64_t last_stats_print = 0;
|
|
|
|
// Check for serial input
|
|
if (USB_SERIAL.available() > 0)
|
|
{
|
|
String input = USB_SERIAL.readStringUntil('\n');
|
|
input.trim();
|
|
handleUserInput(input);
|
|
}
|
|
|
|
// Continuously send the stored throttle value
|
|
if (throttle_now != NULL)
|
|
{
|
|
motor01.sendThrottle(throttle_now);
|
|
|
|
// Print motor stats every 2 seconds
|
|
if (esp_timer_get_time() - last_stats_print >= 2000000)
|
|
{
|
|
motor01.printDShotInfo();
|
|
|
|
// Get Motor RPM
|
|
if (IS_BIDIRECTIONAL)
|
|
{
|
|
uint32_t rpm = motor01.getMotorRPM(MOTOR01_MAGNET_COUNT);
|
|
USB_SERIAL.printf("Motor RPM: %u\n", rpm);
|
|
}
|
|
|
|
// Time Stamp
|
|
last_stats_print = esp_timer_get_time();
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
void handleUserInput(const String &input)
|
|
{
|
|
dshot_command_result_t result;
|
|
|
|
if (input == "1")
|
|
{
|
|
// Stop motor command should also reset the continuous throttle value
|
|
throttle_now = 0;
|
|
USB_SERIAL.print("Stopping motor... ");
|
|
result = commandManager.stopMotor();
|
|
printResult(result);
|
|
}
|
|
else if (input == "2")
|
|
{
|
|
USB_SERIAL.print("Activating beacon 1... ");
|
|
result = commandManager.activateBeacon(1);
|
|
printResult(result);
|
|
}
|
|
else if (input == "3")
|
|
{
|
|
USB_SERIAL.print("Setting normal spin direction... ");
|
|
result = commandManager.setSpinDirection(false);
|
|
printResult(result);
|
|
}
|
|
else if (input == "4")
|
|
{
|
|
USB_SERIAL.print("Setting reversed spin direction... ");
|
|
result = commandManager.setSpinDirection(true);
|
|
printResult(result);
|
|
}
|
|
else if (input == "5")
|
|
{
|
|
USB_SERIAL.print("Enabling 3D mode... ");
|
|
result = commandManager.set3DMode(true);
|
|
printResult(result);
|
|
}
|
|
else if (input == "6")
|
|
{
|
|
USB_SERIAL.print("Disabling 3D mode... ");
|
|
result = commandManager.set3DMode(false);
|
|
printResult(result);
|
|
}
|
|
else if (input == "7")
|
|
{
|
|
USB_SERIAL.print("Saving settings... ");
|
|
result = commandManager.saveSettings();
|
|
printResult(result);
|
|
}
|
|
else if (input == "8")
|
|
{
|
|
USB_SERIAL.print("Turning LED 0 ON... ");
|
|
result = commandManager.setLED(0, true);
|
|
printResult(result);
|
|
}
|
|
else if (input == "9")
|
|
{
|
|
USB_SERIAL.print("Turning LED 0 OFF... ");
|
|
result = commandManager.setLED(0, false);
|
|
printResult(result);
|
|
}
|
|
else if (input == "i")
|
|
{
|
|
USB_SERIAL.print("Executing initialization sequence... ");
|
|
result = commandManager.executeInitSequence();
|
|
printResult(result);
|
|
}
|
|
else if (input == "c")
|
|
{
|
|
USB_SERIAL.print("Executing calibration sequence... ");
|
|
result = commandManager.executeCalibrationSequence();
|
|
printResult(result);
|
|
}
|
|
else if (input == "s")
|
|
{
|
|
commandManager.printStatistics();
|
|
}
|
|
else if (input == "r")
|
|
{
|
|
commandManager.resetStatistics();
|
|
USB_SERIAL.println("Statistics reset.");
|
|
}
|
|
else if (input == "h")
|
|
{
|
|
printMenu();
|
|
}
|
|
else if (input == "help")
|
|
{
|
|
printMenu();
|
|
}
|
|
else if (input.startsWith("cmd "))
|
|
{
|
|
// Direct command execution: "cmd 5" sends command 5
|
|
int cmd_num = input.substring(4).toInt();
|
|
if (DShotCommandManager::isValidCommand(static_cast<dshot_commands_t>(cmd_num)))
|
|
{
|
|
USB_SERIAL.printf("Sending command %d (%s)... ", cmd_num,
|
|
DShotCommandManager::getCommandName(static_cast<dshot_commands_t>(cmd_num)));
|
|
result = commandManager.sendCommand(static_cast<dshot_commands_t>(cmd_num));
|
|
printResult(result);
|
|
}
|
|
else
|
|
{
|
|
USB_SERIAL.printf("Invalid command number: %d (valid range: 0-%d)\n", cmd_num, DSHOT_CMD_MAX);
|
|
}
|
|
}
|
|
else if (input.startsWith("throttle "))
|
|
{
|
|
// Throttle control: "throttle 1000" sets throttle to 1000
|
|
int throttle_value = input.substring(9).toInt();
|
|
if (throttle_value >= DSHOT_THROTTLE_MIN && throttle_value <= DSHOT_THROTTLE_MAX)
|
|
{
|
|
throttle_now = throttle_value;
|
|
USB_SERIAL.printf("Setting continuous throttle to %d\n", throttle_now);
|
|
}
|
|
else if (throttle_value == 0)
|
|
{
|
|
throttle_now = 0;
|
|
USB_SERIAL.println("Continuous throttle stopped.");
|
|
}
|
|
else
|
|
{
|
|
USB_SERIAL.printf("Invalid throttle value: %d (valid range: 48-2047, use 0 to stop)\n", throttle_value);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
USB_SERIAL.println("Unknown command. Type 'h' for help.");
|
|
}
|
|
}
|
|
|
|
//
|
|
void printResult(const dshot_command_result_t &result)
|
|
{
|
|
if (!result.success)
|
|
{
|
|
USB_SERIAL.printf("SUCCESS (%u us)\n", result.execution_time_us);
|
|
}
|
|
else
|
|
{
|
|
USB_SERIAL.printf("FAILED - %s (%u us)\n", result.error_message, result.execution_time_us);
|
|
}
|
|
}
|
|
|
|
//
|
|
void printMenu()
|
|
{
|
|
USB_SERIAL.println("================================================");
|
|
USB_SERIAL.println("\n=== DShot Command Manager Menu ===");
|
|
USB_SERIAL.println("Basic Commands:");
|
|
USB_SERIAL.println(" 1 - Stop Motor");
|
|
USB_SERIAL.println(" 2 - Activate Beacon 1");
|
|
USB_SERIAL.println(" 3 - Set Normal Spin Direction");
|
|
USB_SERIAL.println(" 4 - Set Reversed Spin Direction");
|
|
USB_SERIAL.println(" 5 - Enable 3D Mode");
|
|
USB_SERIAL.println(" 6 - Disable 3D Mode");
|
|
USB_SERIAL.println(" 7 - Save Settings");
|
|
USB_SERIAL.println(" 8 - Turn LED 0 ON");
|
|
USB_SERIAL.println(" 9 - Turn LED 0 OFF");
|
|
USB_SERIAL.println("");
|
|
USB_SERIAL.println("Sequences:");
|
|
USB_SERIAL.println(" i - Execute Initialization Sequence");
|
|
USB_SERIAL.println(" c - Execute Calibration Sequence");
|
|
USB_SERIAL.println("");
|
|
USB_SERIAL.println("Advanced:");
|
|
USB_SERIAL.println(" cmd <number> - Send DShot command (0 - 47)");
|
|
USB_SERIAL.println(" throttle <value> - Set throttle (48 - 2047)");
|
|
USB_SERIAL.println(" throttle 0 - Stop sending throttle");
|
|
USB_SERIAL.println("");
|
|
USB_SERIAL.println(" h - Show this Menu");
|
|
USB_SERIAL.println("");
|
|
USB_SERIAL.println("Examples:");
|
|
USB_SERIAL.println(" cmd 1 - Stop Motor");
|
|
USB_SERIAL.println(" throttle 1000 - Set throttle to 1000");
|
|
USB_SERIAL.println("================================================");
|
|
}
|