Řízení krokového motoru

/**
 * 
 * vha3@cornell.edu
 * September, 2021
 * 
 * This demonstrates position and velocity control
 * of two ULN2003 steppers. If each of those steppers
 * is attached to the knob of an etch-a-sketch with a timing
 * belt, this demo draws the Lorenz curve on the etch-a-sketch.
 * 
 * https://vanhunteradams.com/Pico/Steppers/Lorenz.html
 * 
 * 
*/

#include "motor_library.h"
#include <math.h>

#define MOTOR1_IN1 2

#define convert(a) (unsigned int)(125000000./(Fs*(float)a))

#define Fs 50.0

// Variables to hold motor speed
volatile unsigned int motorspeed = 250000;

// Variables to track motor direction
volatile int direction = 1 ;
volatile int old_direction = 1 ;

// Speed values
volatile float speed ;

// Motor directions
unsigned int motor1_direction ;



// Position control interrupts
void pio0_interrupt_handler() {
    pio_interrupt_clear(pio_0, 0);
    switch( direction ) {
	case CLOCKWISE: 		direction = STOPPED; old_direction = CLOCKWISE; break;
	case STOPPED:   		direction = (old_direction==CLOCKWISE) ? COUNTERCLOCKWISE : CLOCKWISE; break;
	case COUNTERCLOCKWISE:		direction = STOPPED; old_direction = COUNTERCLOCKWISE; break;
    }

    SET_DIRECTION_MOTOR_1(direction);
    if( direction == STOPPED ) {
	MOVE_STEPS_MOTOR_1(2000);
	}
    else {
	MOVE_STEPS_MOTOR_1(4000);
	}
}

int main() {
    stdio_init_all();

    // Nastaveni motoru
    setupMotor1(MOTOR1_IN1, pio0_interrupt_handler);
    SET_DIRECTION_MOTOR_1(STOPPED);
    old_direction = COUNTERCLOCKWISE;
    SET_SPEED_MOTOR_1(250000);
    MOVE_STEPS_MOTOR_1(400);

    
    while (true) {

    }
}

/**
 * V. Hunter Adams
 * vha3@cornell.edu
 * September, 2021
 */

#include <stdio.h>

#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/dma.h"
#include "hardware/irq.h"
#include "stepper.pio.h"
#include "pacer.pio.h"
#include "counter.pio.h"

// Makra pro směr otáčení
#define COUNTERCLOCKWISE 2
#define CLOCKWISE 1
#define STOPPED 0

// Posloupnosti pulsů dopředu, dozadu a stání
unsigned char pulse_sequence_forward[8]    = {0x9, 0x8, 0xc, 0x4, 0x6, 0x2, 0x3, 0x1} ;
unsigned char pulse_sequence_backward[8]   = {0x1, 0x3, 0x2, 0x6, 0x4, 0xc, 0x8, 0x9} ;
unsigned char pulse_sequence_stationary[8] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0} ;

// Unsigned ints to hold pulse length and no. of pulses
unsigned long int pulse_length_motor1 = (1u << 17) - 1 ;
unsigned long int pulse_count_motor1 = 1024 ;

// Pointers to the addresses of the above objects (motor 1)
unsigned char * address_pointer_motor1 = &pulse_sequence_forward[0] ;
unsigned long int * pulse_length_motor1_address_pointer = &pulse_length_motor1 ;
unsigned long int * pulse_count_motor1_address_pointer = &pulse_count_motor1 ;


// Makra pro nastavení motoru: počet kroků, rychlost a směr
#define MOVE_STEPS_MOTOR_1(a) pulse_count_motor1=a; dma_channel_start(dma_chan_4)
#define SET_SPEED_MOTOR_1(a) pulse_length_motor1=a
#define SET_DIRECTION_MOTOR_1(a) address_pointer_motor1 = (a==2) ? &pulse_sequence_forward[0] : (a==1) ? &pulse_sequence_backward[0] : &pulse_sequence_stationary[0]


// Výběr PIO koprocesoru
PIO pio_0 = pio0;

// Nastavení stavových strojů na PIO koprocesoru
int pulse_sm = 0;
int pacer_sm = 1;
int count_sm = 2;

// DMA kanály
// 0 posílá posloupnost pulsů do motoru, 1 konfiguruje a restartuje 0
// 2 sends pulse length data to motor 1, 3 reconfigures and restarts 2
// 4 sends step count to motor 1 (channel started in software)
int dma_chan_0 = 0;
int dma_chan_1 = 1;
int dma_chan_2 = 2;
int dma_chan_3 = 3;
int dma_chan_4 = 4;

