Notifications
Clear all

Pico Robotics Straight Line Motion

17 Posts
4 Users
6 Likes
359 Views
THRandell
(@thrandell)
Estimable Member
Joined: 1 year ago
Posts: 117
Topic starter  

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.

Screen Shot 2022 08 29 at 4.42.46 PM
Screen Shot 2022 08 29 at 4.43.23 PM

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]); 
}

 


   
Inq and robotBuilder reacted
Quote
Topic Tags
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 

@thrandell

So you are using the pico for robot brains?

Ultimately you have to use some means to reset to the actual pose values of the robot as the errors accumulate to a degree that the pose values are simply too inaccurate to be useful.

 

This post was modified 3 months ago 2 times by robotBuilder

   
ReplyQuote
THRandell
(@thrandell)
Estimable Member
Joined: 1 year ago
Posts: 117
Topic starter  

@robotbuilder

Yeah, I like programming the Pico.  Its seems very flexible and coding pins for PWM output is a breeze.  More to the point it's only $4 USD!

As I recall with my previous bots I used the wheel encoders mostly to get more accurate turns.  Like when the hider robot spins to face the angle it was bumped on.  I think it was Will that suggested using a beacon to re-calibrate the robot's pose...

 

Tom

P.S.  I became a Grandpa today.  My eldest daughter gave birth to a boy about 3 hours ago 🙂

 


   
huckOhio reacted
ReplyQuote
Inq
 Inq
(@inq)
Noble Member
Joined: 8 months ago
Posts: 959
 

@thrandell - Very interesting problem isn't it?  

I thought happily that someone else is going through this same problem as I sat down in the office today to solve this exact same problem for my Inqling Jr robot.  I have one major simplification in that all I have to do is tell the motor to take a well defined digital step.  Even so, it is still a staggeringly difficult (well... maybe just really tedious) problem solving.  The one you have undertaking is at least an order of magnitude more difficult.

I copied your code into an editor just so I could get the color coding and readily read/search it... and I'm not seeing some things I was expecting.  

For instance... setting a PWM duty cycle and/or PWM frequency affects the velocity only indirectly.  In reality, it's more like a throttle on a car... it increases/decreases the torque.  From there, the bot accelerates up to the steady-state speed.  Invariably, the inertia on one side is different than the other and the motors don't exert exactly the same acceleration to even identical PWM settings, one side will get up to speed first and has veered the bot.  Even at speed one motor invariably is different than the other under the same PWM speed... again veering the robot.

I was expecting to find some kind of feedback loop code from the encoders as one reaches a mile-stone and checks to see if the other side did as well.  And the differential being used to throttle the PWM accordingly on the offending side.  And this... is just the beginning as some means of over compensating is necessary to bring the slow one to re-veer the bot on the chosen path.

Then the fun really starts as you code for coordinating turns!

At the very least, I have someone to cry in my beer with. 🤣 😜 

VBR,

Inq

 

 

3 lines of code = InqPortal = Complete IoT, App, Web Server w/ GUI Admin Client, Access Point Manager, Drag & Drop File Manager, OTA, Performance Metrics, Web Socket Comms, Easy App API, All running on ESP8266...
Even usable on ESP-01S - Quickest Start Guide


   
THRandell reacted
ReplyQuote
Inq
 Inq
(@inq)
Noble Member
Joined: 8 months ago
Posts: 959
 
Posted by: @robotbuilder

@thrandell

So you are using the pico for robot brains?

Ultimately you have to use some means to reset to the actual pose values of the robot as the errors accumulate to a degree that the pose values are simply too inaccurate to be useful.

 

I know what pose means... does it have a different meaning here?

3 lines of code = InqPortal = Complete IoT, App, Web Server w/ GUI Admin Client, Access Point Manager, Drag & Drop File Manager, OTA, Performance Metrics, Web Socket Comms, Easy App API, All running on ESP8266...
Even usable on ESP-01S - Quickest Start Guide


   
ReplyQuote
Inq
 Inq
(@inq)
Noble Member
Joined: 8 months ago
Posts: 959
 
Posted by: @thrandell

Tom

P.S.  I became a Grandpa today.  My eldest daughter gave birth to a boy about 3 hours ago 🙂

Ah! congratulations.  I got my first one last year.  

3 lines of code = InqPortal = Complete IoT, App, Web Server w/ GUI Admin Client, Access Point Manager, Drag & Drop File Manager, OTA, Performance Metrics, Web Socket Comms, Easy App API, All running on ESP8266...
Even usable on ESP-01S - Quickest Start Guide


   
ReplyQuote
Lee G
(@lee-g)
Trusted Member
Joined: 2 years ago
Posts: 37
 

@thrandell Congratulations Gramps!! 😀 


   
THRandell reacted
ReplyQuote
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 
Posted by: @thrandell

P.S.  I became a Grandpa today.  My eldest daughter gave birth to a boy about 3 hours ago 🙂

Although I don't get to see my grandkids (now 3 years and 5 years) very often as they live in another state they are an absolute delight so congrats to all your family.

 

This post was modified 3 months ago by robotBuilder

   
ReplyQuote
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 
Posted by: @inq

I know what pose means... does it have a different meaning here?

The position and orientation of the robot in a coordinate frame is known as its pose.

 


   
ReplyQuote
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 

@inq 

I was expecting to find some kind of feedback loop code from the encoders as one reaches a mile-stone and checks to see if the other side did as well. And this... is just the beginning as some means of over compensating is necessary to bring the slow one to re-veer the bot on the chosen path.

