超音波センサー HC-SR04をI2Cで使えるようになった

先日超音波センサーを買い足した所

裏面にIICという気になるシルク印刷があり、調べた結果

中華パチVerが素敵にグレードアップしていました。

HC-SR04_2020_表

HC-SR04_2020_裏

通信方式が従来GPIO以外に　UART,IICの両方に対応しておりました。

又、オリジナル版と比べた際に、測定対象と直交でない場合の測定結果が

比較的良好な結果になっていました。

測定対象：平面に対し斜め45度で500mmで測定

オリジナル：1000mm±50mm程度　※たまに500近いのが出たりする

中華新Ver：500mm±10程度

<中華新Ver詳細>

I2Cデバイスアドレス：0x57

　(スキャンした所何故か0x58も出てきますが、反応なし)

測定開始：デバイスに1byteデータで1書き込み

測定結果：0xaf(3byte)

※上記書き込み後200ms後に読む必要あり。

※(byte_H*65535+byte_M*256+byte_L)/1000 　単位(mm)

通信仕様切替方法(裏面抵抗実装にて)

GPIO：R4=N.C.　R5=N.C.(出荷初期状態)

UART：R4=N.C.　R5=10kΩ(写真でチップ抵抗103)

I2C：R4＝10kΩ　R5=N.C.

尚、搭載チップはRCWL-9300、RCWL-9600という捺印があるので

調べてみると、やはりI2C対応のモジュールが幾つか出てきます。

問題なのは、何処で買うとこれが買えるのかというロシアンルーレット

ちなみに、今回の購入元はアマゾンでお馴染みのHiLetgo様になりますが、

写真は正規品の写真なので、コレが必ず買える確証は・・・・

デメリット

測定距離に関係なく必ず200ms待つ必要があります。

HiLetgo 5個セット HC-SR04 超音波距離センサーモジュール Arduino UNO Mega2560/Raspberry Pi B+Robot ロボットに交換 [並行輸入品]

猫を自動追尾で追いかけるミニカーを作る＿一回目

最終目標：猫を自動追尾で追いかけるミニカーを作る

現状：基本走行機能が完成。

同様の物を作ろうとしている方に役立てば幸いです。

暫く続く予定です。

＜ひとまず完成した物＞

■マイコンでカスタム制御基板(シリアル通信で制御)

マイコンは値段からラズパイPicoを採用。言語はC

機能１：駆動用DCモーター制御

＞PWM+PID制御による定速指令

＞過負荷検知

＞積算規定回転数で自動停止

機能２：操舵用サーボ制御

機能３：任意前方180度方向で前方障害物との距離測定

機能４：Lipoバッテリー過放電防止等の各種異常監視

<今後の実施予定>

・ラズパイ側の制御コードを作り込みとクラス化

・6軸ジャイロ情報から進行方向の角度制御

・OpenCVで動体検知

・OpenCVで障害物回避

ミニカー

制御基板

制御基板

drive.google.com

smellbomb.hatenablog.com

smellbomb.hatenablog.com

■用意した物

＞改造ベースのラジコン・・・WPLJAPAN [D12]

荷台があるので丁度使いやすいので選択。

WPLJAPAN [D12] WPL 正規品 技適マーク付き 1/10スケール 軽トラ RCカー トラック バッテリー付き(ホワイト)

＞制御基板部品

・ラズパイPico

akizukidenshi.com

・5V_DCDCコンバータ

akizukidenshi.com

・駆動用DCモータドライバユニット（L298_STﾏｲｸﾛ

HiLetgo 2個セット モジュール L298N デュアルHブリッジ DCステップ モーター ドライバー コントローラー ボード Arduinoと互換 [並行輸入品]

・サーボモーターON/OFF用モータードライブIC

akizukidenshi.com

・超音波センサー　HC-SR04

VKLSVAN 2個 HC-SR04 超音波距離センサーモジュール Arduinoに互換

・DCモーター(エンコーダ付属　AB偏倍で52PLS/rot)

12V DCエンコーダギアモーター 金属減速モーター 低騒音ギアモーター 低騒音 安定性能 ブラシDCモータ 30RPM 70RPM 100RPM 500RPM（選択）

*1:DC12V 30RMP

*2:DC12V 30RMP

*3:DC12V 30RMP

モーター制御基板 PC側ソフト 参考

~
import serial
import time

flg_uart_err=0

ser = serial.Serial('/dev/ttyAMA0', '19200', timeout=0.1)

def send_command(send_data=None):
    ser.reset_output_buffer()
    send_data = send_data + '\n'
    send_data_b = send_data.encode('utf-8')
    ser.write(send_data_b)
    return True

def receive_command(chk_command=None, chk_data=None, retry= 10):
    for n in range(retry):
        ser.reset_input_buffer()
        receive_data = ser.readline().decode('utf-8')
        if len(receive_data) > 0:
            if receive_data == "uart,error":
                flg_uart_err = 1
                return None
            else:
                receive_data = receive_data.replace('\n','')
                receive_data_ls = receive_data.split(',')
                if chk_command != None and receive_data_ls[0] != chk_command:
                    return None
                if chk_data != None and receive_data_ls[1] != chk_data:
                    return None
                return receive_data_ls
    return None

def get_dist():
    command = "get_dist,0"
    send_command(command)
    receive_data_ls = receive_command(chk_command='get_dist')
    if receive_data_ls is None:
        return None
    return int(receive_data_ls[1])

