Pico Robotics Straight Line Motion

It seems that whenever I build a new robot with differential drive I find that the speeds of the motors never match and I need to do some calibration if I want the robot to travel in a straight line.

This little example shows you how to gather the information needed to get your robots to travel straight and also how you can convert rotational and forward velocity steering commands into PWM duty cycle (speed) values for your Pico based robot. 

In the past I would manual set the speed of the motors and run the robot, for one second, beside a yard stick, adjusting the speeds until I got a straight (enough) path and measuring the distance covered.  From this I had enough data points to trend the motor speeds by meters per second traveled.  It was then pretty straight forward to program a conversion of the desired velocity into motor speed values.

Anyway, in an effort to automate that process somewhat, I wrote this program to run on the Pico.  It’s a combination of two programs that I have already uploaded to the Raspberry Pi topic.   Hardware wise it requires wheel encoders to give the distance traveled in one second.  Hopefully, if you are using encoders you know the distance (in millimeters) traveled per click.  The formula I use is: pi * wheel diameter in mm / motor’s gear ratio * clicks per revolution.  Eg,  3.1416 * 32 / 210 * 6 or 100.53 / 1260 = 0.0797 millimeters per click.  If you don’t have encoders and you want to follow along you may need to gather data the manual way.

/*
    A version of motor_test.ino to run on the Pico
*/

#include <stdio.h>
#include <stdlib.h>    // for abs() 
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/pwm.h"

// Motor driver GPIO pin setup
#define M1PWM      17  // connects to DRV A enable - PWM
#define M1DIR      16  // connects to DRV A phase - direction
#define M2DIR      1   // connects to DRV B phase - direction
#define M2PWM      0   // connects to DRV B enable - PWM

// Encoder GPIO pins 
#define LH_ENCODER_A   2  // interrupt
#define LH_ENCODER_B   3  
#define RH_ENCODER_A   4  // interrupt
#define RH_ENCODER_B   5

// pi * wheel diameter / gear ratio * encoder clicks per revolution
// pi * 32mm / 1260 cpr = 0.07978 millimeters
const float distPerClick = 0.07978; 

static volatile int32_t leftCount;
static volatile int32_t rightCount;

int8_t dc_arr[9] = {10, 20, 30, 40, 50, 60, 70, 80, 90};
uint64_t timePeriod;


// Interrupt routine (callback)
void encoder_callback(uint gpio, uint32_t events) {
  uint32_t gpio_state = 0;
  gpio_state = (gpio_get_all() >> 2) & 0b01111;  // mask out all but pins 5, 4, 3 and 2

  // get raw values of all GPIO pins (0-29), >> shift right 2 places, & = binary AND (1&1=1)
  // So the 2 places on the right are dropped and the rightmost places counting toward the
  // left are now GPIO pins ... 5 4 3 2.  These are binary AND'd to give everything 
  // but 5, 4, 3  and 2 a zero.
  // This will need to be changed to match the GPIO pins being used.

  uint8_t enc_value = 0;

  if (gpio == LH_ENCODER_A) {   // pin 2
    enc_value = (gpio_state & 0b00011);  // mask out all but pins 3 and 2
    if (enc_value == 0b01)
      leftCount++;
    else if (enc_value == 0b11)
      leftCount--;
    else if (enc_value == 0b00)
      leftCount--;
    else // enc_value == 0b10 
      leftCount++;
  }

  if (gpio == RH_ENCODER_A) {  // pin 4
    enc_value = (gpio_state & 0b01100);  // mask out all but pins 5 and 4
    if (enc_value == 0b0100)
      rightCount--;
    else if (enc_value == 0b1100)
      rightCount++;
    else if (enc_value == 0b0000)
      rightCount++;
    else // enc_value == 0b1000 
      rightCount--;
  }
}

// PWM functions
uint32_t pwm_set_freq_duty(uint slice_num, uint chan, uint32_t f, int d)
{
  uint32_t clock = 125000000;
  uint32_t divider16 = clock / f / 4096 + (clock % (f * 4096) != 0);
  if (divider16 / 16 == 0)
    divider16 = 16;
  uint32_t wrap = clock * 16 / divider16 / f - 1;
  pwm_set_clkdiv_int_frac(slice_num, divider16 / 16, divider16 & 0xF);
  pwm_set_wrap(slice_num, wrap);
  pwm_set_chan_level(slice_num, chan, wrap * d / 100);
  return wrap;
}