That is what I do. If for example the robot is to travel in a straight line it monitors both encoders and adjusts the pwm so the encoders return the same number of pulses. And as you also alluded to there is the issue of not just adjusting the pwm but actually returning to the path and really you need to feed the encoder values into a function that returns the theoretical current pose of the robot by translating the received encoder pulses into a current direction and position. You should be able to give it a goal position relative to the starting position and let it get there by monitoring the encoders. In other words if you were controlling it yourself by adjusting the pwm using say a joystick the robot should be able to determine where it is and perhaps display it for you.

 

This post was modified 3 months ago 6 times by robotBuilder

   
ReplyQuote
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 

@inq 

I have one major simplification in that all I have to do is tell the motor to take a well defined digital step.

Indeed that should make it a lot simpler which is why they are used in printers, printer plotters and some robotic arms for blind pick up and place tasks. If you take your robot off steroids so it moves a bit slower you might just try it on a hard flat surface and see if you can program it to take a complicated path and return back to the same position and direction.

 

This post was modified 3 months ago by robotBuilder

   
ReplyQuote
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 

@thrandell 

@inq  FYI

This would make an ideal swarm bot 🙂

On this site in a video they show it drawing. It doesn't say it uses stepper motors but if it did it should be able to draw some nice mathematical patterns or accurately move around.

https://meetedison.com/edsketch/

 


   
THRandell reacted
ReplyQuote
Inq
 Inq
(@inq)
Noble Member
Joined: 8 months ago
Posts: 959
 
Posted by: @robotbuilder

And as you also alluded to there is the issue of not just adjusting the pwm but actually returning to the path and really you need to feed the encoder values into a function that returns the theoretical current pose of the robot by translating the received encoder pulses into a current direction and position. You should be able to give it a goal position relative to the starting position and let it get there by monitoring the encoders.

I think I miss-spoke...  TWO ORDERS OF MAGNITUDE!  🤣 😍 I got to hand it to you two and others trying to do this with regular motors, encoders and PWM.  Every time I attempt to wrap my head around all the pitfalls, I simply can't come up with a solution I think will work.

Posted by: @robotbuilder

If you take your robot off steroids so it moves a bit slower you might just try it on a hard flat surface and see if you can program it to take a complicated path and return back to the same position and direction.

Yeah!... the last video of it was way out of control! 😉 And... I've finished having fun with it that way.  Time to get serious about computer controlled versus human controlled movement.

I hope to get out to my test site auditorium tomorrow (if I get this thing coded today) to test my new drive code.  I also have some new stuff to present on the ToF sensor.  Will upload videos from site with good Internet.

VBR,

Inq

3 lines of code = InqPortal = Complete IoT, App, Web Server w/ GUI Admin Client, Access Point Manager, Drag & Drop File Manager, OTA, Performance Metrics, Web Socket Comms, Easy App API, All running on ESP8266...
Even usable on ESP-01S - Quickest Start Guide


   
ReplyQuote
robotBuilder
(@robotbuilder)
Noble Member
Joined: 3 years ago
Posts: 1554
 

@inq 

I think I miss-spoke... TWO ORDERS OF MAGNITUDE! 🤣 😍 I got to hand it to you two and others trying to do this with regular motors, encoders and PWM. Every time I attempt to wrap my head around all the pitfalls, I simply can't come up with a solution I think will work.

Here is an example of the math.
At each unit of time we have DL pulses from the left wheel and DR pulses from the right wheel.
We begin at position x,y in some global coordinate system pointing in some direction given in degrees theta (0 to 359).
Here is the snippet of code I use to compute the change in the x,y coordinates and the direction theta given the pulse counts DL and DR in each unit of time.  

 

 

    
    // DtoR converts degrees to radians
    float DtoR = 0.01745329
    
    if (DL==DR) {
      // Moving in a straight line 
      x = x + DL * cos(theta*DtoR);
      y = y + DL * sin(theta*DtoR);
      
    }else{
        
      // Moving in an arc 
      expr1 = w * (DR + DL)/ 2.0 / (DR - DL);

      x = x + expr1 * (sin((DR - DL) / w + theta*DtoR) - sin(theta*DtoR));

      y = y - expr1 * (cos((DR - DL) / w + theta*DtoR) - cos(theta*DtoR));

      // Calculate new orientation 
      theta = (theta*DtoR + (DR - DL) / w)*RtoD;
      
      // Keep in the range -PI to +PI 
      if theta > 359 then theta = theta - 360;
      if theta < 0   then theta = theta + 360;
      
    }

To enlarge image, right click image and choose Open link in new window.

odometryMath

Some possible sources of error.

§ Limited resolution during integration
§ Unequal wheel diameter
§ Variation in the contact point of the wheel
§ Unequal floor contact and variable friction can lead to slipping

 

This post was modified 3 months ago 3 times by robotBuilder

   
ReplyQuote
THRandell
(@thrandell)
Estimable Member
Joined: 1 year ago
Posts: 117
Topic starter  
Posted by: @robotbuilder

You should be able to give it a goal position relative to the starting position and let it get there by monitoring the encoders.

and @inq

Where this conversation is going reminded me of a Dead Reckoning Competition that I saw years ago.  Check it out.  I believe that the winner said that through out the run the robot used trigonometry to re-calculate its return bearing.

Tom

 

 


   
ReplyQuote
Page 1 / 2