def set_dist_angle(angle=0):
    command = "set_dist_angle," + str(angle)
    send_command(command)
    receive_data_ls = receive_command(chk_command='set_dist_angle',chk_data=str(angle))
    if receive_data_ls is None:
        return None
    return True

def serch_dist(split_angle=30):
    dist_ls = []
    mes_angle = -90
    delay = (split_angle / 60) * 0.15
    while mes_angle <= 90:
        set_dist_angle(angle=mes_angle)
        if mes_angle <= -90:
            time.sleep(0.3)
            dist = get_dist()
        else:
            time.sleep(delay)
            dist = get_dist()
        if dist is None:
            return None
        else:
            dist_ls.append(dist)
            mes_angle += split_angle
    set_dist_angle(angle=0)
    time.sleep(0.2)
    set_dist_angle(angle=999)
    max_p = dist_ls.index(max(dist_ls))
    max_angle = int(-90 + max_p * split_angle)
    dist_ls.append(max_p)
    dist_ls.append(max_angle)
    return dist_ls

def drive_motor(rps=0, stop_rot=0, enable_break=False):
    if enable_break == True:
        if set_drive_break(1) == True:
            return True
        else:
            return None
    if enable_break != True:
        if set_drive_break(0) == None:
            return None
    if clr_drive_rot_count() == None:
        return None
    if set_drive_stop_rot(rot=stop_rot) == None:
        return None
    if clr_drive_auto_break() == None:
        return None
    if set_drive_rps(rps=rps) == None:
        return None
    return True

def set_drive_pwm_freq(freq=100):
    if freq <= 10:
        freq = 10
    if freq >= 2500:
        freq = 2500
    set_clkdiv = 2500 / freq
    if set_drive_pwm_warp(warp=49999) == None:
        return None
    if set_drive_pwm_clkdiv(clkdiv=set_clkdiv) == None:
        return None
    return True

def set_drive_break(enable_break=0):
    command = 'enable_break,' + str(enable_break)
    send_command(command)
    receve_data_ls = receive_command(chk_command='enable_break',chk_data=str(enable_break))
    if receve_data_ls != None:
        return True
    return None

def set_drive_rps(rps=0):
    command = 'set_rps,' + str(rps)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_rps',chk_data=str(rps))
    if receve_data_ls != None:
        return True
    return None

def set_drive_stop_rot(rot=0):
    command = 'set_stop_rot,' + str(rot)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_stop_rot',chk_data=str(rot))
    if receve_data_ls != None:
        return True
    return None

def set_drive_pwm_clkdiv(clkdiv=25):
    command = 'set_drive_pwm_clkdiv,' + str(clkdiv)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_drive_pwm_clkdiv',chk_data=str(clkdiv))
    if receve_data_ls != None:
        return True
    return None

def set_drive_pwm_warp(warp=49999):
    command = 'set_drive_pwm_warp,' + str(warp)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_drive_pwm_warp',chk_data=str(warp))
    if receve_data_ls != None:
        return True
    return None

def chk_auto_stop_drive():
    send_command('get_auto_break_flg,0')
    receve_data_ls = receive_command(chk_command='get_auto_break_flg',chk_data='1')
    if receve_data_ls != None:
        return True
    return None

def clr_drive_rot_count():
    send_command('clr_count_rot,0')
    receve_data_ls = receive_command(chk_command='clr_count_rot',chk_data='0')
    if receve_data_ls != None:
        return True
    return None

def get_drive_rps():
    send_command('get_rps,0')
    receve_data_ls = receive_command(chk_command='get_rps')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def get_drive_rps_ave():
    send_command('get_rps_ave,0')
    receve_data_ls = receive_command(chk_command='get_rps_ave')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def get_drive_duty():
    send_command('get_drive_duty,0')
    receve_data_ls = receive_command(chk_command='get_drive_duty')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def get_rot_count():
    send_command('get_count_rot,0')
    receve_data_ls = receive_command(chk_command='get_count_rot')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def clr_drive_auto_break():
    send_command('clr_auto_break_flg,0')
    receve_data_ls = receive_command(chk_command='clr_auto_break_flg',chk_data='0')
    if receve_data_ls != None:
        return True
    return None

def chk_trq_err():
    send_command('get_trq_err,0')
    receve_data_ls = receive_command(chk_command='get_trq_err',chk_data='1')
    if receve_data_ls != None:
        return True
    return None

def clr_trq_err():
    send_command('clr_trq_err,0')
    receve_data_ls = receive_command(chk_command='clr_trq_err',chk_data='0')
    if receve_data_ls != None:
        return True
    return None

def set_steering(angle=0):
    command = 'set_steering,' + str(angle)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_steering',chk_data=str(angle))
    if receve_data_ls != None:
        return True
    return None

def chk_ready():
    send_command('get_total_fault,0')
    receve_data_ls = receive_command(chk_command='get_total_fault',chk_data=str(0))
    if receve_data_ls != None:
        return True
    else:
        return None

def get_temp():
    send_command('get_temp,0')
    receve_data_ls = receive_command(chk_command='get_temp')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_temp_max():
    send_command('get_temp_max,0')
    receve_data_ls = receive_command(chk_command='get_temp_max')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_temp_min():
    send_command('get_temp_min,0')
    receve_data_ls = receive_command(chk_command='get_temp_min')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vsys():
    send_command('get_Vsys,0')
    receve_data_ls = receive_command(chk_command='get_Vsys')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vsys_max():
    send_command('get_Vsys_max,0')
    receve_data_ls = receive_command(chk_command='get_Vsys_max')
    if receve_data_ls != None:
        return float(receve_data_ls[1])