uint32_t pwm_get_wrap(uint slice_num)
{
  valid_params_if(PWM, slice_num >= 0 && slice_num < NUM_PWM_SLICES);
  return pwm_hw->slice[slice_num].top;
}

void pwm_set_duty(uint slice_num, uint chan, int d)
{
  pwm_set_chan_level(slice_num, chan, pwm_get_wrap(slice_num) * d / 100);
}

// Motor speed commands
void setM1Speed(uint slice_num, uint chanM1, int speed) { // values can be -100 to 100.
  bool reverse = 0;

  if (speed < 0) {   // make speed a positive number
     speed = -speed;
     reverse = 1;
  }
  if (speed > 100)  // max PWM duty cycle
     speed = 100;
  pwm_set_chan_level(slice_num, chanM1, pwm_hw->slice[slice_num].top * speed / 100);

  if (reverse)
     gpio_put(M1DIR, 1);                // drive m1 direction gpio high 
  else
     gpio_put(M1DIR, 0);                // drive m1 direction gpio low 
}

void setM2Speed(uint slice_num, uint chanM2, int speed) {
  bool reverse = 0;

  if (speed < 0) {   // make speed a positive number
     speed = -speed;
     reverse = 1;
  }
  if (speed > 100)  // max PWM duty cycle
     speed = 100;
  pwm_set_chan_level(slice_num, chanM2, pwm_hw->slice[slice_num].top * speed / 100);
  // This will set the current PWM counter compare value for the channel
  if (reverse)
    gpio_put(M2DIR, 1);                // drive m2 direction gpio high 
  else
    gpio_put(M2DIR, 0);                // drive m2 direction gpio low
}

void setSpeeds(uint slice_num, uint chanM1, uint chanM2, int m1Speed, int m2Speed) {
   setM1Speed(slice_num, chanM1, m1Speed);
   setM2Speed(slice_num, chanM2, m2Speed);
}


int main()
{
  stdio_init_all();
  gpio_init(LH_ENCODER_A);             // Initialize a GPIO, enable I/O set to GPIO_SIO
  gpio_set_dir(LH_ENCODER_A, GPIO_IN); // Set a single GPIO direction
  gpio_pull_up(LH_ENCODER_A);
  gpio_init(LH_ENCODER_B);
  gpio_set_dir(LH_ENCODER_B, GPIO_IN);
  gpio_pull_up(LH_ENCODER_B);

  gpio_init(RH_ENCODER_A); 
  gpio_set_dir(RH_ENCODER_A, GPIO_IN);
  gpio_pull_up(RH_ENCODER_A);        
  gpio_init(RH_ENCODER_B);
  gpio_set_dir(RH_ENCODER_B, GPIO_IN);
  gpio_pull_up(RH_ENCODER_B);

  // you can only have one GPIO irq callback function per processor
  gpio_set_irq_enabled_with_callback(RH_ENCODER_A, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true, &encoder_callback);
  gpio_set_irq_enabled_with_callback(LH_ENCODER_A, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true, &encoder_callback);
  
  gpio_init(M1DIR); 
  gpio_set_dir(M1DIR, GPIO_OUT);
  gpio_put(M1DIR, 0);
  gpio_init(M2DIR);                
  gpio_set_dir(M2DIR, GPIO_OUT);  
  gpio_put(M2DIR, 0);            
  gpio_set_function(M1PWM, GPIO_FUNC_PWM); 
  gpio_set_function(M2PWM, GPIO_FUNC_PWM); 
  uint slice_num = pwm_gpio_to_slice_num(M1PWM); 

  uint chanM1 = pwm_gpio_to_channel(M1PWM);
  uint chanM2 = pwm_gpio_to_channel(M2PWM);
  
  pwm_set_freq_duty(slice_num, chanM1, 40000, 0);
  pwm_set_chan_level(slice_num, chanM2, 0); 
  pwm_set_phase_correct(slice_num, true); // this will half the frequency
  pwm_set_enabled(slice_num, true);        

  leftCount = 0;
  rightCount = 0;

  sleep_ms(5000);
  while (true)
  {
    for (int x = 0; x <= 8; x++) {
      for (int y = dc_arr[x]-3; y <= dc_arr[x]+3; y++) {
        leftCount = 0;
        rightCount = 0;
        timePeriod = time_us_64() + 1000000;
        setSpeeds(slice_num, chanM1, chanM2, dc_arr[x], y);
        while (time_us_64() < timePeriod) 
          ;  // nop 
        setSpeeds(slice_num, chanM1, chanM2, 0, 0);
        printf("Left speed = %d  Right speed = %d Distance = %.1f mm/sec  ", dc_arr[x], y, leftCount*distPerClick);
        printf("Left clicks = %d  Right clicks = %d Difference = %d\n", leftCount, rightCount, abs(leftCount-rightCount));
        sleep_ms(5000);
      }
      printf("\n");
      sleep_ms(5000);
    }
  }
}