void setupMotor1(unsigned int in1, irq_handler_t handler) {
    // Načteme PIO program do PIO0
    uint pio0_offset_0 = pio_add_program(pio_0, &stepper_program);
    uint pio0_offset_1 = pio_add_program(pio_0, &pacer_program);
    uint pio0_offset_2 = pio_add_program(pio_0, &counter_program);

    // Inicializujeme PIO programy
    stepper_program_init(pio_0, pulse_sm, pio0_offset_0, in1);
    pacer_program_init(pio_0, pacer_sm, pio0_offset_1);
    counter_program_init(pio_0, count_sm, pio0_offset_2) ;

    // Spustíme PIO programy
    pio_sm_set_enabled(pio_0, pulse_sm, true);
    pio_sm_set_enabled(pio_0, pacer_sm, true);
    pio_sm_set_enabled(pio_0, count_sm, true);

    // Nastavení přerušení
    pio_interrupt_clear(pio_0, 0) ;
    pio_set_irq0_source_enabled(pio_0, PIO_INTR_SM0_LSB, true) ;
    irq_set_exclusive_handler(PIO0_IRQ_0, handler) ;
    irq_set_enabled(PIO0_IRQ_0, true) ;

    ////////////////////////////////////////////////////////////////////////////////////
    // ===========================-== DMA datové kanály ================================
    ////////////////////////////////////////////////////////////////////////////////////

    // Channel Zero (sends pulse train data to PIO stepper machine)
    dma_channel_config c0 = dma_channel_get_default_config(dma_chan_0); // default configs
    channel_config_set_transfer_data_size(&c0, DMA_SIZE_8);             // 32-bit txfers
    channel_config_set_read_increment(&c0, true);                       // no read incrementing
    channel_config_set_write_increment(&c0, false);                     // no write incrementing
    channel_config_set_dreq(&c0, DREQ_PIO0_TX0) ;                       // DREQ_PIO0_TX0 pacing (FIFO)
    channel_config_set_chain_to(&c0, dma_chan_1);                       // chain to other channel

    dma_channel_configure(
        dma_chan_0,                 // Channel to be configured
        &c0,                        // The configuration we just created
        &pio_0->txf[pulse_sm],      // write address (stepper PIO TX FIFO)
        address_pointer_motor1,
        8,                          // Number of transfers; in this case each is 4 byte.
        false                       // Don't start immediately.
    );

    // Channel One (reconfigures the first channel)
    dma_channel_config c1 = dma_channel_get_default_config(dma_chan_1);   // default configs
    channel_config_set_transfer_data_size(&c1, DMA_SIZE_32);              // 32-bit txfers
    channel_config_set_read_increment(&c1, false);                        // no read incrementing
    channel_config_set_write_increment(&c1, false);                       // no write incrementing
    channel_config_set_chain_to(&c1, dma_chan_0);                         // chain to other channel

    dma_channel_configure(
        dma_chan_1,                         // Channel to be configured
        &c1,                                // The configuration we just created
        &dma_hw->ch[dma_chan_0].read_addr,  // Write address (channel 0 read address)
        &address_pointer_motor1,                   // Read address (POINTER TO AN ADDRESS)
        1,                                  // Number of transfers, in this case each is 4 byte
        false                               // Don't start immediately.
    );

    // ------

    // Channel 2 (sends pulse length data to PIO pacer machine)
    dma_channel_config c2 = dma_channel_get_default_config(dma_chan_2);  // default configs
    channel_config_set_transfer_data_size(&c2, DMA_SIZE_32);              // 32-bit txfers
    channel_config_set_read_increment(&c2, false);                        // no read incrementing
    channel_config_set_write_increment(&c2, false);                      // no write incrementing
    channel_config_set_dreq(&c2, DREQ_PIO0_TX1) ;                        // DREQ_PIO0_TX1 pacing (FIFO)
    channel_config_set_chain_to(&c2, dma_chan_3);                        // chain to other channel

    dma_channel_configure(
        dma_chan_2,                 // Channel to be configured
        &c2,                        // The configuration we just created
        &pio_0->txf[pacer_sm],        // write address (pacer PIO TX FIFO)
        pulse_length_motor1_address_pointer,
        1,                          // Number of transfers; in this case each is 4 byte.
        false                       // Don't start immediately.
    );

    // Channel 3 (reconfigures the second channel)
    dma_channel_config c3 = dma_channel_get_default_config(dma_chan_3);   // default configs
    channel_config_set_transfer_data_size(&c3, DMA_SIZE_32);              // 32-bit txfers
    channel_config_set_read_increment(&c3, false);                        // no read incrementing
    channel_config_set_write_increment(&c3, false);                       // no write incrementing
    channel_config_set_chain_to(&c3, dma_chan_2);                         // chain to other channel

    dma_channel_configure(
        dma_chan_3,                         // Channel to be configured
        &c3,                                // The configuration we just created
        &dma_hw->ch[dma_chan_2].read_addr,  // Write address (channel 2 read address)
        &pulse_length_motor1_address_pointer,      // Read address (POINTER TO AN ADDRESS)
        1,                                  // Number of transfers, in this case each is 4 byte
        false                               // Don't start immediately.
    );

    // -------

    // Channel 4 (sends pulse count to PIO counter machine)
    dma_channel_config c4 = dma_channel_get_default_config(dma_chan_4);  // default configs
    channel_config_set_transfer_data_size(&c4, DMA_SIZE_32);              // 32-bit txfers
    channel_config_set_read_increment(&c4, false);                        // no read incrementing
    channel_config_set_write_increment(&c4, false);                      // no write incrementing

    dma_channel_configure(
        dma_chan_4,                 // Channel to be configured
        &c4,                        // The configuration we just created
        &pio_0->txf[count_sm],        // write address (pacer PIO TX FIFO)
        pulse_count_motor1_address_pointer,
        1,                          // Number of transfers; in this case each is 4 byte.
        false                       // Don't start immediately.
    );

    // Start the two data channels
    dma_start_channel_mask((1u << dma_chan_0) | (1u << dma_chan_2)) ;
}