def get_Vsys_min():
    send_command('get_Vsys_min,0')
    receve_data_ls = receive_command(chk_command='get_Vsys_min')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vbat():
    send_command('get_Vbat,0')
    receve_data_ls = receive_command(chk_command='get_Vbat')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vbat_max():
    send_command('get_Vbat_max,0')
    receve_data_ls = receive_command(chk_command='get_Vbat_max')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vbat_min():
    send_command('get_Vbat_min,0')
    receve_data_ls = receive_command(chk_command='get_Vbat_min')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_pid_kp():
    send_command('get_pid_kp,0')
    receve_data_ls = receive_command(chk_command='get_pid_kp')
    if receve_data_ls != None:
        return receve_data_ls[1]

def get_pid_ki():
    send_command('get_pid_ki,0')
    receve_data_ls = receive_command(chk_command='get_pid_ki')
    if receve_data_ls != None:
        return receve_data_ls[1]

def get_pid_kd():
    send_command('get_pid_kd,0')
    receve_data_ls = receive_command(chk_command='get_pid_kd')
    if receve_data_ls != None:
        return receve_data_ls[1]

def set_pid_kp(kp=2):
    kp = '{:.6f}'.format(kp)
    command = 'set_pid_kp,' + kp
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_pid_kp',chk_data=kp)
    if receve_data_ls != None:
        return True

def set_pid_ki(ki=0.1):
    ki = '{:.6f}'.format(ki)
    command = 'set_pid_ki,' + ki
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_pid_ki',chk_data=ki)
    if receve_data_ls != None:
        return True

def set_pid_kd(kd=0.1):
    kd = '{:.6f}'.format(kd)
    command = 'set_pid_kd,' + kd
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_pid_kd',chk_data=kd)
    if receve_data_ls != None:
        return True

def chk_condition():
    print(chk_ready())
    print(get_temp())
    print(get_temp_max())
    print(get_temp_min())
    print(get_Vbat())
    print(get_Vbat_max())
    print(get_Vbat_min())
    print(get_Vsys())
    print(get_Vsys_max())
    print(get_Vsys_min())
    return True




ser.close()

~

ラズパイPico モーター制御基板ソースコード

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
//#include <cstdint>
#include <cstdio>
#include <cstring>
//#include <memory.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "hardware/pwm.h"
#include "pico/multicore.h"
#include "hardware/uart.h"
#include "hardware/irq.h"
#include "hardware/adc.h"

#define GPIO_PICO_LED       25
#define UART_TX_PIN         0
#define UART_RX_PIN         1
#define GPIO_ENC_A          2
#define GPIO_ENC_B          3
#define GPIO_DRIVE_PWM      4
#define GPIO_DRIVE_CW       5
#define GPIO_DRIVE_CCW      6
#define GPIO_SERVO1_PWM     7
#define GPIO_SERVO1_ENABLE  10
#define GPIO_SERVO2_PWM     11
#define GPIO_SERVO2_ENABLE  12
#define GPIO_DIST1_TRG      20
#define GPIO_DIST1_ECH      21
#define GPIO_ADC_VSYS       29
#define GPIO_ADC_BATT       26
#define GPIO_ADC_SERVO2     27


#define IRQ_enca_enable     gpio_set_irq_enabled(GPIO_ENC_A, GPIO_IRQ_EDGE_RISE, true);
#define IRQ_enca_disable    gpio_set_irq_enabled(GPIO_ENC_A, GPIO_IRQ_EDGE_RISE, false);

#define UART_ID uart0
#define BAUD_RATE 19200
#define DATA_BITS 8
#define STOP_BITS 1
#define PARITY    UART_PARITY_NONE

#define UART_SEND_ERR_MESSAGE uart_puts(UART_ID,"uart,error\n");
#define ERROR 1
#define NON_ERROR 0
#define SEND_TOTAL_FAULT "total_fault"

int g_set_rps = 0, g_enable_break=0,g_auto_break_flg=0;
long long int g_enca_val = 0, g_encb_val = 0, g_enc_count_total = 0, g_enc_count_sub = 0,g_stop_rot = 0,g_count_rot = 0;
float g_kp=3, g_ki=0.1, g_kd=0.005, g_drive_rps = 0, g_pid_duty = 0 , g_drive_rps_ave = 0;;
int g_drive_pwm_clkdiv = 25, g_drive_pwm_warp = 49999;
const int g_servo1_duty100 = 49999;
const int g_servo2_duty100 = 49999;
int g_trq_error_flg = 0, g_trq_error_chk = 1;
int chars_rxed = 0;
int uart_flg = 0;
char uart_rx_data[64];
const float adc_conv_fact = 3.3f / (1 << 12);
uint pico_led_freq = 100;
float g_Vbat,g_Vbat_max,g_Vbat_min,g_Vsys,g_Vsys_max,g_Vsys_min,g_temp,g_temp_max,g_temp_min;
int g_total_fault = 0;



