prettyprint

2023年8月24日 星期四

PWM Ep.2: Raspberry Pi Pico Remotely Controls Servo Motors Using ADC and nRF24L01+

本篇文章說明以PWM控制Servo Motor。以Raspberry Pi Pico整合ADC, nRF24L01+遠端遙控ESP32-CAM鏡頭拍攝的角度。

一、Servo motor and PWM:

上圖說明Servo的PWM Period為20ms(50Hz)，Pico-SDK以下列指令來設定：

    gpio_set_function(pin, GPIO_FUNC_PWM);
    slice = pwm_gpio_to_slice_num(pin);
    pwm_ch = pwm_gpio_to_channel(pin);
    pwm_config c = pwm_get_default_config();
    pwm_config_set_clkdiv(&c, 125);  // 20ms period, steps 20000, clkdiv = 125 
    pwm_config_set_wrap(&c, 20000);
    pwm_config_set_phase_correct(&c, false);
    pwm_init(*slice, &c, true);  

系統clock為125M Hz
125M Hz/125/20000 = 50Hz。

以下列指令設定PWM duty Cycle:

pwm_set_chan_level(slice, pwm_ch, top_count);

top_count:angle*1000/180+15000, 0度時1.5ms duty cycle。-90~90為1ms~2ms。

實際使用時可能因為所使用元件誤差，需要調整，在本篇文章的實驗時-90~90為0.5ms~2.5ms。

二、ADC:

Raspberry Pi Pico 共有4個ADC ports，如上圖所示。解析度為12 bit因此取樣值為0~4095。下列指令

adc_init();
adc_gpio_init(ADC_PIN); // init and assign ADC pin

adc_select_input(ADC_NUM);  // gpio 26: adc_num 0, gpio 27:1,  gpio 28 : 2
adv_value = adc_read();

三、nRF24L01+:

驅動程式請參閱[Raspberry Pi Pico] nRF24L01+ Ep. 1 : Pico-SDK C-code driver using IRQ，相關網路拓撲請參閱[Raspberry Pi Pico] nRF24L01+ Ep. 2 : Various types of network topologies 

  • 發送端設定：

nRF24_spi_default_init(20, 21, nRF24_irq_callback);
nRF24_config_mode(TRANSMITTER);
nRF24_enable_feature(FEATURE_EN_DPL, true);
nRF24_set_TX_addr(nRF24_addr0, 5);
nRF24_set_RX_addr(0, nRF24_addr0, 5);
nRF24_enable_data_pipe_dynamic_payload_length(0, true);

  • 接收端設定：

nRF24_spi_default_init(20, 21, nRF24_irq_callback);
nRF24_config_mode(RECEIVER);
nRF24_enable_feature(FEATURE_EN_DPL, true);
nRF24_set_RX_addr(0, nRF24_addr0, 5);
nRF24_enable_data_pipe_dynamic_payload_length(0, true);

  • nRF24_irq_callback:

void nRF24_irq_callback(uint8_t event_type, uint8_t datapipe, uint8_t* data, uint8_t width) {
    static uint8_t XY_AXIS;
    static uint8_t ang[4];
    uint8_t data_buffer[33];
    uint8_t data_len; 
      switch(event_type) {
        case EVENT_RX_DR:
#ifdef TRANS_SIDE
#else   
            data_len = width;
            memcpy(data_buffer, data, width);
            deencrypt_data(data_buffer, &data_len);
            XY_AXIS=data_buffer[0];
            memset(ang,0,sizeof(ang));
            memcpy(ang, data_buffer+2, width-2);
            if (XY_AXIS == 'X') {
                setServoAngle(atoi(ang), pwm_ch_x, pwm_slice_x);
            }
            if (XY_AXIS == 'Y') {
                setServoAngle(atoi(ang), pwm_ch_y, pwm_slice_y);
            }
#endif
                
        break;
        case EVENT_TX_DS:
        break;
        case EVENT_MAX_RT:
        break;
    }
}

四、成果影片：



五、程式碼：
main.c
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
#include "nRF24L01.h"
#include "string.h"
 
#define PWM_X_PIN   14
#define PWM_Y_PIN   15
uint pwm_ch_x, pwm_slice_x;
uint pwm_ch_y, pwm_slice_y;
int32_t servo_x_ang, servo_y_ang;
#define ADC_X_PIN   26
#define ADC_X_NUM   0
#define ADC_Y_PIN   27
#define ADC_Y_NUM   1
#define ADC_RANGE (1<<12)    // pi pico 12 bits ADC
int32_t adc_x_center, adc_y_center;
 
#define TRANS_SIDE
 
uint8_t nRF24_addr0[] = "0node";
 
void encrypt_data(uint8_t *data, uint8_t *len) {
    // do data encryption
    return;
}
 
void deencrypt_data(uint8_t *data, uint8_t *len) {
    // do data deencryption
    return;
}
void setServoAngle(int ang, uint pwm_ch, uint slice) {
    //uint16_t top_count = (uint16_t)(1000/180*ang + 1500);  // angle 0: 1.5 ms duty
    uint16_t top_count = (uint16_t)(ang/0.09 + 1500);  // angle 0: 1.5 ms duty
    pwm_set_chan_level(slice, pwm_ch, top_count);
}
 