The program uses fixed left wheel speed values to come up with right wheel speed values, that will move the robot in a straight line, and the velocity in meters per second.  The next step is to create a spreadsheet of the data points and graph them.  Most spreadsheets have a way to create a linear trend line from a graph and that linear equation is what we need for the final step.

I know, I know it’s a slow robot!

Finally, we can use those linear equations in a subroutine that takes the rotational and forward velocity we want the robot to use and converts them to speed values that the Pico can implement.  Here is the routine to do the conversion and populate the results into two arrays, left_vel[ ] and right_vel[ ].  Those arrays get used later in the code to move the actuators.  In our case with the Pico, speed values are just the PWM duty cycles for the motors.

void steer(float rotation, float translation, unsigned char beh_id) {
  float radius = baseline/1000.0;  // the radius of rotation = baseline, only in meters not mm
  float velRight, velLeft, leftWheelSpeed, rightWheelSpeed = 0;

//  Calculation of left and right wheel velocities 
//    from forward velocity v(t) and rotational velocity w(t).  
//  Let L = the baseline in meters, velocities are in units of meters/sec, 
//    rotational velocity is in radians/sec.
//         vRight = v + (wL/2);
//         vLeft  = v - (wL/2);        so -w => clockwise and +w => counter clockwise      
//
  velLeft  = translation - (rotation * radius)/2;
  velRight = translation + (rotation * radius)/2;   

  if (rotation == 0.0 && translation == 0.0) {
    left_vel[beh_id] = 0;
    right_vel[beh_id] = 0;
  }
  else {   // calculate wheel speeds (ie duty cycle) from velocity
    printf("absolute of velLeft = %f and fabsolute of velRight = %f\n", abs(velLeft), fabs(velRight));
    rightWheelSpeed = 524.17 * fabs(velRight) + 1.2242;
    right_vel[beh_id] = rightWheelSpeed * signfloat(velRight); // float to int16
    leftWheelSpeed = 525.47 * fabs(velLeft) + 0.8799;
    left_vel[beh_id] = leftWheelSpeed * signfloat(velLeft); // float to int16
  }
}

Oh, I almost forgot the baseline value used above is the distance between the wheels on your differential drive robot.  I grab the mid-wheel to mid-wheel distance in millimeters.  Here is an example of calling the steer( ) command and another of using the arrays to set motor speeds.  Have fun…

Tom

steer(0.0, pc_trans_vel,CRUISE_ID);  

int signum(signed int value) {  // Returns the sign of the argument.
  if (value > 0)
    return 1;
  else
    return -1;
}

int16_t xfer (int16_t signal, uint16_t saturation, uint8_t dz_half_width) {  
                // From Jones, to account for actuator saturation and dead zone
                // It may be useful with some Proportional controlers I'll implement one day.
  if (abs(signal) < dz_half_width)
    return 0;
  else if (abs(signal) > saturation)
    return signum(signal) * saturation;
  else
    return signal;
}

void move_dual(unsigned char winner) {
// The saturation and deadzone number for the two metal gear motors
  left_vel[winner] = xfer(left_vel[winner], 100, 5);
  right_vel[winner] = xfer(right_vel[winner], 100, 6);

  //set the two motor speeds here
  setSpeeds(slice_Motors, chan_M1, chan_M2, left_vel[winner], right_vel[winner]); 
}