void enc_callback(uint gpio, uint32_t events) {
    g_encb_val = gpio_get(GPIO_ENC_B);
    g_enca_val = gpio_get(GPIO_ENC_A);
    if (gpio == GPIO_ENC_A && events == GPIO_IRQ_EDGE_RISE){
        if(g_encb_val == 0){
            g_enc_count_total++;
            g_enc_count_sub++;
        }else{
            g_enc_count_total--;
            g_enc_count_sub--;
        }
    }else if (gpio == GPIO_ENC_A && events == GPIO_IRQ_EDGE_FALL){
        if(g_encb_val == 1){
            g_enc_count_total++;
            g_enc_count_sub++;
        }else{
            g_enc_count_total--;
            g_enc_count_sub--;
        }
    }else if (gpio == GPIO_ENC_B && events == GPIO_IRQ_EDGE_RISE){
        if(g_enca_val == 1){
            g_enc_count_total++;
            g_enc_count_sub++;
        }else{
            g_enc_count_total--;
            g_enc_count_sub--;
        }
    }else if (gpio == GPIO_ENC_B && events == GPIO_IRQ_EDGE_FALL){
        if(g_enca_val == 0){
            g_enc_count_total++;
            g_enc_count_sub++;
        }else{
            g_enc_count_total--;
            g_enc_count_sub--;
        }
    }
}

bool drive_motor(){
    static uint32_t stop_st = 0;
    uint32_t stop_t = 0;
    if (g_total_fault == 1){
        gpio_put(GPIO_DRIVE_CW, 0);
        gpio_put(GPIO_DRIVE_CCW, 0);
        pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
        g_set_rps = 0;
        return true;
    }
    if (g_trq_error_flg == 1){
        gpio_put(GPIO_DRIVE_CW, 0);
        gpio_put(GPIO_DRIVE_CCW, 0);
        pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
        g_set_rps = 0;
        return true;
    }
    if(g_enable_break == 1 ){
        pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp);
        gpio_put(GPIO_DRIVE_CW, 1);
        gpio_put(GPIO_DRIVE_CCW, 1);
        g_set_rps = 0;
        return true;
    }
    if(g_auto_break_flg == 1 ){
        pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp);
        gpio_put(GPIO_DRIVE_CW, 1);
        gpio_put(GPIO_DRIVE_CCW, 1);
        g_set_rps = 0;
        return true;
    }
    else if (g_set_rps==0){
        pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
        gpio_put(GPIO_DRIVE_CW, 0);
        gpio_put(GPIO_DRIVE_CCW, 0);
        return true;
    }
    else if (((g_stop_rot > 0) && (g_count_rot > g_stop_rot))||((g_stop_rot < 0) && (g_count_rot < g_stop_rot))){
        g_auto_break_flg = 1;
        gpio_put(GPIO_DRIVE_CW, 1);
        gpio_put(GPIO_DRIVE_CCW, 1);
        pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp);
        g_set_rps = 0;
        return true;
    }
    if (g_pid_duty > 0){
        gpio_put(GPIO_DRIVE_CW, 1);
        gpio_put(GPIO_DRIVE_CCW, 0);
    }
    else if (g_pid_duty < 0){
        gpio_put(GPIO_DRIVE_CW, 0);
        gpio_put(GPIO_DRIVE_CCW, 1);
    }
    pwm_set_gpio_level(GPIO_DRIVE_PWM, g_drive_pwm_warp*(abs(g_pid_duty)/100.f));
    
    if (abs(g_pid_duty) >= 99.f && abs(g_drive_rps) < 0.5f && g_trq_error_chk == 1){
        if (stop_st == 0){
            stop_st = to_ms_since_boot(get_absolute_time());
            return true;
        }else{
            stop_t = to_ms_since_boot(get_absolute_time()) - stop_st;
            if (stop_t > 2000){
                g_trq_error_flg =1 ;
                g_set_rps = 0;
                return true;
            }
        return true;
        }
    }
    stop_st = 0;
    return true;
}


bool repeating_timer_callback(struct repeating_timer *t) {
    const int rot_pls = 52;
    static uint32_t pretime = 0;
    static float rps_arr[50]={};
    static int ave_count=0;
    float rps_sum = 0;
    g_drive_rps = ((float)g_enc_count_sub / (float)rot_pls)*100;
    g_count_rot = (float)g_enc_count_total / (float)rot_pls;
    g_enc_count_sub = 0;
    if (ave_count == 50){
        ave_count = 0;
    }
    rps_arr[ave_count] = g_drive_rps;
    ave_count++;
    for (int i = 0; i < 50; i++){
        rps_sum += rps_arr[i];
    }
    g_drive_rps_ave = rps_sum / 50;
    //printf("rps_ave:%3.1f___rps:%3.1f\n",g_drive_rps_ave,g_drive_rps);
    


    

    static float P,I,D,dt,preP=0,duty,bef_set_rps=0;
    dt = to_ms_since_boot(get_absolute_time()) - pretime;
    pretime = to_ms_since_boot(get_absolute_time());
    preP = P;
    
    P = g_drive_rps - (float)g_set_rps;
    I += (P + preP) / 2.0 * dt;
    if (g_set_rps == 0){
        I = 0;
    }
    //If the number of revolutions is changed, reset it.
    if (g_set_rps != bef_set_rps){
        I = 0;
    }
    bef_set_rps = g_set_rps;
    

    D = (P - preP) / dt;
    duty = (g_kp * P + g_kd * D + g_ki * I)* 1;
    if (duty>=100){
        duty = 100.0;
    }
    else if (duty<=-100.0){
        duty = -100.0;
    }
    g_pid_duty = duty;
    drive_motor();
    return true;
}