void servo_init(uint pin, uint *pwm_ch, uint* slice) {
    // set Servo pwm
    gpio_set_function(pin, GPIO_FUNC_PWM);
    *slice = pwm_gpio_to_slice_num(pin);
    *pwm_ch = pwm_gpio_to_channel(pin);
    pwm_config c = pwm_get_default_config();
    pwm_config_set_clkdiv(&c, 125);  // 20ms period, steps 20000, clkdiv = 125 
    pwm_config_set_wrap(&c, 20000);
    pwm_config_set_phase_correct(&c, false);
    pwm_init(*slice, &c, true);    
}
 
void nRF24_irq_callback(uint8_t event_type, uint8_t datapipe, uint8_t* data, uint8_t width) {
    static uint8_t XY_AXIS;
    static uint8_t ang[4];
    uint8_t data_buffer[33];
    uint8_t data_len; 
      switch(event_type) {
        case EVENT_RX_DR:
#ifdef TRANS_SIDE
#else   
            data_len = width;
            memcpy(data_buffer, data, width);
            deencrypt_data(data_buffer, &data_len);
            XY_AXIS=data_buffer[0];
            memset(ang,0,sizeof(ang));
            memcpy(ang, data_buffer+2, width-2);
            if (XY_AXIS == 'X') {
                setServoAngle(atoi(ang), pwm_ch_x, pwm_slice_x);
            }
            if (XY_AXIS == 'Y') {
                setServoAngle(atoi(ang), pwm_ch_y, pwm_slice_y);
            }
#endif
        break;
        case EVENT_TX_DS:
 
 
        break;
        case EVENT_MAX_RT:
 
        break;
    }
}
 
int main()
{
    uint8_t payload[33];
    uint8_t payload_len;
    stdio_init_all();
    nRF24_spi_default_init(20, 21, nRF24_irq_callback);
 
#ifdef TRANS_SIDE
    nRF24_config_mode(TRANSMITTER);
    nRF24_enable_feature(FEATURE_EN_DPL, true);
    nRF24_set_TX_addr(nRF24_addr0, 5);
    nRF24_set_RX_addr(0, nRF24_addr0, 5);
    nRF24_enable_data_pipe_dynamic_payload_length(0, true);
 
    adc_init();                     // init and assign ADC pin
    adc_gpio_init(ADC_X_PIN);
    adc_gpio_init(ADC_Y_PIN);
    for (int i=0; i < 100; i++) {     // average 100 samples as mouse static position
        adc_select_input(ADC_X_NUM);
        adc_x_center += adc_read();
        adc_select_input(ADC_Y_NUM);
        adc_y_center += adc_read();
    }
    adc_x_center /=100;
    adc_y_center /=100;
#else 
    nRF24_config_mode(RECEIVER);
    nRF24_enable_feature(FEATURE_EN_DPL, true);
    nRF24_set_RX_addr(0, nRF24_addr0, 5);
    nRF24_enable_data_pipe_dynamic_payload_length(0, true);
 
    servo_y_ang = 0;
    servo_x_ang = 0;
    servo_init(PWM_X_PIN, &pwm_ch_x, &pwm_slice_x);
    servo_init(PWM_Y_PIN, &pwm_ch_y, &pwm_slice_y);
 
    setServoAngle(servo_x_ang, pwm_ch_x, pwm_slice_x);
    setServoAngle(servo_y_ang, pwm_ch_y, pwm_slice_y);
#endif 
    int32_t adc_raw;
    while(1) {
#ifdef TRANS_SIDE
        adc_select_input(ADC_X_NUM);
        adc_raw = adc_read();
        if(abs(adc_raw-adc_x_center) > 60) {
            //servo_x_ang=(adc_raw-adc_x_center)*90/adc_x_center;
            if (adc_raw < adc_x_center) {
                servo_x_ang++;
                if (servo_x_ang > 90) servo_x_ang = 90;
            }
            else { 
                servo_x_ang--;
                if (servo_x_ang < -90) servo_x_ang = -90;
           }
            //setServoAngle(servo_x_ang, pwm_ch_x, pwm_slice_x);
            sprintf(payload,"X:%d", servo_x_ang);
            payload_len = strlen(payload); 
            encrypt_data(payload, &payload_len);
            nRF24_write_payload(payload, payload_len);
            printf("payload:%s\n", payload);
        }
 
        adc_select_input(ADC_Y_NUM);
        adc_raw = adc_read();
        if(abs(adc_raw-adc_y_center) > 60) {
            if (adc_raw < adc_y_center) {
                servo_y_ang++;
                if (servo_y_ang > 90) servo_y_ang = 90;
            }
            else { 
                servo_y_ang--;
                if (servo_y_ang < -90) servo_y_ang = -90;
            }
            //setServoAngle(servo_y_ang, pwm_ch_y, pwm_slice_y);
            sprintf(payload,"Y:%d", servo_y_ang);
            payload_len = strlen(payload); 
            encrypt_data(payload, &payload_len);
            nRF24_write_payload(payload, payload_len);
            printf("payload:%s\n", payload);
        }
        //printf("payload:%s\n", payload);
#endif
       sleep_ms(20);
       
    }
 
    return 0;
}

張貼者： 於

沒有留言:

張貼留言

訂閱： 張貼留言 (Atom)