Servo2040: servos 1 and 17 and 2 and 18 moving together

[firkowski]

edit: in the code, some parts are not used to simplify debugging. the #define servo constants are not used

hello, I am working on a hexapod project and i am using Servo2040 board with micro-ROS. I noticed strange behavior when running my C++ code. servos 1 and 17 would shake uncontrollably and change directions rapidly. it took me a while to realize that servos 1 and 17 and 2 and 18 were moving in the same way. I may have an issue with the I2C causing them to receive the same signals (i think the reason for the shaking was that servos 1 and 17 were receiving two signals at once). I would like to be able to control them independently. My full code (modified for testing) is below but I've highlighted the part that causes problems.

problematic part:

void init() {
    for (int i = 0; i < NUM_SERVOS; i++) {
        cals[i] = std::make_unique<Calibration>();
        cals[i]->apply_three_pairs(min_pulses[i]      , mid_pulses[i], max_pulses[i]     ,
                               -WIDE_ANGLE_RANGE/2,             0, WIDE_ANGLE_RANGE/2);
        servos[i] = std::make_unique<Servo>(i, *cals[i]);
        servos[i]->init();
    }
    for (int i = 0; i < NUM_SERVOS; i++) {
        servos[i]->enable();
    }
    servos[16]->value(-30); //causes servo 16 and 1 to move
    servos[17]->value(45); //causes servo 17 and 2 to move**

whole code:

#include <stdio.h>
#include "pico/stdlib.h"

#include "rclc/rclc.h"

#include <time.h>
#include <vector>
#include <memory>
#include <map>
#include <cmath>

#include "servo2040_uros/msg/feet_contacts.h"
#include "servo2040_uros/msg/current.h"
#include "servo2040_uros/msg/joint_angles.h"
#include "sensor_msgs/msg/joint_state.h"

extern "C"{
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>

#include "pico_uart_transports.h"
}

#include "servo2040.hpp"
#include "analogmux.hpp"
#include "analog.hpp"
#include "button.hpp"

#define NUM_LEGS 6
#define NUM_SERVOS_PER_LEG 3
#define NUM_SERVOS 18

#define SERVO_L1C 0 //1
#define SERVO_L1F 1 //2
#define SERVO_L1T 2 //3
#define SERVO_L2C 3 //4
#define SERVO_L2F 4 //5
#define SERVO_L2T 5 //6
#define SERVO_L3C 6 //7
#define SERVO_L3F 7 //8
#define SERVO_L3T 8 //9
#define SERVO_R1C 9 //16
#define SERVO_R1F 10 //17
#define SERVO_R1T 11 //18
#define SERVO_R2C 12 //13 
#define SERVO_R2F 13 //14
#define SERVO_R2T 14 //15
#define SERVO_R3C 15 //10
#define SERVO_R3F 16 //11
#define SERVO_R3T 17                                                                                                                                                                                                                     //12

#define WIDE_ANGLE_RANGE 180

#define SENSOR_L1 0
#define SENSOR_L2 1
#define SENSOR_L3 2
#define SENSOR_R1 3
#define SENSOR_R2 4
#define SENSOR_R3 5

#define SENSOR_VOLTAGE_THRESHOLD 2.0

using namespace servo;

//   L1C   L1F   L1T   L2C   L2F   L2T   L3C   L3F   L3T 
//   R1C   R1F   R1T   R2C   R2F   R2T   R3C   R3F   R3T
std::vector<int> min_pulses = {
     580,  500,  580,  580,  500,  500,  500,  500,  500, 
     580,  580,  500,  550,  500,  530,  520,  520,  500
};

std::vector<int> max_pulses = {
    2500, 2420, 2500, 2500, 2500, 2400, 2500, 2450, 2500, 
    2500, 2580, 2500, 2500, 2500, 2500, 2500, 2500, 2500
};

std::vector<int> mid_pulses = {
    1550, 1450, 1580, 1580, 1580, 1450, 1500, 1420, 1500, 
    1580, 1580, 1500, 1550, 1500, 1530, 1520, 1550, 1520
};

std::map<int, int> sensor_IDs {
    {0,  SENSOR_L1},
    {1,  SENSOR_L2},
    {2,  SENSOR_L3},
    {3,  SENSOR_R1},
    {4,  SENSOR_R2},
    {5,  SENSOR_R3}
};

rcl_publisher_t    feet_contacts_pub_;
rcl_publisher_t    current_pub_;
rcl_subscription_t joint_angles_sub_;
servo2040_uros__msg__FeetContacts   feet_contacts_msg;
servo2040_uros__msg__Current current_msg;
servo2040_uros__msg__JointAngles    joint_angles_msg;

rcl_publisher_t joint_state_pub_;
sensor_msgs__msg__JointState         joint_state_msg;

enum states {
  WAITING_AGENT,
  AGENT_AVAILABLE,
  AGENT_CONNECTED,
  AGENT_DISCONNECTED
} state;

rcl_timer_t timer;
rcl_node_t node;
rcl_allocator_t allocator;
rclc_support_t support;
rclc_executor_t executor;

std::array<std::unique_ptr<Servo>, NUM_SERVOS> servos;
std::array<std::unique_ptr<Calibration>, NUM_SERVOS> cals;

Analog sen_adc = Analog(servo2040::SHARED_ADC);
Analog vol_adc = Analog(servo2040::SHARED_ADC, servo2040::VOLTAGE_GAIN);
Analog cur_adc = Analog(servo2040::SHARED_ADC, servo2040::CURRENT_GAIN,
                        servo2040::SHUNT_RESISTOR, servo2040::CURRENT_OFFSET);

AnalogMux mux = AnalogMux(servo2040::ADC_ADDR_0, servo2040::ADC_ADDR_1, servo2040::ADC_ADDR_2,
                          PIN_UNUSED, servo2040::SHARED_ADC);

void set_servo(int i, float deg) {
    if (i > 8 && (i % 3) != 0) {
        servos[i]->value(-deg);
        return;
    }
    servos[i]->value(deg);
}

**void init() {
    for (int i = 0; i < NUM_SERVOS; i++) {
        cals[i] = std::make_unique<Calibration>();
        cals[i]->apply_three_pairs(min_pulses[i]      , mid_pulses[i], max_pulses[i]     ,
                               -WIDE_ANGLE_RANGE/2,             0, WIDE_ANGLE_RANGE/2);
        servos[i] = std::make_unique<Servo>(i, *cals[i]);
        servos[i]->init();
    }
    for (int i = 0; i < NUM_SERVOS; i++) {
        servos[i]->enable();
    }
    servos[16]->value(-30); //causes servo 16 and 1 to move
    servos[17]->value(45); //causes servo 17 and 2 to move**

    for(auto i = 0u; i < servo2040::NUM_SENSORS; i++) {
        mux.configure_pulls(servo2040::SENSOR_1_ADDR + i, false, true);
    }

    joint_state_msg.position.capacity = NUM_SERVOS;
    joint_state_msg.position.data = (double*) malloc(joint_state_msg.position.capacity * sizeof(double));
    joint_state_msg.position.size = 0;

    for (int i = 0; i < NUM_SERVOS; i++) {
        joint_state_msg.position.data[i] = 0.0;
        joint_state_msg.position.size++;
    }

    rcl_ret_t feet_contacts_ret = rcl_publish(&joint_state_pub_, &joint_state_msg, NULL);

}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
    // ground detection
    for(auto i = 0u; i < servo2040::NUM_SENSORS; i++) {
        mux.select(servo2040::SENSOR_1_ADDR + i);
        feet_contacts_msg.is_active[sensor_IDs[i]] = (sen_adc.read_voltage() < SENSOR_VOLTAGE_THRESHOLD);
    }
    rcl_ret_t feet_contacts_ret = rcl_publish(&feet_contacts_pub_, &feet_contacts_msg, NULL);

    mux.select(servo2040::CURRENT_SENSE_ADDR);
    current_msg.current = cur_adc.read_current();
    rcl_ret_t current_ret = rcl_publish(&current_pub_, &current_msg, NULL);

}