void setup_enc(void){
    gpio_init(GPIO_ENC_A);
    gpio_init(GPIO_ENC_B);
    gpio_set_dir(GPIO_ENC_A, GPIO_IN);
    gpio_set_dir(GPIO_ENC_B, GPIO_IN);
    gpio_pull_down(GPIO_ENC_A);
    gpio_pull_down(GPIO_ENC_B);
}

void setup_drive_motor(void){
    gpio_set_function(GPIO_DRIVE_PWM, GPIO_FUNC_PWM);
    uint slice_num_drive = pwm_gpio_to_slice_num(GPIO_DRIVE_PWM);
    pwm_set_clkdiv(slice_num_drive, g_drive_pwm_clkdiv);
    pwm_set_wrap(slice_num_drive, g_drive_pwm_warp);
    pwm_set_gpio_level(GPIO_DRIVE_PWM, 0);
    //pwm_set_gpio_level(GPIO_SERVO1_PWM, 0);
    //pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
    pwm_set_enabled(slice_num_drive, true);
    gpio_init(GPIO_DRIVE_CW);
    gpio_init(GPIO_DRIVE_CCW);
    gpio_set_dir(GPIO_DRIVE_PWM, GPIO_OUT);
    gpio_set_dir(GPIO_DRIVE_CW, GPIO_OUT);
    gpio_set_dir(GPIO_DRIVE_CCW, GPIO_OUT);
    gpio_put(GPIO_DRIVE_CW, 0);
    gpio_put(GPIO_DRIVE_CCW, 0);
}





void setup_servo_motor(void){
    gpio_set_function(GPIO_SERVO1_PWM, GPIO_FUNC_PWM);
    gpio_set_function(GPIO_SERVO2_PWM, GPIO_FUNC_PWM);
    uint slice_num_servo1 = pwm_gpio_to_slice_num(GPIO_SERVO1_PWM);
    uint slice_num_servo2 = pwm_gpio_to_slice_num(GPIO_SERVO2_PWM);
    pwm_set_clkdiv(slice_num_servo1, 50);
    pwm_set_clkdiv(slice_num_servo2, 50);
    pwm_set_wrap(slice_num_servo1, g_servo1_duty100);
    pwm_set_wrap(slice_num_servo2, g_servo2_duty100);
    pwm_set_enabled(slice_num_servo1, true);
    pwm_set_enabled(slice_num_servo2, true);
    gpio_init(GPIO_SERVO1_ENABLE);
    gpio_init(GPIO_SERVO2_ENABLE);
    gpio_set_dir(GPIO_SERVO1_PWM, GPIO_OUT);
    gpio_set_dir(GPIO_SERVO1_ENABLE, GPIO_OUT);
    gpio_set_dir(GPIO_SERVO2_PWM, GPIO_OUT);
    gpio_set_dir(GPIO_SERVO2_ENABLE, GPIO_OUT);
    gpio_put(GPIO_SERVO1_ENABLE, 0);
    gpio_put(GPIO_SERVO2_ENABLE, 0);
    pwm_set_gpio_level(GPIO_SERVO1_PWM, 0);
    pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
}

void setup_dist_wave(void){
    gpio_init(GPIO_DIST1_ECH);
    gpio_init(GPIO_DIST1_TRG);
    gpio_set_dir(GPIO_DIST1_TRG, GPIO_OUT);
    gpio_set_dir(GPIO_DIST1_ECH, GPIO_IN);
    gpio_put(GPIO_DIST1_TRG, 0);
}

void on_uart_rx(void){    
    uart_set_irq_enables(UART_ID, false, false);
    while (uart_is_readable(UART_ID)) {
        char ch = uart_getc(UART_ID);
        uart_rx_data[chars_rxed] = ch;
        if (uart_is_writable(UART_ID)) {
            ch++;
        }
    }
    if (uart_rx_data[chars_rxed] == '\n'){
        //printf("%s\n",uart_rx_data);
        uart_flg = 1;
    }else{
        chars_rxed++;
    }
    uart_set_irq_enables(UART_ID, true, false);
}

void setup_uart(void){
    uart_init(UART_ID, BAUD_RATE);
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
    int actual = uart_set_baudrate(UART_ID, BAUD_RATE);
    uart_set_hw_flow(UART_ID, false, false);
    uart_set_format(UART_ID, DATA_BITS, STOP_BITS, PARITY);
    uart_set_fifo_enabled(UART_ID, false);
    int UART_IRQ = UART_ID == uart0 ? UART0_IRQ : UART1_IRQ;
    irq_set_exclusive_handler(UART_IRQ, on_uart_rx);
    irq_set_enabled(UART_IRQ, true);
    uart_set_irq_enables(UART_ID, true, false);
}

void setup_adc(void){
    adc_init();
    adc_set_temp_sensor_enabled(true);
    adc_gpio_init(GPIO_ADC_VSYS);
    adc_gpio_init(GPIO_ADC_BATT);
    adc_gpio_init(GPIO_ADC_SERVO2);
}

float get_temp(){
    adc_select_input(4);
    uint16_t adc_val = adc_read();
    float volt = adc_val * adc_conv_fact;
    float temp = 27 - (volt - 0.706) / 0.001721;
    return temp;
}

