Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RobotisServo: fixes to give smooth operation on multiple servos

Browse Source
This commit is contained in:
Andrew Tridgell 2018-12-21 16:48:53 +11:00
parent 403bfa1031
commit 92f4b37e90
2 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_RobotisServo/AP_RobotisServo.cpp
View File

@ -97,6 +97,9 @@ extern const AP_HAL::HAL& hal;
#define REG_GOAL_POSITION 116 #define REG_GOAL_POSITION 116
// how many times to send servo configure msgs
#define CONFIGURE_SERVO_COUNT 4
const AP_Param::GroupInfo AP_RobotisServo::var_info[] = { const AP_Param::GroupInfo AP_RobotisServo::var_info[] = {
// @Param: POSMIN // @Param: POSMIN
@ -285,32 +288,34 @@ void AP_RobotisServo::detect_servos(void)
*/ */
void AP_RobotisServo::configure_servos(void) void AP_RobotisServo::configure_servos(void)
{ {
// disable replies unless we read // disable torque control
send_command(BROADCAST_ID, REG_STATUS_RETURN, STATUS_RETURN_READ); send_command(BROADCAST_ID, REG_TORQUE_ENABLE, 0, 1);
// disable replies unless we read
send_command(BROADCAST_ID, REG_STATUS_RETURN, STATUS_RETURN_READ, 1);
// use position control mode // use position control mode
send_command(BROADCAST_ID, REG_OPERATING_MODE, OPMODE_POS_CONTROL); send_command(BROADCAST_ID, REG_OPERATING_MODE, OPMODE_POS_CONTROL, 1);
// enable torque control // enable torque control
send_command(BROADCAST_ID, REG_TORQUE_ENABLE, 1); send_command(BROADCAST_ID, REG_TORQUE_ENABLE, 1, 1);
} }
/* /*
send a command to a single servo, changing a register value send a command to a single servo, changing a register value
*/ */
void AP_RobotisServo::send_command(uint8_t id, uint16_t reg, uint16_t value) void AP_RobotisServo::send_command(uint8_t id, uint16_t reg, uint32_t value, uint8_t len)
{ {
uint8_t txpacket[16] {}; uint8_t txpacket[16] {};
txpacket[PKT_ID] = id; txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH_L] = 9; txpacket[PKT_LENGTH_L] = 5 + len;
txpacket[PKT_LENGTH_H] = 0; txpacket[PKT_LENGTH_H] = 0;
txpacket[PKT_INSTRUCTION] = INST_WRITE; txpacket[PKT_INSTRUCTION] = INST_WRITE;
txpacket[PKT_INSTRUCTION+1] = DXL_LOBYTE(reg); txpacket[PKT_INSTRUCTION+1] = DXL_LOBYTE(reg);
txpacket[PKT_INSTRUCTION+2] = DXL_HIBYTE(reg); txpacket[PKT_INSTRUCTION+2] = DXL_HIBYTE(reg);
txpacket[PKT_INSTRUCTION+3] = DXL_LOBYTE(value); memcpy(&txpacket[PKT_INSTRUCTION+3], &value, MIN(len,4));
txpacket[PKT_INSTRUCTION+4] = DXL_HIBYTE(value);
send_packet(txpacket); send_packet(txpacket);
} }
@ -416,8 +421,8 @@ void AP_RobotisServo::update()
return; return;
} }
if (!configured_servos) { if (configured_servos < CONFIGURE_SERVO_COUNT) {
configured_servos = true; configured_servos++;
last_send_us = now; last_send_us = now;
configure_servos(); configure_servos();
return; return;
@ -439,7 +444,7 @@ void AP_RobotisServo::update()
const uint16_t min = c->get_output_min(); const uint16_t min = c->get_output_min();
const uint16_t max = c->get_output_max(); const uint16_t max = c->get_output_max();
float v = float(pwm - min) / (max - min); float v = float(pwm - min) / (max - min);
uint16_t value = pos_min + v * (pos_max - pos_min); uint32_t value = pos_min + v * (pos_max - pos_min);
send_command(i+1, REG_GOAL_POSITION, value); send_command(i+1, REG_GOAL_POSITION, value, 4);
} }
} }

4
libraries/AP_RobotisServo/AP_RobotisServo.h
View File

@ -48,12 +48,12 @@ private:
void send_packet(uint8_t *txpacket); void send_packet(uint8_t *txpacket);
void read_bytes(); void read_bytes();
void process_packet(const uint8_t *pkt, uint8_t length); void process_packet(const uint8_t *pkt, uint8_t length);
void send_command(uint8_t id, uint16_t reg, uint16_t value); void send_command(uint8_t id, uint16_t reg, uint32_t value, uint8_t len);
void configure_servos(void); void configure_servos(void);
// auto-detected mask of available servos, from a broadcast ping // auto-detected mask of available servos, from a broadcast ping
uint16_t servo_mask; uint16_t servo_mask;
bool configured_servos; uint8_t configured_servos;
bool initialised; bool initialised;
uint8_t pktbuf[64]; uint8_t pktbuf[64];