To drive two stepper motors via their stepper controllers I'm trying to control two pins (GPIO 16 and GPIO 17) on the Pico by using a PIO. The idea is, that a function in main determines, if either GPIO 16 or GPIO 17 or both have to be set to HIGH.
I want to send a two bit sequence to the FIFO of the PIO, so that: 0x00: no pins are set to HIGH 0x01: pin 16 is set to HIGH 0x10: pin 17 is set to HIGH 0x11: both pins are set to HIGH
the PIO block looks like this (without the inline c code to configure the state machine)
.program stepper
set pindirs, 1
.wrap_target
pull block
out pins, 2 [31]
set pins, 0
.wrap
In the PIO program the pull block should take the next sequence (0x00, 0x01, 0x10 or 0x11) from the FIFO and send it to the pins.
For example:
If the sequence 0x10 has been sent to the PIO, Pin 16 should be turned on, Pin 17 should remain off. after that, the next sequence should be loaded into the PIO OSR with the next pull command.
I'm calculating the sequence to be sent in main like this:
pio_signal = signal_1 + 2*signal_2;
that should result in numbers from 0 to 3, that translate to 00 to 11 in binary.
I think I could probably achieve this in in python (where I do most of my work), but i have no idea how to do this in C. Unfortunately python is off the table here, as the python code turned out to be much too slow to feed the steppers fast enough... (such a shame, as programming in micropython on the pico is so much more convenient than C).
do I have to set up a DMA (please god no...), or is there an easier way to do this?
I already tried to find some examples, that do similar things, but it seems their functionality is much broader and their solutions would be overkill here.
Edit: Currently I'm using the following code (I simplified it a little). In this code example the stepper takes 100.000 steps.
The general idea of the stepper code is to produce a very smooth motion with nice acceleration and deceleration phase with constant speed in between.
This is achieved by running a loop, that does the following:
- add acceleration to current speed value
- add speed to current position value
- if position value gets to the next whole number, take a step
This code only uses addition and subtraction to control the motors and should therefore run really fast.
the "source.h" file (shown below the main code) only includes a function to get the current time in microseconds.
#include "source.h"
#define STEP_1 16 // step pin of stepper 1
#define DIR_1 17. // direction pin of stepper 1
#define STEP_2 20 // step pin of stepper 2
#define DIR_2 21 // direction pin of stepper 2
#define OVERLOAD_LED 25 // onboard LED
// main stepper loop should run once every 32 microseconds.
// the steppers take 6400 steps for 1 revolution,
// I am targeting a maximum speed of 1 rotation per second
// and the loop has to be executed a minimum of 4 times for
// a step to happen (at maximum speed)
#define TIME_RASTER_US 32
int main() {
gpio_init(OVERLOAD_LED); // LED used to indicate if loop is running behind
gpio_set_dir(OVERLOAD_LED, GPIO_OUT);
gpio_put(OVERLOAD_LED, 0); // set onboard led to off
// setting up the GPIOs
gpio_init(DIR_1);
gpio_init(DIR_2);
gpio_init(STEP_1);
gpio_init(STEP_2);
gpio_set_dir(DIR_1, GPIO_OUT);
gpio_set_dir(DIR_2, GPIO_OUT);
gpio_set_dir(STEP_1, GPIO_OUT);
gpio_set_dir(STEP_2, GPIO_OUT);
gpio_put(DIR_1, 0);
gpio_put(DIR_2, 0);
// variables to control the steppers
float accel_1 = 0.000001; // current acceleration of the stepper
float accel_2 = 0.000001;
float max_accel_1 = 0.000001; // fixed maximum acceleration
float max_accel_2 = 0.000001;
float speed_1 = 0; // current speed of the stepper
float speed_2 = 0;
float pos_1 = 0; // current position of the stepper
float pos_2 = 0;
int step_1 = 0; // current number of steps taken
int step_2 = 0;
// maximum speed of the stepper in steps per loop cycle
float max_speed_1 = 0.2;
float max_speed_2 = 0.2;
float target_speed_1 = max_speed_1;
float target_speed_2 = max_speed_2;
int target_pos_1 = 500000;
int target_pos_2 = 500000;
int distance_left_1 = target_pos_1;
int distance_left_2 = target_pos_2;
// variables to indicate the state of the steppers
bool accelerate_1 = true;
bool accelerate_2 = true;
bool decelerate_1 = false;
bool decelerate_2 = false;
bool steppers_on = true;
// variable that holds the number of steps it took
// to reach the end of the acceleration phase.
// this is done to tell the steppers when to start
// slowing down again. (acceleration phase should be
// equal to the deceleration phase)
uint acc_phase_1 = 0;
uint acc_phase_2 = 0;
// variables that are 0 if a stepper should not take a step
// and 1 if a stepper should take a step.
uint signal_1;
uint signal_2;
// take the current time and schedule the first loop.
long ctime = get_time_us();
long ttime = get_time_us() + TIME_RASTER_US*100;
while(steppers_on) {
ctime = get_time_us(); // take the current time
if(ctime > ttime) { // see if it's time to run the stepper code
// light LED if loop runs behind
if(ctime > ttime + TIME_RASTER_US) {
gpio_put(OVERLOAD_LED, 1);
}
// set timer for next cycle
ttime = ttime + TIME_RASTER_US;
// run stepper code
if (accelerate_1) {
if (speed_1 < target_speed_1) {
speed_1 = speed_1 + accel_1;
}
if (speed_1 > target_speed_1) {
acc_phase_1 = step_1;
speed_1 = target_speed_1;
accel_1 = 0;
accelerate_1 = false;
}
}
if (accelerate_2) {
if (speed_2 < target_speed_2) {
speed_2 = speed_2 + accel_2;
}
if (speed_2 > target_speed_2) {
acc_phase_2 = step_2;
speed_2 = target_speed_2;
accel_2 = 0;
accelerate_2 = false;
}
}
if (decelerate_1) {
speed_1 = speed_1 - accel_1;
if (speed_1 < target_speed_1) {
speed_1 = target_speed_1;
accel_1 = 0;
decelerate_1 = false;
}
}
if (decelerate_2) {
speed_2 = speed_2 - accel_2;
if (speed_2 < target_speed_2) {
speed_2 = target_speed_2;
accel_2 = 0;
decelerate_2 = false;
}
}
pos_2 = pos_2 + speed_2;
pos_1 = pos_1 + speed_1;
if (pos_1 > step_1) {
step_1 += 1; // take a step
// todo: replace this with PIO state machine call
signal_1 = 1;
// correct the remaining distance to the target dist
distance_left_1 -= 1;
if (distance_left_1 < step_1) {
accelerate_1 = false;
target_speed_1 = 0;
}
if (distance_left_1 < acc_phase_1) {
decelerate_1 = true;
accel_1 = max_accel_1;
}
}
if (pos_2 > step_2) {
step_2 += 1; // take a step
// todo: replace this with PIO state machine call
signal_2 = 1;
// correct the remaining distance to the target dist
distance_left_2 -= 1;
if (distance_left_2 < step_2) {
accelerate_2 = false;
target_speed_2 = 0;
}
if (distance_left_2 < acc_phase_2) {
decelerate_2 = true;
accel_2 = max_accel_2;
}
}
gpio_put(STEP_1, signal_1);
gpio_put(STEP_2, signal_2);
signal_1 = 0;
signal_2 = 0;
sleep_us(5);
gpio_put(STEP_1, 0);
gpio_put(STEP_2, 0);
}
}
return 0;
}
Here the source file:
#include <stdio.h>
#include <sys/time.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "hardware/gpio.h"
// Returns the current time in microseconds.
long get_time_us(){
struct timeval currentTime;
gettimeofday(¤tTime, NULL);
return currentTime.tv_sec * (int)1e6 + currentTime.tv_usec;
}
As you can see, there is a sleep_us(5) step in the main loop. This is done to produce a signal sufficiently stron for the stepper motor controller to register. Shorter pulses are ignored by the stepper controller.
As it stands now, the code seems to run slower than I expected and lags behind a few microseconds each time it runs through the stepper loop.
My idea was to "outsource" the GPIO operations to a PIO. That way I also get rid of the sleep_us(5) step in the main code, saving significant execution time.