float get_vsys(){
    adc_select_input(3);
    uint16_t adc_val = adc_read();
    float volt = adc_val * adc_conv_fact * 3;
    return volt;
}

float get_batt(){
    adc_select_input(0);
    uint16_t adc_val = adc_read();
    float volt = adc_val * adc_conv_fact * 10.8650693;
    return volt;
}

int get_srv2_angle(){
    adc_select_input(1);
    const float angle_r90 = 0.183243;
    const float angle_l90 = 3.019942;
    const float angle_0 =   1.603495;
    const float angle_1 = (angle_l90 - angle_r90)/ 180.f;
    uint16_t adc_val = adc_read();
    float volt = adc_val * adc_conv_fact;
    int angle = (angle_0 - volt) / angle_1;
    return angle;
}



int steering(int angle=0){
    const float center = 3775;
    const float max_val = 700; 
    if (angle == 999){
        gpio_put(GPIO_SERVO1_ENABLE, 0);
        pwm_set_gpio_level(GPIO_SERVO1_PWM, 0);
        return NON_ERROR;
    }
    if (angle > 100)
    {
        angle = 100;
    }else if (angle < -100){
        angle = -100;
    }
    int set_pwm = center - max_val * (float(angle) / 100.f);
    gpio_put(GPIO_SERVO1_ENABLE, 1);
    pwm_set_gpio_level(GPIO_SERVO1_PWM, set_pwm);
    return NON_ERROR;
}

/*
int move_sensor(int angle=0){
    const float center = 3624;
    const float max_val = 2100; 
    uint32_t pretime,dt;
    if (angle == 999){
        gpio_put(GPIO_SERVO2_ENABLE, 0);
        pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
        return NON_ERROR;
    }
    if (angle > 90)
    {
        angle = 90;
    }else if (angle < -90){
        angle = -90;
    }
    int set_pwm = center - max_val * (float(angle) / 90.f);
    gpio_put(GPIO_SERVO2_ENABLE, 1);
    pwm_set_gpio_level(GPIO_SERVO2_PWM, set_pwm);
    
    int chk = 0;
    while (true){
        int chk_angle = get_srv2_angle();
        if (angle-1 <= chk_angle && chk_angle <= angle+1){
            chk++;
            sleep_us(500);
            if (chk >= 5){
                break;
            }   
        }else{
            chk = 0;
        }
    }
    gpio_put(GPIO_SERVO2_ENABLE, 0);
    pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
    return NON_ERROR;   
}
*/



int set_distance_angle(int angle=0){
    const float center = 3624;
    const float max_val = 2100; 
    uint32_t pretime,dt;
    if (angle == 999){
        gpio_put(GPIO_SERVO2_ENABLE, 0);
        pwm_set_gpio_level(GPIO_SERVO2_PWM, 0);
        return NON_ERROR;
    }
    if (angle > 90)
    {
        angle = 90;
    }else if (angle < -90){
        angle = -90;
    }
    int set_pwm = center - max_val * (float(angle) / 90.f);
    gpio_put(GPIO_SERVO2_ENABLE, 1);
    pwm_set_gpio_level(GPIO_SERVO2_PWM, set_pwm);
    return NON_ERROR;   
}

int mes_distance(){
    uint64_t trig_on_t,trig_off_t,echo_off_t,echo_on_t;
    gpio_put(GPIO_DIST1_TRG, 1);
    trig_on_t = to_us_since_boot(get_absolute_time());
    trig_off_t = trig_on_t;
    while (trig_off_t - trig_on_t< 20){
        trig_off_t = to_us_since_boot(get_absolute_time());
    }
    gpio_put(GPIO_DIST1_TRG, 0);
    while (gpio_get(GPIO_DIST1_ECH) == 0){
        echo_off_t = to_us_since_boot(get_absolute_time());
        if (echo_off_t - trig_off_t > 30000){
            return -1;
        }
    }
    while (gpio_get(GPIO_DIST1_ECH) == 1){
        echo_on_t = to_us_since_boot(get_absolute_time());
        if (echo_on_t - echo_off_t >= 30000){        
            return -999;
        }
    }
    int dist = (float(echo_on_t - echo_off_t)* (0.3315 + 0.0006 * g_temp)) / 2.f;
    return dist;
}

/*
int chk_distance(int angle = 0){
    move_sensor(angle);
    int dist = mes_distance();
    return dist;
}
*/

