mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Ongoing work to update RoboClaw driver
This commit is contained in:
Timothy Scott
Julian Oes
parent
cfdabb26d7
commit
6ea03bee58
@ -17,6 +17,8 @@
|
||||
|
||||
sh /etc/init.d/rc.rover_defaults
|
||||
|
||||
roboclaw start /dev/ttyS3 128 1200
|
||||
|
||||
if [ $AUTOCNF = yes ]
|
||||
then
|
||||
param set BAT_N_CELLS 4
|
||||
|
||||
@ -57,21 +57,45 @@
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
uint8_t RoboClaw::checksum_mask = 0x7f;
|
||||
// The RoboClaw has a serial communication timeout of 10ms.
|
||||
#define TIMEOUT_US 10000
|
||||
|
||||
RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
|
||||
uint16_t pulsesPerRev):
|
||||
// The RoboClaw determines the change in the wheel encoder value when it overflows
|
||||
#define OVERFLOW_AMOUNT 0x100000000LL
|
||||
|
||||
//void printbytes(const char *msg, uint8_t *bytes, int numbytes)
|
||||
//{
|
||||
// if (numbytes < 0) {
|
||||
// numbytes = 0;
|
||||
// }
|
||||
//
|
||||
// size_t msglen = strlen(msg);
|
||||
// char buff[msglen + (4 * numbytes) + 1];
|
||||
// char *cur = &buff[0];
|
||||
// cur += sprintf(cur, "%s ", msg);
|
||||
//
|
||||
// for (int i = 0; i < numbytes; i++) {
|
||||
// cur += sprintf(cur, "0x%02X ", bytes[i]);
|
||||
// }
|
||||
//
|
||||
// PX4_INFO("%s", buff);
|
||||
//}
|
||||
|
||||
RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerRev):
|
||||
ScheduledWorkItem(px4::wq_configurations::hp_default),
|
||||
_address(address),
|
||||
_pulsesPerRev(pulsesPerRev),
|
||||
_uart(0),
|
||||
_controlPoll(),
|
||||
_actuators(ORB_ID(actuator_controls_0), 20),
|
||||
_motor1Position(0),
|
||||
_motor1Speed(0),
|
||||
_motor1EncoderCounts(0),
|
||||
_motor1Revolutions(0),
|
||||
_motor1Overflow(0),
|
||||
_motor2Position(0),
|
||||
_motor2Speed(0),
|
||||
_motor2Overflow(0)
|
||||
_motor1Speed(0),
|
||||
_motor2EncoderCounts(0),
|
||||
_motor2Revolutions(0),
|
||||
_motor2Overflow(0),
|
||||
_motor2Speed(0)
|
||||
{
|
||||
// setup control polling
|
||||
_controlPoll.fd = _actuators.getHandle();
|
||||
@ -83,7 +107,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
|
||||
if (_uart < 0) { err(1, "could not open %s", deviceName); }
|
||||
|
||||
int ret = 0;
|
||||
struct termios uart_config;
|
||||
struct termios uart_config {};
|
||||
ret = tcgetattr(_uart, &uart_config);
|
||||
|
||||
if (ret < 0) { err(1, "failed to get attr"); }
|
||||
@ -101,7 +125,14 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
|
||||
|
||||
if (ret < 0) { err(1, "failed to set attr"); }
|
||||
|
||||
// setup default settings, reset encoders
|
||||
FD_ZERO(&_uart_set);
|
||||
|
||||
_uart_timeout.tv_sec = 0;
|
||||
_uart_timeout.tv_usec = TIMEOUT_US;
|
||||
|
||||
pthread_mutex_init(&_uart_mutex, NULL);
|
||||
|
||||
// setup default settings, reset encoders
|
||||
resetEncoders();
|
||||
}
|
||||
|
||||
@ -110,86 +141,79 @@ RoboClaw::~RoboClaw()
|
||||
setMotorDutyCycle(MOTOR_1, 0.0);
|
||||
setMotorDutyCycle(MOTOR_2, 0.0);
|
||||
close(_uart);
|
||||
|
||||
pthread_mutex_destroy(&_uart_mutex);
|
||||
}
|
||||
|
||||
void RoboClaw::Run()
|
||||
{
|
||||
readEncoder(MOTOR_1);
|
||||
readEncoder(MOTOR_2);
|
||||
|
||||
PX4_INFO("Motor1: (%d, %d), Motor2: (%d, %d)", _motor1EncoderCounts, _motor1Revolutions, _motor2EncoderCounts,
|
||||
_motor2Revolutions);
|
||||
}
|
||||
|
||||
int RoboClaw::readEncoder(e_motor motor)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
e_command cmd = motor == MOTOR_1 ? CMD_READ_ENCODER_1 : CMD_READ_ENCODER_2;
|
||||
uint8_t rbuf[7];
|
||||
|
||||
if (motor == MOTOR_1) {
|
||||
_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
|
||||
int nread = _transaction(cmd, nullptr, 0, rbuf, 7, false, true);
|
||||
|
||||
} else if (motor == MOTOR_2) {
|
||||
_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
|
||||
}
|
||||
|
||||
uint8_t rbuf[50];
|
||||
usleep(5000);
|
||||
int nread = read(_uart, rbuf, 50);
|
||||
|
||||
if (nread < 6) {
|
||||
printf("failed to read\n");
|
||||
if (nread < 7) {
|
||||
PX4_ERR("Error reading RoboClaw encoders: %d", nread);
|
||||
return -1;
|
||||
}
|
||||
|
||||
//printf("received: \n");
|
||||
//for (int i=0;i<nread;i++) {
|
||||
//printf("%d\t", rbuf[i]);
|
||||
//}
|
||||
//printf("\n");
|
||||
uint32_t count = 0;
|
||||
uint8_t *countBytes = (uint8_t *)(&count);
|
||||
auto countBytes = (uint8_t *)(&count);
|
||||
countBytes[3] = rbuf[0];
|
||||
countBytes[2] = rbuf[1];
|
||||
countBytes[1] = rbuf[2];
|
||||
countBytes[0] = rbuf[3];
|
||||
uint8_t status = rbuf[4];
|
||||
uint8_t checksum = rbuf[5];
|
||||
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
|
||||
checksum_mask;
|
||||
|
||||
// check if checksum is valid
|
||||
if (checksum != checksum_computed) {
|
||||
printf("checksum failed: expected %d got %d\n",
|
||||
checksum, checksum_computed);
|
||||
return -1;
|
||||
}
|
||||
|
||||
int overFlow = 0;
|
||||
|
||||
if (status & STATUS_REVERSE) {
|
||||
//printf("roboclaw: reverse\n");
|
||||
}
|
||||
int overflowFlag = 0;
|
||||
|
||||
if (status & STATUS_UNDERFLOW) {
|
||||
//printf("roboclaw: underflow\n");
|
||||
overFlow = -1;
|
||||
overflowFlag = -1;
|
||||
PX4_INFO("=====UNDERFLOW=====");
|
||||
|
||||
} else if (status & STATUS_OVERFLOW) {
|
||||
//printf("roboclaw: overflow\n");
|
||||
overFlow = +1;
|
||||
PX4_INFO("=====OVERFLOW=====");
|
||||
overflowFlag = +1;
|
||||
}
|
||||
|
||||
static int64_t overflowAmount = 0x100000000LL;
|
||||
int32_t *encoderCount;
|
||||
int32_t *revolutions;
|
||||
int32_t *overflow;
|
||||
|
||||
if (motor == MOTOR_1) {
|
||||
_motor1Overflow += overFlow;
|
||||
_motor1Position = float(int64_t(count) +
|
||||
_motor1Overflow * overflowAmount) / _pulsesPerRev;
|
||||
encoderCount = &_motor1EncoderCounts;
|
||||
revolutions = &_motor1Revolutions;
|
||||
overflow = &_motor1Overflow;
|
||||
|
||||
} else if (motor == MOTOR_2) {
|
||||
_motor2Overflow += overFlow;
|
||||
_motor2Position = float(int64_t(count) +
|
||||
_motor2Overflow * overflowAmount) / _pulsesPerRev;
|
||||
} else {
|
||||
encoderCount = &_motor2EncoderCounts;
|
||||
revolutions = &_motor2Revolutions;
|
||||
overflow = &_motor2Overflow;
|
||||
}
|
||||
|
||||
PX4_INFO("COUNT: %08X STATUS: %02X", count, status);
|
||||
|
||||
*overflow += overflowFlag;
|
||||
int64_t totalCounts = int64_t(count) + (int64_t(*overflow) * OVERFLOW_AMOUNT);
|
||||
PX4_INFO("Total counts: %lld", totalCounts);
|
||||
*encoderCount = int32_t(totalCounts % _pulsesPerRev);
|
||||
*revolutions = int32_t(totalCounts / _pulsesPerRev);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void RoboClaw::printStatus(char *string, size_t n)
|
||||
{
|
||||
snprintf(string, n,
|
||||
"pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
|
||||
snprintf(string, n, "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
|
||||
double(getMotorPosition(MOTOR_1)),
|
||||
double(getMotorSpeed(MOTOR_1)),
|
||||
double(getMotorPosition(MOTOR_2)),
|
||||
@ -199,10 +223,10 @@ void RoboClaw::printStatus(char *string, size_t n)
|
||||
float RoboClaw::getMotorPosition(e_motor motor)
|
||||
{
|
||||
if (motor == MOTOR_1) {
|
||||
return _motor1Position;
|
||||
return _motor1EncoderCounts;
|
||||
|
||||
} else if (motor == MOTOR_2) {
|
||||
return _motor2Position;
|
||||
return _motor2EncoderCounts;
|
||||
|
||||
} else {
|
||||
warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
|
||||
@ -226,69 +250,72 @@ float RoboClaw::getMotorSpeed(e_motor motor)
|
||||
|
||||
int RoboClaw::setMotorSpeed(e_motor motor, float value)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
|
||||
// bound
|
||||
if (value > 1) { value = 1; }
|
||||
|
||||
if (value < -1) { value = -1; }
|
||||
|
||||
uint8_t speed = fabs(value) * 127;
|
||||
e_command command;
|
||||
|
||||
// send command
|
||||
if (motor == MOTOR_1) {
|
||||
if (value > 0) {
|
||||
return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
|
||||
command = CMD_DRIVE_FWD_1;
|
||||
|
||||
} else {
|
||||
return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
|
||||
command = CMD_DRIVE_REV_1;
|
||||
}
|
||||
|
||||
} else if (motor == MOTOR_2) {
|
||||
if (value > 0) {
|
||||
return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
|
||||
command = CMD_DRIVE_FWD_2;
|
||||
|
||||
} else {
|
||||
return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
|
||||
command = CMD_DRIVE_REV_2;
|
||||
}
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return -1;
|
||||
return _sendUnsigned7Bit(command, value);
|
||||
}
|
||||
|
||||
int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
|
||||
// bound
|
||||
if (value > 1) { value = 1; }
|
||||
|
||||
if (value < -1) { value = -1; }
|
||||
|
||||
int16_t duty = 1500 * value;
|
||||
e_command command;
|
||||
|
||||
// send command
|
||||
if (motor == MOTOR_1) {
|
||||
return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
|
||||
(uint8_t *)(&duty), 2, sum);
|
||||
command = CMD_SIGNED_DUTYCYCLE_1;
|
||||
|
||||
} else if (motor == MOTOR_2) {
|
||||
return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
|
||||
(uint8_t *)(&duty), 2, sum);
|
||||
command = CMD_SIGNED_DUTYCYCLE_2;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return -1;
|
||||
return _sendSigned16Bit(command, value);
|
||||
}
|
||||
|
||||
int RoboClaw::drive(float value)
|
||||
{
|
||||
e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX;
|
||||
return _sendUnsigned7Bit(command, value);
|
||||
}
|
||||
|
||||
int RoboClaw::turn(float value)
|
||||
{
|
||||
e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT;
|
||||
return _sendUnsigned7Bit(command, value);
|
||||
}
|
||||
|
||||
int RoboClaw::resetEncoders()
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
return _sendCommand(CMD_RESET_ENCODERS,
|
||||
nullptr, 0, sum);
|
||||
return _sendNothing(CMD_RESET_ENCODERS);
|
||||
}
|
||||
|
||||
int RoboClaw::update()
|
||||
{
|
||||
//TODO: Also update motor locations and speeds here
|
||||
|
||||
// wait for an actuator publication,
|
||||
// check for exit condition every second
|
||||
// note "::poll" is required to distinguish global
|
||||
@ -298,57 +325,166 @@ int RoboClaw::update()
|
||||
// if new data, send to motors
|
||||
if (_actuators.updated()) {
|
||||
_actuators.update();
|
||||
setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]);
|
||||
setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]);
|
||||
// setMotorDutyCycle(MOTOR_1, _actuators.get().control[actuator_controls_s::INDEX_]);
|
||||
// setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]);
|
||||
int drive_ret = drive(_actuators.get().control[actuator_controls_s::INDEX_THROTTLE]);
|
||||
int turn_ret = turn(_actuators.get().control[actuator_controls_s::INDEX_YAW]);
|
||||
|
||||
if (drive_ret <= 0 || turn_ret <= 0) {
|
||||
PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
|
||||
}
|
||||
}
|
||||
|
||||
Run();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n)
|
||||
int RoboClaw::_sendUnsigned7Bit(e_command command, float data)
|
||||
{
|
||||
uint16_t sum = 0;
|
||||
data = fabs(data);
|
||||
|
||||
//printf("sum\n");
|
||||
for (size_t i = 0; i < n; i++) {
|
||||
sum += buf[i];
|
||||
//printf("%d\t", buf[i]);
|
||||
if (data > 1.0f) {
|
||||
data = 1.0f;
|
||||
}
|
||||
|
||||
//printf("total sum %d\n", sum);
|
||||
return sum;
|
||||
uint8_t byte = (uint8_t)(data * 127);
|
||||
uint8_t recv_byte;
|
||||
return _transaction(command, &byte, 1, &recv_byte, 1);
|
||||
}
|
||||
|
||||
int RoboClaw::_sendCommand(e_command cmd, uint8_t *data,
|
||||
size_t n_data, uint16_t &prev_sum)
|
||||
int RoboClaw::_sendSigned16Bit(e_command command, float data)
|
||||
{
|
||||
if (data > 1.0f) {
|
||||
data = 1.0f;
|
||||
|
||||
} else if (data < -1.0f) {
|
||||
data = -1.0f;
|
||||
}
|
||||
|
||||
uint16_t buff = (uint16_t)(data * 32767);
|
||||
uint8_t recv_buff;
|
||||
return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 1);
|
||||
}
|
||||
|
||||
int RoboClaw::_sendNothing(e_command command)
|
||||
{
|
||||
uint8_t recv_buff;
|
||||
return _transaction(command, nullptr, 0, &recv_buff, 1);
|
||||
}
|
||||
|
||||
uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n, uint16_t init)
|
||||
{
|
||||
uint16_t crc = init;
|
||||
|
||||
for (size_t byte = 0; byte < n; byte++) {
|
||||
crc = crc ^ (((uint16_t) buf[byte]) << 8);
|
||||
|
||||
for (uint8_t bit = 0; bit < 8; bit++) {
|
||||
if (crc & 0x8000) {
|
||||
crc = (crc << 1) ^ 0x1021;
|
||||
|
||||
} else {
|
||||
crc = crc << 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
||||
uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum)
|
||||
{
|
||||
// WRITE
|
||||
|
||||
pthread_mutex_lock(&_uart_mutex);
|
||||
|
||||
tcflush(_uart, TCIOFLUSH); // flush buffers
|
||||
uint8_t buf[n_data + 3];
|
||||
uint8_t buf[wbytes + 4];
|
||||
buf[0] = _address;
|
||||
buf[1] = cmd;
|
||||
|
||||
for (size_t i = 0; i < n_data; i++) {
|
||||
buf[i + 2] = data[n_data - i - 1]; // MSB
|
||||
if (wbuff) {
|
||||
memcpy(&buf[2], wbuff, wbytes);
|
||||
}
|
||||
|
||||
uint16_t sum = _sumBytes(buf, n_data + 2);
|
||||
prev_sum += sum;
|
||||
buf[n_data + 2] = sum & checksum_mask;
|
||||
//printf("\nmessage:\n");
|
||||
//for (size_t i=0;i<n_data+3;i++) {
|
||||
//printf("%d\t", buf[i]);
|
||||
//}
|
||||
//printf("\n");
|
||||
return write(_uart, buf, n_data + 3);
|
||||
wbytes = wbytes + (send_checksum ? 4 : 2);
|
||||
|
||||
if (send_checksum) {
|
||||
uint16_t sum = _sumBytes(buf, wbytes - 2);
|
||||
buf[wbytes - 2] = (sum >> 8) & 0xFF;
|
||||
buf[wbytes - 1] = sum & 0xFFu;
|
||||
}
|
||||
|
||||
int count = write(_uart, buf, wbytes);
|
||||
|
||||
if (count < (int) wbytes) { // Did not successfully send all bytes.
|
||||
PX4_ERR("Only wrote %d out of %d bytes", count, (int) wbytes);
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// READ
|
||||
|
||||
FD_ZERO(&_uart_set);
|
||||
FD_SET(_uart, &_uart_set);
|
||||
|
||||
int rv = select(_uart + 1, &_uart_set, NULL, NULL, &_uart_timeout);
|
||||
|
||||
//TODO: Clean up this mess of IFs and returns
|
||||
|
||||
if (rv > 0) {
|
||||
// select() returns as soon as ANY bytes are available. I need to wait until ALL of the bytes are available.
|
||||
// TODO: Make sure this is not a busy wait.
|
||||
usleep(2000);
|
||||
int bytes_read = read(_uart, rbuff, rbytes);
|
||||
|
||||
if (recv_checksum) {
|
||||
if (bytes_read < 2) {
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// The checksum sent back by the roboclaw is calculated based on the address and command bytes as well
|
||||
// as the data returned.
|
||||
uint16_t checksum_calc = _sumBytes(buf, 2);
|
||||
checksum_calc = _sumBytes(rbuff, bytes_read - 2, checksum_calc);
|
||||
uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1];
|
||||
|
||||
if (checksum_calc == checksum_recv) {
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return bytes_read;
|
||||
|
||||
} else {
|
||||
PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv);
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return -10;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (bytes_read == 1 && rbuff[0] == 0xFF) {
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return -11;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
pthread_mutex_unlock(&_uart_mutex);
|
||||
return rv;
|
||||
}
|
||||
}
|
||||
|
||||
int roboclawTest(const char *deviceName, uint8_t address,
|
||||
uint16_t pulsesPerRev)
|
||||
int RoboClaw::roboclawTest(int argc, char *argv[])
|
||||
{
|
||||
printf("roboclaw test: starting\n");
|
||||
|
||||
// setup
|
||||
RoboClaw roboclaw(deviceName, address, pulsesPerRev);
|
||||
RoboClaw roboclaw("/dev/ttyS3", 128, 1200);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
|
||||
char buf[200];
|
||||
@ -372,8 +508,10 @@ int roboclawTest(const char *deviceName, uint8_t address,
|
||||
printf("%s", buf);
|
||||
}
|
||||
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.0);
|
||||
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.0);
|
||||
|
||||
printf("Test complete\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
|
||||
|
||||
@ -48,14 +48,21 @@
|
||||
#include <uORB/SubscriptionPollable.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/time.h>
|
||||
#include <pthread.h>
|
||||
//#include <px4.h>
|
||||
#include <px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
/**
|
||||
* This is a driver for the RoboClaw motor controller
|
||||
*/
|
||||
class RoboClaw
|
||||
class RoboClaw : public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
static int roboclawTest(int argc, char *argv[]);
|
||||
|
||||
/** control channels */
|
||||
enum e_channel {
|
||||
CH_VOLTAGE_LEFT = 0,
|
||||
@ -101,10 +108,20 @@ public:
|
||||
int setMotorSpeed(e_motor motor, float value);
|
||||
|
||||
/**
|
||||
* set the duty cycle of a motor, rev/sec
|
||||
* set the duty cycle of a motor
|
||||
*/
|
||||
int setMotorDutyCycle(e_motor motor, float value);
|
||||
|
||||
/**
|
||||
* Drive both motors. +1 = full forward, -1 = full backward
|
||||
*/
|
||||
int drive(float value);
|
||||
|
||||
/**
|
||||
* Turn. +1 = full right, -1 = full left
|
||||
*/
|
||||
int turn(float value);
|
||||
|
||||
/**
|
||||
* reset the encoders
|
||||
* @return status
|
||||
@ -127,6 +144,8 @@ public:
|
||||
*/
|
||||
void printStatus(char *string, size_t n);
|
||||
|
||||
void Run();
|
||||
|
||||
private:
|
||||
|
||||
// Quadrature status flags
|
||||
@ -146,6 +165,11 @@ private:
|
||||
CMD_DRIVE_FWD_2 = 4,
|
||||
CMD_DRIVE_REV_2 = 5,
|
||||
|
||||
CMD_DRIVE_FWD_MIX = 8,
|
||||
CMD_DRIVE_REV_MIX = 9,
|
||||
CMD_TURN_RIGHT = 10,
|
||||
CMD_TURN_LEFT = 11,
|
||||
|
||||
// encoder commands
|
||||
CMD_READ_ENCODER_1 = 16,
|
||||
CMD_READ_ENCODER_2 = 17,
|
||||
@ -158,12 +182,14 @@ private:
|
||||
CMD_SIGNED_DUTYCYCLE_2 = 33,
|
||||
};
|
||||
|
||||
static uint8_t checksum_mask;
|
||||
|
||||
uint16_t _address;
|
||||
uint8_t _address;
|
||||
uint16_t _pulsesPerRev;
|
||||
|
||||
int _uart;
|
||||
fd_set _uart_set;
|
||||
struct timeval _uart_timeout;
|
||||
|
||||
pthread_mutex_t _uart_mutex;
|
||||
|
||||
/** poll structure for control packets */
|
||||
struct pollfd _controlPoll;
|
||||
@ -172,17 +198,42 @@ private:
|
||||
uORB::SubscriptionPollable<actuator_controls_s> _actuators;
|
||||
|
||||
// private data
|
||||
float _motor1Position;
|
||||
int32_t _motor1EncoderCounts;
|
||||
int32_t _motor1Revolutions;
|
||||
int32_t _motor1Overflow;
|
||||
float _motor1Speed;
|
||||
int16_t _motor1Overflow;
|
||||
|
||||
float _motor2Position;
|
||||
int32_t _motor2EncoderCounts;
|
||||
int32_t _motor2Revolutions;
|
||||
int32_t _motor2Overflow;
|
||||
float _motor2Speed;
|
||||
int16_t _motor2Overflow;
|
||||
|
||||
// private methods
|
||||
uint16_t _sumBytes(uint8_t *buf, size_t n);
|
||||
int _sendCommand(e_command cmd, uint8_t *data, size_t n_data, uint16_t &prev_sum);
|
||||
uint16_t _sumBytes(uint8_t *buf, size_t n, uint16_t init = 0);
|
||||
int _sendUnsigned7Bit(e_command command, float data);
|
||||
int _sendSigned16Bit(e_command command, float data);
|
||||
int _sendNothing(e_command);
|
||||
|
||||
/**
|
||||
* Perform a round-trip write and read.
|
||||
* @param cmd Command to send to the Roboclaw
|
||||
* @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be
|
||||
* one or two bytes. Can be null iff wbytes == 0.
|
||||
* @param wbytes Number of bytes to write. Can be 0.
|
||||
* @param rbuff Read buffer. Will be filled with the entire response, including a checksum if the Roboclaw sends
|
||||
* a checksum for this command.
|
||||
* @param rbytes Maximum number of bytes to read.
|
||||
* @param send_checksum If true, then the checksum will be calculated and sent to the Roboclaw.
|
||||
* This is an option because some Roboclaw commands expect no checksum.
|
||||
* @param recv_checksum If true, then this function will calculate the checksum of the returned data and compare
|
||||
* it to the checksum received. If they are not equal, OR if fewer than 2 bytes were received, then an
|
||||
* error is returned.
|
||||
* If false, then this function will expect to read exactly one byte, 0xFF, and will return an error otherwise.
|
||||
* @return If successful, then the number of bytes read from the Roboclaw is returned. If there is a timeout
|
||||
* reading from the Roboclaw, then 0 is returned. If there is an IO error, then a negative value is returned.
|
||||
*/
|
||||
int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
||||
uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false);
|
||||
};
|
||||
|
||||
// unit testing
|
||||
|
||||
@ -107,14 +107,14 @@ int roboclaw_main(int argc, char *argv[])
|
||||
deamon_task = px4_task_spawn_cmd("roboclaw",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
2048,
|
||||
2500,
|
||||
roboclaw_thread_main,
|
||||
(char *const *)argv);
|
||||
exit(0);
|
||||
|
||||
} else if (!strcmp(argv[1], "test")) {
|
||||
|
||||
const char *deviceName = "/dev/ttyS2";
|
||||
const char *deviceName = "/dev/ttyS3";
|
||||
uint8_t address = 128;
|
||||
uint16_t pulsesPerRev = 1200;
|
||||
|
||||
@ -134,7 +134,9 @@ int roboclaw_main(int argc, char *argv[])
|
||||
printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
|
||||
deviceName, address, pulsesPerRev);
|
||||
|
||||
roboclawTest(deviceName, address, pulsesPerRev);
|
||||
//RoboClaw::roboclawTest(deviceName, address, pulsesPerRev);
|
||||
px4_task_spawn_cmd("robclwtst", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2500, RoboClaw::roboclawTest,
|
||||
(char *const *)argv);
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
|
||||
@ -184,11 +186,17 @@ int roboclaw_thread_main(int argc, char *argv[])
|
||||
|
||||
thread_running = true;
|
||||
|
||||
//TODO: Make constants
|
||||
//roboclaw.ScheduleOnInterval(1000000, 1000000);
|
||||
|
||||
// TODO: Move the main loop into the class
|
||||
// loop
|
||||
while (!thread_should_exit) {
|
||||
roboclaw.update();
|
||||
}
|
||||
|
||||
//roboclaw.ScheduleClear();
|
||||
|
||||
// exit
|
||||
printf("[roboclaw] exiting.\n");
|
||||
thread_running = false;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user