stepper_experiments/stepper_example.c at main · infnorm/stepper_experiments · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
/**
 * Stepper Motor Control Example using PIO
 *
 * This example demonstrates how to use the PIO state machine to control
 * a stepper motor with adjustable frequency and step counting.
 */

#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "stepgen.h"
//#include "stepper.pio.h"
#include <stdio.h>

// Pin definitions
// #define STEP_PIN 2       // GPIO pin for STEP signal
// #define DIR_PIN 3        // GPIO pin for DIR signal (controlled by PIO side-set)
// #define STEP2_PIN 4       // GPIO pin for STEP signal
// #define DIR2_PIN 5        // GPIO pin for DIR signal (controlled by PIO side-set)
// #define STEP3_PIN 6       // GPIO pin for STEP signal
// #define DIR3_PIN 7        // GPIO pin for DIR signal (controlled by PIO side-set)
// #define STEP4_PIN 8       // GPIO pin for STEP signal
// #define DIR4_PIN 9        // GPIO pin for DIR signal (controlled by PIO side-set)

// PIO instance

int main() {
    timer_hw->dbgpause = 0;


    // Setup serial port
    setup_default_uart(); // uart0, 9600);
    uart_init(uart0, 115200);

    stepgen_init();


    printf("Stepper motor controller started!\n");

    uint32_t loop_count = 0;

    //stepper_set_params(pio0, sm2, 100, 1);
    int freqs[] = {500,10000,100000,1000000};
    int directions[] = {1,1,1,1};
    int delay = 0;
    stepgen_set_frequency(0, freqs[0], directions[1]);
    // stepper_set_params(pio0, sm, freqs[0], directions[0]);
    // stepper_set_params(pio0, sm2, freqs[1], directions[1]);
    // stepper_set_params(pio0, sm3, freqs[2], directions[2]);
    // stepper_set_params(pio0, sm4, freqs[3], directions[3]);

    int32_t last_counts[4] = {0};
    uint64_t last_timestamp_us = 0;

    while (true) {
        int c = getchar_timeout_us(100);
        //int c = PICO_ERROR_TIMEOUT;
        if(c != PICO_ERROR_TIMEOUT) {
            if(c=='0') {
                freqs[0] = 0;
                printf("Stopping stepping.\n");
            }
            else if(c=='+') freqs[0] += 100;
            else if (c=='-') freqs[0] -= 100;
            else if (c=='*') freqs[0] *= 2;
            else if (c=='/') freqs[0] /= 2;
            else if (c=='d') delay += 1;
            else if (c=='D') delay -= 1;
            else if (c=='f') {
                directions[0] = 1;
            }
            else if (c=='r') {
                directions[0] = 0;
            }
            //if(freq < 1) freq = 1;
            stepgen_set_frequency(0, freqs[0], directions[0]);
        }

        if(loop_count % 10 == 0) {
            printf("Trying to read...\n");
            uint64_t current_timestamp_us = time_us_64();
            for(int k=0;k<4;k++) {
                int32_t current_count = stepgen_get_step_count(k);
                int32_t delta = (current_count - last_counts[k]);

                float actual_frequency = 0.0f;
                if (last_timestamp_us != 0 && current_timestamp_us > last_timestamp_us) {
                    uint64_t time_delta_us = current_timestamp_us - last_timestamp_us;
                    if (time_delta_us > 0) {
                        actual_frequency = (float)delta * 1000000.0f / (float)time_delta_us;
                    }
                }

                last_counts[k]  = current_count;

                printf("count: %10d delta: %10d set_freq: %d Hz actual_freq: %.2f Hz state: %s\n",
                    current_count, delta, freqs[k], actual_frequency, stepgen_get_state(k));
            }
            last_timestamp_us = current_timestamp_us;
            printf("Done to read...%d\n", loop_count);

        }
        sleep_ms(100);
        loop_count++;
    }

    return 0;
}