Rename RoboClaw -> Roboclaw

The manufacturer uses both naming schemes, RoboClaw more than Roboclaw
but it's always one word and hence I think it's more consistent to name
it the latter.
This commit is contained in:
Matthias Grob 2023-11-14 12:50:27 +01:00
parent 87683aa790
commit c27181a154
3 changed files with 35 additions and 35 deletions

View File

@ -36,7 +36,7 @@ px4_add_module(
MAIN roboclaw MAIN roboclaw
COMPILE_FLAGS COMPILE_FLAGS
SRCS SRCS
RoboClaw.cpp Roboclaw.cpp
MODULE_CONFIG MODULE_CONFIG
module.yaml module.yaml
) )

View File

@ -32,9 +32,9 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file RoboClaw.cpp * @file Roboclaw.cpp
* *
* RoboClaw Motor Driver * Roboclaw Motor Driver
* *
* references: * references:
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
@ -42,7 +42,7 @@
*/ */
#include "RoboClaw.hpp" #include "Roboclaw.hpp"
#include <poll.h> #include <poll.h>
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
@ -58,7 +58,7 @@
#include <math.h> #include <math.h>
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
// The RoboClaw has a serial communication timeout of 10ms. // The Roboclaw has a serial communication timeout of 10ms.
// Add a little extra to account for timing inaccuracy // Add a little extra to account for timing inaccuracy
#define TIMEOUT_US 10500 #define TIMEOUT_US 10500
@ -77,7 +77,7 @@
using namespace time_literals; using namespace time_literals;
RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) : Roboclaw::Roboclaw(const char *deviceName, const char *baudRateParam) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{ {
strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1);
@ -87,12 +87,12 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) :
_storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination
} }
RoboClaw::~RoboClaw() Roboclaw::~Roboclaw()
{ {
close(_uart_fd); close(_uart_fd);
} }
int RoboClaw::init() int Roboclaw::init()
{ {
_uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US };
@ -185,12 +185,12 @@ int RoboClaw::init()
} }
} }
int RoboClaw::task_spawn(int argc, char *argv[]) int Roboclaw::task_spawn(int argc, char *argv[])
{ {
const char *deviceName = argv[1]; const char *deviceName = argv[1];
const char *baud_rate_parameter_value = argv[2]; const char *baud_rate_parameter_value = argv[2];
RoboClaw *instance = new RoboClaw(deviceName, baud_rate_parameter_value); Roboclaw *instance = new Roboclaw(deviceName, baud_rate_parameter_value);
if (instance) { if (instance) {
_object.store(instance); _object.store(instance);
@ -212,7 +212,7 @@ int RoboClaw::task_spawn(int argc, char *argv[])
return PX4_ERROR; return PX4_ERROR;
} }
bool RoboClaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) unsigned num_outputs, unsigned num_control_groups_updated)
{ {
float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f;
@ -231,7 +231,7 @@ bool RoboClaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
return true; return true;
} }
void RoboClaw::Run() void Roboclaw::Run()
{ {
if (should_exit()) { if (should_exit()) {
ScheduleClear(); ScheduleClear();
@ -266,7 +266,7 @@ void RoboClaw::Run()
} }
int RoboClaw::readEncoder() int Roboclaw::readEncoder()
{ {
uint8_t buffer_positon[ENCODER_MESSAGE_SIZE]; uint8_t buffer_positon[ENCODER_MESSAGE_SIZE];
uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE];
@ -300,7 +300,7 @@ int RoboClaw::readEncoder()
return 1; return 1;
} }
int32_t RoboClaw::reverseInt32(uint8_t *buffer) int32_t Roboclaw::reverseInt32(uint8_t *buffer)
{ {
return (buffer[0] << 24) return (buffer[0] << 24)
| (buffer[1] << 16) | (buffer[1] << 16)
@ -308,7 +308,7 @@ int32_t RoboClaw::reverseInt32(uint8_t *buffer)
| buffer[3]; | buffer[3];
} }
void RoboClaw::setMotorSpeed(e_motor motor, float value) void Roboclaw::setMotorSpeed(e_motor motor, float value)
{ {
e_command command; e_command command;
@ -336,7 +336,7 @@ void RoboClaw::setMotorSpeed(e_motor motor, float value)
_sendUnsigned7Bit(command, value); _sendUnsigned7Bit(command, value);
} }
void RoboClaw::setMotorDutyCycle(e_motor motor, float value) void Roboclaw::setMotorDutyCycle(e_motor motor, float value)
{ {
e_command command; e_command command;
@ -354,24 +354,24 @@ void RoboClaw::setMotorDutyCycle(e_motor motor, float value)
return _sendSigned16Bit(command, value); return _sendSigned16Bit(command, value);
} }
void RoboClaw::drive(float value) void Roboclaw::drive(float value)
{ {
e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX; e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX;
_sendUnsigned7Bit(command, value); _sendUnsigned7Bit(command, value);
} }
void RoboClaw::turn(float value) void Roboclaw::turn(float value)
{ {
e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT; e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT;
_sendUnsigned7Bit(command, value); _sendUnsigned7Bit(command, value);
} }
void RoboClaw::resetEncoders() void Roboclaw::resetEncoders()
{ {
sendTransaction(CMD_RESET_ENCODERS, nullptr, 0); sendTransaction(CMD_RESET_ENCODERS, nullptr, 0);
} }
void RoboClaw::_sendUnsigned7Bit(e_command command, float data) void Roboclaw::_sendUnsigned7Bit(e_command command, float data)
{ {
data = fabs(data); data = fabs(data);
@ -383,7 +383,7 @@ void RoboClaw::_sendUnsigned7Bit(e_command command, float data)
sendTransaction(command, &byte, 1); sendTransaction(command, &byte, 1);
} }
void RoboClaw::_sendSigned16Bit(e_command command, float data) void Roboclaw::_sendSigned16Bit(e_command command, float data)
{ {
int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX; int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX;
uint8_t buff[2]; uint8_t buff[2];
@ -392,7 +392,7 @@ void RoboClaw::_sendSigned16Bit(e_command command, float data)
sendTransaction(command, (uint8_t *) &buff, 2); sendTransaction(command, (uint8_t *) &buff, 2);
} }
uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) uint16_t Roboclaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
{ {
uint16_t crc = init; uint16_t crc = init;
@ -412,7 +412,7 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
return crc; return crc;
} }
int RoboClaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read) int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read)
{ {
if (writeCommand(cmd) != RoboClawError::Success) { if (writeCommand(cmd) != RoboClawError::Success) {
return -1; return -1;
@ -459,7 +459,7 @@ int RoboClaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t byt
return total_bytes_read; return total_bytes_read;
} }
void RoboClaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write) void Roboclaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write)
{ {
writeCommandWithPayload(cmd, write_buffer, bytes_to_write); writeCommandWithPayload(cmd, write_buffer, bytes_to_write);
int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout);
@ -478,7 +478,7 @@ void RoboClaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t byte
} }
} }
RoboClaw::RoboClawError RoboClaw::writeCommand(e_command cmd) Roboclaw::RoboClawError Roboclaw::writeCommand(e_command cmd)
{ {
uint8_t buffer[2]; uint8_t buffer[2];
@ -495,7 +495,7 @@ RoboClaw::RoboClawError RoboClaw::writeCommand(e_command cmd)
return RoboClawError::Success; return RoboClawError::Success;
} }
RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write) Roboclaw::RoboClawError Roboclaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write)
{ {
size_t packet_size = 2 + bytes_to_write + 2; size_t packet_size = 2 + bytes_to_write + 2;
uint8_t buffer[packet_size]; uint8_t buffer[packet_size];
@ -526,12 +526,12 @@ RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t
return RoboClawError::Success; return RoboClawError::Success;
} }
int RoboClaw::custom_command(int argc, char *argv[]) int Roboclaw::custom_command(int argc, char *argv[])
{ {
return print_usage("unknown command"); return print_usage("unknown command");
} }
int RoboClaw::print_usage(const char *reason) int Roboclaw::print_usage(const char *reason)
{ {
if (reason) { if (reason) {
PX4_WARN("%s\n", reason); PX4_WARN("%s\n", reason);
@ -560,7 +560,7 @@ The command to start this driver is: `$ roboclaw start <UART device> <baud rate>
return 0; return 0;
} }
int RoboClaw::print_status() int Roboclaw::print_status()
{ {
PX4_ERR("Running, Initialized: %f", (double)_initialized); PX4_ERR("Running, Initialized: %f", (double)_initialized);
return 0; return 0;
@ -568,5 +568,5 @@ int RoboClaw::print_status()
extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]) extern "C" __EXPORT int roboclaw_main(int argc, char *argv[])
{ {
return RoboClaw::main(argc, argv); return Roboclaw::main(argc, argv);
} }

View File

@ -34,7 +34,7 @@
/** /**
* @file RoboClas.hpp * @file RoboClas.hpp
* *
* RoboClaw Motor Driver * Roboclaw Motor Driver
* *
* Product page: https://www.basicmicro.com/motor-controller * Product page: https://www.basicmicro.com/motor-controller
* Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf
@ -55,9 +55,9 @@
#include <uORB/topics/wheel_encoders.h> #include <uORB/topics/wheel_encoders.h>
/** /**
* This is a driver for the RoboClaw motor controller * This is a driver for the Roboclaw motor controller
*/ */
class RoboClaw : public ModuleBase<RoboClaw>, public OutputModuleInterface class Roboclaw : public ModuleBase<Roboclaw>, public OutputModuleInterface
{ {
public: public:
/** /**
@ -68,12 +68,12 @@ public:
* (selectable on roboclaw) * (selectable on roboclaw)
* @param baudRateParam Name of the parameter that holds the baud rate of this serial port * @param baudRateParam Name of the parameter that holds the baud rate of this serial port
*/ */
RoboClaw(const char *deviceName, const char *baudRateParam); Roboclaw(const char *deviceName, const char *baudRateParam);
/** /**
* deconstructor * deconstructor
*/ */
virtual ~RoboClaw(); virtual ~Roboclaw();
/** control channels */ /** control channels */
enum e_channel { enum e_channel {