/*
int uart_process(char uart_rxdata_arr[],char cmd_ar[],char cmd_data_ar[]);
int motor_cnt(char cmd_ar[],char cmd_data_ar[]);
*/
int command_processing(char cmd_ar[],char cmd_data_ar[]){
    char send_data[32]={};
    if (strcmp(cmd_ar,"get_total_fault") == 0 ){
        snprintf(send_data, sizeof(send_data), "%d", g_total_fault);
    }
    else if (strcmp(cmd_ar,"get_rps") == 0 ){
        snprintf(send_data, sizeof(send_data), "%f", g_drive_rps);
    }
    else if (strcmp(cmd_ar,"get_rps_ave") == 0 ){
        snprintf(send_data, sizeof(send_data), "%f", g_drive_rps_ave);
    }
    else if (strcmp(cmd_ar,"get_count_rot") == 0 ){
        snprintf(send_data, sizeof(send_data), "%lld", g_count_rot);
    }
    else if (strcmp(cmd_ar,"clr_count_rot") == 0 ){
        g_enc_count_total = 0;
        g_count_rot = 0;
        snprintf(send_data, sizeof(send_data), "%lld", g_count_rot);
    }
    else if (strcmp(cmd_ar,"set_rps") == 0 ){
        g_set_rps = atoi(cmd_data_ar);
        snprintf(send_data, sizeof(send_data), "%d", g_set_rps);
    }
    else if (strcmp(cmd_ar,"set_stop_rot") == 0 ){
        g_stop_rot = atoi(cmd_data_ar);
        snprintf(send_data, sizeof(send_data), "%lld", g_stop_rot);
    }
    else if (strcmp(cmd_ar,"get_drive_duty") == 0){
        snprintf(send_data, sizeof(send_data), "%3.1f", g_pid_duty);
    }
    else if (strcmp(cmd_ar,"set_pid_kp") == 0){
        g_kp = atof(cmd_data_ar);
        snprintf(send_data, sizeof(send_data), "%f", g_kp);
    }
    else if (strcmp(cmd_ar,"set_pid_ki") == 0){
        g_ki = atof(cmd_data_ar);
        snprintf(send_data, sizeof(send_data), "%f", g_ki);
    }
    else if (strcmp(cmd_ar,"set_pid_kd") == 0){
        g_kd = atof(cmd_data_ar);
        snprintf(send_data, sizeof(send_data), "%f", g_kd);
    }
    else if (strcmp(cmd_ar,"get_pid_kp") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_kp);
    }
    else if (strcmp(cmd_ar,"get_pid_ki") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_ki);
    }
    else if (strcmp(cmd_ar,"get_pid_kd") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_kd);
    }
    else if (strcmp(cmd_ar,"set_trq_err") == 0){
        if (atoi(cmd_data_ar) == 0){
            g_trq_error_chk = 0;
            g_trq_error_flg = 0;
        }else{
            g_trq_error_chk = 1;
            g_trq_error_flg = 0;
        }
        snprintf(send_data, sizeof(send_data), "%d", g_trq_error_chk);
    }
    else if (strcmp(cmd_ar,"clr_trq_err") == 0){
        g_trq_error_flg = 0;
        snprintf(send_data, sizeof(send_data), "%d", g_trq_error_flg);
    }
    else if (strcmp(cmd_ar,"get_trq_err") == 0){
    snprintf(send_data, sizeof(send_data), "%d", g_trq_error_flg);
    }
    else if (strcmp(cmd_ar,"set_dist_angle") == 0){
        int angle = atoi(cmd_data_ar);
        set_distance_angle(angle);
        snprintf(send_data, sizeof(send_data), "%d", angle);
    }
    else if (strcmp(cmd_ar,"get_dist_angle") == 0){
        int angle = get_srv2_angle();
        snprintf(send_data, sizeof(send_data), "%d", angle);
    }
    else if (strcmp(cmd_ar,"get_dist") == 0){
        //int chk_angle = atoi(cmd_data_ar);
        int dist = mes_distance();
        snprintf(send_data, sizeof(send_data), "%d", dist);
    }
    else if (strcmp(cmd_ar,"set_steering") == 0){
        int angle = atoi(cmd_data_ar);
        steering(angle);
        snprintf(send_data, sizeof(send_data), "%d", angle);
    }
    else if (strcmp(cmd_ar,"get_auto_break_flg") == 0){
        snprintf(send_data, sizeof(send_data), "%d", g_auto_break_flg);
    }
    else if (strcmp(cmd_ar,"clr_auto_break_flg") == 0){
        g_auto_break_flg = 0;
        snprintf(send_data, sizeof(send_data), "%d", g_auto_break_flg);
    }
    else if (strcmp(cmd_ar,"get_temp") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_temp);
    }
    else if (strcmp(cmd_ar,"get_temp_max") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_temp_max);
    }
    else if (strcmp(cmd_ar,"get_temp_min") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_temp_min);
    }
    else if (strcmp(cmd_ar,"get_Vsys") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_Vsys);
    }
    else if (strcmp(cmd_ar,"get_Vsys_max") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_Vsys_max);
    }
    else if (strcmp(cmd_ar,"get_Vsys_min") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_Vsys_min);
    }
    else if (strcmp(cmd_ar,"get_Vbat") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_Vbat);
    }
    else if (strcmp(cmd_ar,"get_Vbat_max") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_Vbat_max);
    }
    else if (strcmp(cmd_ar,"get_Vbat_min") == 0){
        snprintf(send_data, sizeof(send_data), "%f", g_Vbat_min);
    }
    else if (strcmp(cmd_ar,"set_drive_pwm_clkdiv") == 0){

        g_drive_pwm_clkdiv = atoi(cmd_data_ar);
        uint slice_num_drive = pwm_gpio_to_slice_num(GPIO_DRIVE_PWM);
        pwm_set_clkdiv(slice_num_drive, g_drive_pwm_clkdiv);
        snprintf(send_data, sizeof(send_data), "%d", g_drive_pwm_clkdiv);
    }
    else if (strcmp(cmd_ar,"set_drive_pwm_warp") == 0){
        g_drive_pwm_warp = atoi(cmd_data_ar);
        snprintf(send_data, sizeof(send_data), "%d", g_drive_pwm_warp);
    }
    else if (strcmp(cmd_ar,"enable_break") == 0 ){
        if (atoi(cmd_data_ar) == 1){
            g_enable_break = 1;
        }else{
            g_enable_break = 0;
        }
        snprintf(send_data, sizeof(send_data), "%d", g_enable_break);
    }
    else{
        return ERROR;
    }
    uart_puts(UART_ID,cmd_ar);
    uart_puts(UART_ID,",");
    uart_puts(UART_ID,send_data);
    uart_puts(UART_ID,"\n");
    return NON_ERROR;
}