void subscription_callback(const void * msgin)
{
    const servo2040_uros__msg__JointAngles * msg = (const servo2040_uros__msg__JointAngles *)msgin;

    if (msg == NULL) 
        return;

    for (size_t i = 0; i < NUM_SERVOS; i++) {
        float deg = static_cast<float>(msg->joint_angles[i]) * 180.0 / M_PI;
        joint_state_msg.position.data[i] = deg;
        set_servo(i, deg);
    }
    rcl_ret_t feet_contacts_ret = rcl_publish(&joint_state_pub_, &joint_state_msg, NULL);
}

bool pingAgent(){
    const int timeout_ms = 1000;
    const uint8_t attempts = 10;

    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);

    if (ret != RCL_RET_OK){
        return false;
    } 
    return true;
}

void createEntities() {

    allocator = rcl_get_default_allocator();
    rclc_support_init(&support, 0, NULL, &allocator);

    rclc_node_init_default(&node, "servo2040_node", "", &support);

    rclc_subscription_init_default(
        &joint_angles_sub_,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(servo2040_uros, msg, JointAngles),
        "/joint_angles");

    rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(100),
        timer_callback);

    // create publishers
    rclc_publisher_init_default(
        &feet_contacts_pub_,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(servo2040_uros, msg, FeetContacts),
        "/servo2040/feet_contacts");

    rclc_publisher_init_default(
        &joint_state_pub_,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
        "/servo2040/joint_states");

    rclc_publisher_init_default(
        &current_pub_,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(servo2040_uros, msg, Current),
        "/servo2040/current");

    rclc_executor_init(&executor, &support.context, 2, &allocator);
    rclc_executor_add_timer(&executor, &timer);

    rclc_executor_add_subscription(&executor,
            &joint_angles_sub_,
            &joint_angles_msg,
            &subscription_callback,
            ON_NEW_DATA);
}

