/**
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/uart.h"
#include "hardware/irq.h"
//#define USEN
#define PWM_ON
#define UART_ID uart1
#define BAUD_RATE 9600
#define DATA_BITS 8
#define STOP_BITS 1
#define PARITY UART_PARITY_NONE
#define ON 1
#define OFF 0
#define UART_TX_PIN 8
#define UART_RX_PIN 9
const int ON_BORD_LED = 25;
// Define the Pins
const int L_FIN = 0 ;
const int L_RIN = 2;
const int R_FIN = 4;
const int R_RIN = 6;
//SLIDE_POT = 26
const int FRONT_BUTTON_Pin = 21;
const int LEFT_BUTTON_Pin = 20;
const int RIGHT_BUTTON_Pin = 19;
const int BACK_BUTTON_Pin = 18;
const int DIV_CLK = 50.0;
const int SYS_CLK = 125000000; //125MHz
const int PWM_FREQ = 50000; //range:20kHz~100kHz
const int PWM_CYCLE = 1/PWM_FREQ*1000000; //us
//グルーバル変数の宣言
static int chars_rxed = 0;
static int data_num=0;
uint8_t sbus_data[1];
uint8_t ch;
static pwm_config pwm_slice_config_l, pwm_slice_config_r;
uint pwm0_slice_num_l, pwm0_slice_num_r;
//uint16_t Chdata[6];
//int duty_l, duty_r;
int duty;
//int duty_rev;
//関数の宣言
uint8_t init_serial(void);
uint8_t init_pwm();
uint8_t init_gpio();
void on_uart_rx();
uint8_t init_serial(void){
/// シリアル通信の設定
// UARTを基本の通信速度で設定
uart_init(UART_ID, 2400);
// 指定のGPIOをUARTのTX、RXピンに設定する
gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
//指定のUARTを指定の通信速度に設定する
int actual = uart_set_baudrate(UART_ID, BAUD_RATE);
//UART flow control CTS/RTSを使用しない設定
uart_set_hw_flow(UART_ID, false, false);
//通信フォーマットの設定
uart_set_format(UART_ID, DATA_BITS, STOP_BITS, PARITY);
// Turn off FIFO's - we want to do this character by character
uart_set_fifo_enabled(UART_ID, false);
// Set up a RX interrupt
// We need to set up the handler first
// Select correct interrupt for the UART we are using
int UART_IRQ = UART_ID == uart0 ? UART0_IRQ : UART1_IRQ;
// And set up and enable the interrupt handlers
irq_set_exclusive_handler(UART_IRQ, on_uart_rx);
irq_set_enabled(UART_IRQ, true);
// Now enable the UART to send interrupts - RX only
uart_set_irq_enables(UART_ID, true, false);
}
uint8_t init_pwm(){
// PWMの設定
// Tell GPIO 0 and 1 they are allocated to the PWM
gpio_set_function( L_FIN, GPIO_FUNC_PWM );
gpio_set_function( R_FIN, GPIO_FUNC_PWM );
// Find out which PWM slice is connected to GPIO 0 (it's slice 0)
pwm0_slice_num_l = pwm_gpio_to_slice_num( L_FIN );
pwm0_slice_num_r = pwm_gpio_to_slice_num( R_FIN );
//PWM wrap計算
uint pwm_wrap;
//pwm_wrap = SYS_CLK/(PWM_FREQ*DIV_CLK)-1;
pwm_wrap = SYS_CLK/(PWM_FREQ*DIV_CLK*2)-1; //位相補正ONのとき、周期が2倍になるため
//PWMコンフィグdefault値取得
pwm_slice_config_l = pwm_get_default_config();
pwm_slice_config_r = pwm_get_default_config();
//PWM周波数設定
pwm_config_set_clkdiv( &pwm_slice_config_l,DIV_CLK );
pwm_config_set_clkdiv( &pwm_slice_config_r,DIV_CLK );
pwm_config_set_wrap( &pwm_slice_config_l,pwm_wrap );
pwm_config_set_wrap( &pwm_slice_config_r,pwm_wrap );
//PWM位相補正ON
pwm_config_set_phase_correct (&pwm_slice_config_l,ON);
pwm_config_set_phase_correct (&pwm_slice_config_r,ON);
//PWMコンフィグ値反映
pwm_init( pwm0_slice_num_l, &pwm_slice_config_l, false );
pwm_init( pwm0_slice_num_r, &pwm_slice_config_r, false );
//デューティー比設定
//duty = 50;
duty = 100; //PWM出力初期値'H'
//pwm_set_gpio_level( L_FIN, ( pwm_slice_config_l.top * duty/100 ) );
//pwm_set_gpio_level( R_FIN, ( pwm_slice_config_r.top * duty/100 ) );
pwm_set_chan_level( pwm0_slice_num_l, PWM_CHAN_A, ( pwm_slice_config_l.top * duty/100 ));//*0.5 );//位相補正ONのとき、周期が2倍になるため
pwm_set_chan_level( pwm0_slice_num_r, PWM_CHAN_A, ( pwm_slice_config_r.top * duty/100 ));//*0.5 );//位相補正ONのとき、周期が2倍になるため
pwm_set_enabled (pwm0_slice_num_l, 1);
pwm_set_enabled (pwm0_slice_num_r, 1);
sleep_us(10);
pwm_set_enabled (pwm0_slice_num_l, 0);
pwm_set_enabled (pwm0_slice_num_r, 0);
// // Set channel A output high for one cycle before dropping
// pwm_set_chan_level(slice_num, PWM_CHAN_A, 2315);
// // Set initial B output high for three cycles before dropping
// pwm_set_chan_level(slice_num, PWM_CHAN_B, 1330);
// // Set the PWM running
// pwm_set_enabled(slice_num, true);
// /// \end::setup_pwm[]
sleep_ms(200);
// pwm_set_chan_level(slice_num, PWM_CHAN_A, 1330);
}
uint8_t init_gpio(void){
//ボタン入力GPIOを入力に設定しpullup
gpio_init( FRONT_BUTTON_Pin );
gpio_init( LEFT_BUTTON_Pin );
gpio_init( RIGHT_BUTTON_Pin );
gpio_init( BACK_BUTTON_Pin );
gpio_set_dir( FRONT_BUTTON_Pin, GPIO_IN );
gpio_set_dir( LEFT_BUTTON_Pin, GPIO_IN );
gpio_set_dir( RIGHT_BUTTON_Pin, GPIO_IN );
gpio_set_dir( BACK_BUTTON_Pin, GPIO_IN );
gpio_pull_up( FRONT_BUTTON_Pin );
gpio_pull_up( LEFT_BUTTON_Pin );
gpio_pull_up( RIGHT_BUTTON_Pin );
gpio_pull_up( BACK_BUTTON_Pin );
//PWM出力GPIO設定
//初期化
// gpio_init(L_FIN);
gpio_init(L_RIN);
// gpio_init(R_FIN);
gpio_init(R_RIN);
//gpio set dir
gpio_set_dir(L_RIN, GPIO_OUT);
gpio_set_dir(R_RIN, GPIO_OUT);
gpio_put(L_RIN,1);
gpio_put(R_RIN,1);
return 0;
}
// RX interrupt handler
void on_uart_rx() {
short data;
while (uart_is_readable(UART_ID)) {
ch = uart_getc(UART_ID);
//sbus_data[chars_rxed]=ch;
//Chdata[0] = sbus_data[chars_rxed];
}
}
int main(){
stdio_init_all();
printf("config start\n");
//LED GPIO設定
gpio_init(ON_BORD_LED);
gpio_set_dir(ON_BORD_LED,GPIO_OUT);
gpio_put(ON_BORD_LED, 1);
sleep_ms(750);
init_gpio();
init_serial();
init_pwm();
gpio_put( L_RIN, 1);
gpio_put( L_RIN, 1);
//PWM ENABLE
uint pwm_mask_enabled=0x0;
pwm_mask_enabled = ( 0x01 << pwm0_slice_num_l ) | ( 0x01 << pwm0_slice_num_r );
pwm_set_mask_enabled( pwm_mask_enabled );
uint8_t dir = 0;
uint8_t dir0 = 0;
uint8_t pwm = 1;
ch = 0x10;
gpio_put(ON_BORD_LED, 0);
sleep_ms(250);
duty = 50;
//duty_rev = 50;
uint8_t duty_l, duty_r;
uint8_t l_rin_out, r_rin_out;
uint8_t dbg_pin0=1;
uint8_t dbg_pin1=1;
while(1){
tight_loop_contents();
dir0 = ch & 0xF;
pwm = (ch & 0x30)>>4;
// printf("uart in=%04x, dir(uart[3:0])=%04x, pwm(uart[5:4])=%04x\n",ch,dir0,pwm);
#ifdef USEN
dir = ((!gpio_get(BACK_BUTTON_Pin))<<3 )|((!gpio_get(RIGHT_BUTTON_Pin))<<2)|((!gpio_get(LEFT_BUTTON_Pin))<<1)|(!gpio_get(FRONT_BUTTON_Pin));
#else
dir = ch & 0xF;
pwm = (ch & 0x30)>>4;
#ifdef PWM_ON
switch(pwm){
case 0: duty = 25; break;
case 1: duty = 40; break;
case 2: duty = 50; break;
case 3: duty = 75; break;
}
//duty_rev = 100 - duty;
#endif
#endif
//printf("uart in=%04x, dir(uart[3:0])=%04x, pwm(uart[5:4])=%04x, duty = %d\n",ch,dir0,pwm,duty);
switch(dir){
// case 0:
case 1:
//printf("front, ");
duty_l = duty;
duty_r = duty;
l_rin_out = 0;
r_rin_out = 0;
break;
case 2:
//printf("left, ");
duty_l = 100; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
duty_r = duty;
l_rin_out = 1; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
r_rin_out = 0;
break;
case 4:
//printf("rigth, ");
duty_l = duty;
duty_r = 100; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
l_rin_out = 0;
r_rin_out = 1; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
break;
case 8:
//printf("back, ");
duty_l = 100 - duty;
duty_r = 100 - duty;
l_rin_out = 1;
r_rin_out = 1;
break;
case 3:
//printf("left front, ");
duty_l = duty*0.5;
duty_r = duty;
l_rin_out = 0;
r_rin_out = 0;
break;
case 5:
//printf("right front, ");
duty_l = duty;
duty_r = duty*0.5;
l_rin_out = 0;
r_rin_out = 0;
break;
case 10:
//printf("left back, ");
duty_l = 100 - duty*0.5;
duty_r = 100 - duty;
l_rin_out = 1;
r_rin_out = 1;
break;
case 12:
//printf("right back, ");
duty_l = 100 - duty;
duty_r = 100 - duty*0.5;
l_rin_out = 1;
r_rin_out = 1;
break;
case 6:
//printf("right yaw, ");
duty = 25;
duty_l = duty;
duty_r = 100 - duty;
l_rin_out = 0;
r_rin_out = 1;
break;
case 9:
//printf("left yaw, ");
duty = 25;
duty_l = 100 - duty;
duty_r = duty;
l_rin_out = 1;
r_rin_out = 0;
break;
default:
//printf("stop, ");
duty_l = 100; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
duty_r = 100; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
l_rin_out = 1; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
r_rin_out = 1; //0; 停止時のモータードライバのmode設定をd(ブレーキ:FIN=H,RIN=Hに変更)
break;
}
pwm_set_chan_level( pwm0_slice_num_l, PWM_CHAN_A, ( pwm_slice_config_l.top * duty_l/100 ) );
pwm_set_chan_level( pwm0_slice_num_r, PWM_CHAN_A, ( pwm_slice_config_r.top * duty_r/100 ) );
dbg_pin0 = gpio_get(L_FIN);
dbg_pin1 = gpio_get(R_FIN);
//printf("duty_r=%d, duty_l=%d, pwm_mask_enable=%x, l_rin_out=%d, r_rin_out=%d, dbg_pin_LFIN=%d, dbg_pin_RFIN=%d\n",duty_r,duty_l,pwm_mask_enabled,l_rin_out,r_rin_out,dbg_pin0,dbg_pin1);
printf("dir=%04x, pwm=%04x, duty_r=%d, duty_l=%d, pwm_mask_enable=%x, l_rin_out=%d, r_rin_out=%d, dbg_pin_LFIN=%d, dbg_pin_RFIN=%d\n",dir,pwm,duty_r,duty_l,pwm_mask_enabled,l_rin_out,r_rin_out,dbg_pin0,dbg_pin1);
//sleep_ms(1); //PWM設定の反映待ち(主に停止動作のため)
sleep_us(PWM_CYCLE*2); //PWM2周期分wait
//pwm_set_mask_enabled( pwm_mask_enabled ); //PWM disable時動作が安定しないため、FINの'H''L'はduty=100 or 0で実現する=PWMは常にON
gpio_put( L_RIN, l_rin_out);
gpio_put( R_RIN, r_rin_out);
//sleep_us(1);
sleep_ms(5); //uartの通信間隔と調整が必要?
}
return 0;
}
//void set_moter(uint pwm_slice_num, uint PWM_CHAN, pwm_config *pwm_slice_config, uint duty, int RIN, uint8_t rin_out){
// pwm_set_chan_level( pwm_slice_num, PWM_CHAN, ( pwm_slice_config.top * duty/100 ) );
// gpio_put( RIN, rin_out);
//}