int uart_process(char uart_rxdata_arr[], char cmd_ar[], char cmd_data_ar[]){
    int i = 0;
    while (uart_rxdata_arr[i] != ','){
        cmd_ar[i] = uart_rxdata_arr[i];
        i++;
        if (i > 20){
            chars_rxed = 0;
            return ERROR;
        }
    }
    cmd_ar[i] = '\0';
    i++;
    int command_len = i;
    while (uart_rxdata_arr[i] != '\n'){
        cmd_data_ar[i-command_len] = uart_rxdata_arr[i];
        i++;
        if (i > 40){
            chars_rxed = 0;
            return ERROR;
        }
    }
    cmd_data_ar[i-command_len] = '\0';
    chars_rxed = 0;
    return NON_ERROR;
}

void master_off(){
    g_drive_rps = 0;
    setup_drive_motor();
    setup_servo_motor();
}

int chk_condition(void){
    g_Vbat = get_batt();
    g_Vsys = get_vsys();
    g_temp = get_temp();
    if (g_Vbat_max < g_Vbat){
        g_Vbat_max = g_Vbat;
    }
    if (g_Vbat_min > g_Vbat){
        g_Vbat_min = g_Vbat;
    }
    if (g_Vsys_max < g_Vsys){
        g_Vsys_max = g_Vsys;
    }
    if (g_Vsys_min > g_Vsys){
        g_Vsys_min = g_Vsys;
    }
    if (g_temp_max < g_temp){
        g_temp_max = g_temp;
    }
    if (g_temp_min > g_temp){
        g_temp_min = g_temp;
    }
    if(g_Vbat_max >= 14){
        return ERROR;
    }
    if (g_Vbat_min < 7){
        return ERROR;
    }
    if (g_Vsys_max > 5.5){
        return ERROR;
    }
    if (g_Vsys_min < 4.5){
        return ERROR;
    }
    if (g_temp_max > 50){
        return ERROR;
    }
    if (g_temp_min < 0){
        return ERROR;
    }
    return NON_ERROR;
}

void core1_main(void){
    setup_enc();
    setup_drive_motor();
    gpio_set_irq_enabled_with_callback(GPIO_ENC_A, GPIO_IRQ_EDGE_RISE, true, enc_callback);
    gpio_set_irq_enabled_with_callback(GPIO_ENC_A, GPIO_IRQ_EDGE_FALL, true, enc_callback);
    gpio_set_irq_enabled_with_callback(GPIO_ENC_B, GPIO_IRQ_EDGE_RISE, true, enc_callback);
    gpio_set_irq_enabled_with_callback(GPIO_ENC_B, GPIO_IRQ_EDGE_FALL, true, enc_callback);
    static repeating_timer timer;
    add_repeating_timer_ms(-10, repeating_timer_callback, NULL, &timer);
}

int main(void) {
    stdio_init_all();
    multicore_launch_core1(core1_main);
    setup_servo_motor();
    setup_dist_wave();
    setup_adc();
    setup_uart();
    gpio_init(GPIO_PICO_LED);
    gpio_set_dir(GPIO_PICO_LED, GPIO_OUT);
    gpio_put(GPIO_PICO_LED, 0);
    char cmd_ar[20] = {};
    char cmd_data_ar[20] = {};
    uint led_flash = 500;
    uint32_t led_on_time=0,led_off_time=to_ms_since_boot(get_absolute_time());
    sleep_ms(1000);
    g_Vbat_max = get_batt();
    g_Vbat_min = get_batt();
    g_Vsys_max = get_vsys();
    g_Vsys_min = get_vsys();
    g_temp_max = get_temp();
    g_temp_min = get_temp();
    

    while (true) {
        if (gpio_get(GPIO_PICO_LED) == 0 && to_ms_since_boot(get_absolute_time()) - led_off_time >= led_flash){
            led_on_time = to_ms_since_boot(get_absolute_time());
            gpio_put(GPIO_PICO_LED,1);
        }
        if (gpio_get(GPIO_PICO_LED) == 1 && to_ms_since_boot(get_absolute_time()) - led_on_time >= led_flash){
            gpio_put(GPIO_PICO_LED,0);
            led_off_time = to_ms_since_boot(get_absolute_time());
        }
        if (chk_condition() == ERROR){
            master_off();
            led_flash = 100;
            g_total_fault = 1;
        }
        if (uart_flg == 1){
            uart_flg = 0;
            if (uart_process(uart_rx_data,cmd_ar, cmd_data_ar) == NON_ERROR){
                //printf("%s__%s\n",cmd_ar,cmd_data_ar);
                if (command_processing(cmd_ar,cmd_data_ar) == ERROR){
                    UART_SEND_ERR_MESSAGE
                }
            }else{
                UART_SEND_ERR_MESSAGE
            }
        }
    }
    return 0;
}