void destroyEntities(){
    rmw_context_t * rmw_context = rcl_context_get_rmw_context(&support.context);
    (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

    rcl_publisher_fini(&feet_contacts_pub_, &node);
    rcl_publisher_fini(&current_pub_, &node);
    rcl_publisher_fini(&joint_state_pub_, &node);
    rcl_subscription_fini(&joint_angles_sub_, &node);
    rcl_timer_fini(&timer);
    rclc_executor_fini(&executor);
    rcl_node_fini(&node);
    rclc_support_fini(&support);
}

int main()
{   
    stdio_init_all();

    init();

    rmw_uros_set_custom_transport(
        true,
        NULL,
        pico_serial_transport_open,
        pico_serial_transport_close,
        pico_serial_transport_write,
        pico_serial_transport_read
    );

    state = WAITING_AGENT;

    while (true){
        switch (state) {
            case WAITING_AGENT:
              state = pingAgent() ? AGENT_AVAILABLE : WAITING_AGENT;
              break;
            case AGENT_AVAILABLE:
              createEntities();
              state = AGENT_CONNECTED ;
              break;
            case AGENT_CONNECTED:
              state = pingAgent() ? AGENT_CONNECTED : AGENT_DISCONNECTED;
              if (state == AGENT_CONNECTED) {
                rclc_executor_spin(&executor);
              }
              break;
            case AGENT_DISCONNECTED:
              destroyEntities();
              state = WAITING_AGENT;
              break;
            default:
              break;
          }
    }
}

[ZodiusInfuser]

Hi @firkowski,

Cool to see someone using Servo 2040 for the primary purpose I designed it for!

The issue you are experiencing is expected, due to the RP2040 *only having 16 PWM channels. As such when you create the 17th and 18th servo classes, even though they are independent classes, the same hardware registers will be controlled, resulting in what you describe.

The way around this is to switch from using the Servo class to using the ServoCluster class. This is a PIO implementation of PWM that supports 1 to 30 of RP2040's pins. It also offers phase control, so that servos don't all receive their commands at the same time, spreading out the peak current draw.

You can find the code for ServoCluster at: https://github.com/pimoroni/pimoroni-pico/blob/main/drivers/servo/servo_cluster.hpp

It seems I never got to writing documentation for the C implementation of this class, but the Micropython documentation should help you. https://github.com/pimoroni/pimoroni-pico/tree/main/micropython/modules/servo#servocluster There is also this example: https://github.com/pimoroni/pimoroni-pico/blob/main/examples/servo2040/servo2040_servo_cluster.cpp

This should be a relatively painless transition for your code, as the APIs for the two classes were written concurrently (they both use the same ServoState object internally). So for your code, the main change would be to swap std::array<std::unique_ptr<Servo>, NUM_SERVOS> servos; over to a single ServoCluster instance.

Hope that helps. Let me know if you have any specific questions

[ZodiusInfuser]

Closing this. If there are any bugs with getting it working, please reopen this or raise a new issue.

[firkowski]

It works great now, thank you! One thing is that I can't figure out how to apply calibration to the servos in ServoCluster. There does not seem to be a calibration(servo, calibration) function in the c++ code, or at least i can't find it