RP2040 MicroROS

[tianrking]

ADC

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/adc.h"

// 定义ADC引脚
#define ADC_PIN_1 26
#define ADC_PIN_2 27
#define ADC_PIN_3 28
#define ADC_PIN_4 29

// ADC检测周期（毫秒）
#define ADC_READ_INTERVAL 50

void init_adc(uint gpio) {
    adc_gpio_init(gpio);
    adc_select_input(gpio - 26);
}

uint16_t read_adc(uint gpio) {
    adc_select_input(gpio - 26);
    return adc_read();
}

int main() {
    stdio_init_all();

    // 初始化ADC
    adc_init();

    // 设置ADC时钟
    adc_set_clkdiv(24000000 / 1000000);

    // 初始化ADC引脚
    init_adc(ADC_PIN_1);
    init_adc(ADC_PIN_2);
    init_adc(ADC_PIN_3);
    init_adc(ADC_PIN_4);

    uint16_t adc_value[4];

    while (1) {
        // 读取ADC值
        adc_value[0] = read_adc(ADC_PIN_1);
        adc_value[1] = read_adc(ADC_PIN_2);
        adc_value[2] = read_adc(ADC_PIN_3);
        adc_value[3] = read_adc(ADC_PIN_4);

        // 打印ADC值
        printf("ADC Values: %d, %d, %d, %d\n", adc_value[0], adc_value[1], adc_value[2], adc_value[3]);

        // 等待下一次读取
        sleep_ms(ADC_READ_INTERVAL);
    }

    return 0;
}

[tianrking]

Encoder

// https://www.waveshare.net/w/upload/e/ed/RPI-PICO-R3-PUBLIC-SCHEMATIC.pdf
// https://www.waveshare.net/w/upload/5/52/Pico-R3-A4-Pinout.pdf

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/irq.h"

// 定义编码器引脚
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3

// 旋转编码器每一圈的计数
#define COUNTS_PER_REV 500

// 速度计算周期（毫秒）
#define SPEED_CALC_INTERVAL 100

volatile int32_t current_count = 0;

void encoder_interrupt_handler(uint gpio, uint32_t events) {
    static uint8_t encoder_state = 0;

    // 获取编码器A和B引脚的状态
    encoder_state = (encoder_state << 2) | ((gpio_get(ENCODER_B_PIN) << 1) | gpio_get(ENCODER_A_PIN));
    encoder_state &= 0x0F;

    // 根据状态确定方向并增加或减少计数值
    if (encoder_state == 0x01 || encoder_state == 0x0E) {
        current_count++;
    } else if (encoder_state == 0x04 || encoder_state == 0x0B) {
        current_count--;
    }
}

// 速度计算定时器回调
void speed_calculation_callback() {
    static int32_t last_count = 0;

    int32_t count_diff = current_count - last_count;
    last_count = current_count;

    float speed_rpm = (float)count_diff / COUNTS_PER_REV * 60000.0f / SPEED_CALC_INTERVAL;

    printf("Speed: %.2f RPM\n", speed_rpm);
}

int main() {
    stdio_init_all();

    // 初始化编码器输入引脚
    gpio_init(ENCODER_A_PIN);
    gpio_init(ENCODER_B_PIN);
    gpio_set_dir(ENCODER_A_PIN, GPIO_IN);
    gpio_set_dir(ENCODER_B_PIN, GPIO_IN);

    // 设置编码器引脚中断处理程序
    gpio_set_irq_enabled_with_callback(ENCODER_A_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &encoder_interrupt_handler);
    gpio_set_irq_enabled_with_callback(ENCODER_B_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, &encoder_interrupt_handler);

    // 添加定时器回调以计算速度
    add_repeating_timer_ms(SPEED_CALC_INTERVAL, speed_calculation_callback, NULL, NULL);

    while (1) {
        sleep_ms(10);
    }

    return 0;
}

[tianrking]

#include "pico/stdlib.h"
#include "hardware/pwm.h"

// Declare global slice numbers for the PWM
uint motor1_slice, motor2_slice, servo_slice;

void setup() {
    // Set the GPIOs to be used as PWM
    gpio_set_function(20, GPIO_FUNC_PWM);
    gpio_set_function(21, GPIO_FUNC_PWM);
    gpio_set_function(19, GPIO_FUNC_PWM);

    // Get the PWM slice numbers for the GPIOs
    motor1_slice = pwm_gpio_to_slice_num(20);
    motor2_slice = pwm_gpio_to_slice_num(21);
    servo_slice = pwm_gpio_to_slice_num(19);

    // Set PWM wrap (resolution)
    pwm_set_wrap(motor1_slice, 0xFFFF); // Set resolution for motor 1
    pwm_set_wrap(motor2_slice, 0xFFFF); // Set resolution for motor 2
    pwm_set_wrap(servo_slice, 0xFFFF); // Set resolution for servo

    // Set PWM frequency
    pwm_set_clkdiv(motor1_slice, 4.0f); // Set frequency for motor 1
    pwm_set_clkdiv(motor2_slice, 4.0f); // Set frequency for motor 2
    pwm_set_clkdiv(servo_slice, 20.0f); // Set frequency for servo to 50Hz

    // Enable the PWMs
    pwm_set_enabled(motor1_slice, true);
    pwm_set_enabled(motor2_slice, true);
    pwm_set_enabled(servo_slice, true);
}

void setMotorSpeed(int motor, float speed) {
    // Speed should be between 0.0 (stop) and 1.0 (full speed)
    uint slice = (motor == 1 ? motor1_slice : motor2_slice);
    uint16_t level = (uint16_t)(speed * (float)0xFFFF);
    pwm_set_gpio_level((motor == 1 ? 20 : 21), level);
}

void setServoAngle(float angle) {
    // Angle should be between 0.0 (0 degrees) and 1.0 (180 degrees)
    uint16_t level = (uint16_t)((angle * 0.5f + 0.5f) * (float)0xFFFF);
    pwm_set_gpio_level(19, level);
}

int main() {
    setup();

    while (true) {
        // Control motors and servo here
        // setMotorSpeed(1, 0.5); // Set motor 1 at half speed
        // setMotorSpeed(2, 0.5); // Set motor 2 at half speed
        // setServoAngle(0.5); // Set servo at 90 degrees (assuming 0.5 corresponds to 90 degrees)

        sleep_ms(1000);
    }
}