.program stepper

wait 1 irq 3            ; Wait for signal to put pulse on pins
out pins, 4             ; Put a pulse on the pins, AUTOPULL ENGAGED

% c-sdk {
static inline void stepper_program_init(PIO pio, uint sm, uint offset, uint pin) {
   
   pio_sm_config c = stepper_program_get_default_config(offset);

   sm_config_set_out_pins(&c, pin, 4);

   // Setup autopull, 32-bit threshold, right-shift OSR
   sm_config_set_out_shift(&c, 1, 1, 4) ;

   pio_gpio_init(pio, pin);
   pio_gpio_init(pio, pin+1);
   pio_gpio_init(pio, pin+2);
   pio_gpio_init(pio, pin+3);

   pio_sm_set_consecutive_pindirs(pio, sm, pin, 4, true);
   
   pio_sm_init(pio, sm, offset, &c);
}
%}

.program pacer

out x, 32                     ; Shift value from OSR to scratch X (AUTOPULL ENGAGED)

countloop:
    jmp x-- countloop        ; Loop until X hits 0

wait 1 irq 2                 ; Wait for signal to pulse from counter state machine
irq 3                        ; Signal to send a pulse



% c-sdk {
static inline void pacer_program_init(PIO pio, uint sm, uint offset) {
   
   pio_sm_config c = pacer_program_get_default_config(offset);
   sm_config_set_out_shift(&c, 1, 1, 32) ;
   pio_sm_init(pio, sm, offset, &c);
}
%}

.program counter

pull block                   ; Copy commanded # of steps from FIFO to OSR
mov x, osr                   ; Copy value from OSR to scratch X

countloop:
   irq wait 2               ; Signal for a step to occur, wait for flag to clear
   jmp x-- countloop        ; Loop until X hits 0

irq wait 0                  ; IRQ to CPU ISR, wait for CPU to clear




% c-sdk {
static inline void counter_program_init(PIO pio, uint sm, uint offset) {
   
   pio_sm_config c = counter_program_get_default_config(offset);
   pio_sm_init(pio, sm, offset, &c);
}
%}

# cmake version
cmake_minimum_required(VERSION 3.13)

# include the sdk.cmake file
include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)

# give the project a name (anything you want)
project(Stepper_Motors_position_and_speed C CXX ASM)

# initialize the sdk
pico_sdk_init()

add_executable(Stepper_Motors_position_and_speed)

pico_generate_pio_header(Stepper_Motors_position_and_speed ${CMAKE_CURRENT_LIST_DIR}/stepper.pio)
pico_generate_pio_header(Stepper_Motors_position_and_speed ${CMAKE_CURRENT_LIST_DIR}/pacer.pio)
pico_generate_pio_header(Stepper_Motors_position_and_speed ${CMAKE_CURRENT_LIST_DIR}/counter.pio)

target_sources(Stepper_Motors_position_and_speed PRIVATE stepper.c)

target_link_libraries(Stepper_Motors_position_and_speed PRIVATE pico_stdlib pico_bootsel_via_double_reset hardware_pio hardware_dma hardware_irq)

pico_add_extra_outputs(Stepper_Motors_position_and_speed)