smellbomb’s blog

主にマイコンと電子工作

ESP32のUDP通信のベンチマーク

  • 目的
    • ESP32でWifiUDP通信を行った場合
      遅延と実効通信レートがどの程度になるか調査をする。
  • 試験環境
    • サーバー側:ESP-WROOM-32
      • ソフト:framework_arduino
      • Wifi設定:STAモード、送信電力19.5dB(MAX)
    • クライアント側:デスクトップPC(AP間は有線1Gbps接続)
      • OS:Windows10 21H2/64bit
      • ソフト:python3.9.2_64bit
    • ESP32放熱対策(シールドにヒートシンク取付)
      • サーマルパッド経由の放熱が無い事もあり、動作異常が発生してしまう為実施。
ESP32放熱対策
  • 試験内容(ソースコードは記事巻末参照。前回SPP通信同様手順とした)
    • 通信手順
      • ASCIIコードを用い、LFで送受信における完了トリガーとする。
    • 通信サイクル
      • ラズパイよりデータを送信し、ESP32が受信データと同一データを送信。
      • ラズパイ上で送受信データの一致確認をもってサイクル完了とした。
    • 測定方法
      • PC側でtime関数を使用し、上記サイクル時間を計測。
      • 1サイクル内の各種オーバーヘッドは含む。但しloopのオーバーヘッドは除く。
      • シリアルポートopen後の初回通信は評価対象としない。(時間が長い為)
      • 通信レートは1000サイクルにおける1サイクル平均値にて算出



  • 試験結果
    • 各送受信データ量における、N=1000の試験結果を示す。
      • 表1:試験結果概要
      • 各転送データ量における通信時間の分布は以下の通り。
        • 図1:1~10byte時の分布
        • 図2:10~100byte時の分布
        • 図3:100~500byte時の分布
        • 図4:500~900byte時の分布

表1:試験結果一覧

送受信データ
(byte)
通信時間
AVE(s)
通信時間
MAX(s)
通信時間
MIN(s)
通信時間
3σ(s)
通信時間
6σ(s)
実効通信レート
(bps)
1 0.006728 0.037318 0.002986 0.011635 0.02327 2378
2 0.006747 0.036077 0.002707 0.011704 0.023407 4743
3 0.006944 0.050978 0.002646 0.012661 0.025323 6912
4 0.00681 0.035284 0.002574 0.011763 0.023526 9398
5 0.007037 0.029986 0.002837 0.012069 0.024138 11368
10 0.006832 0.030259 0.002804 0.011145 0.022291 23419
20 0.006815 0.039036 0.00283 0.011928 0.023855 46955
30 0.00695 0.039083 0.002977 0.010938 0.021876 69065
40 0.00701 0.031827 0.002954 0.0109 0.0218 91298
50 0.006951 0.034668 0.002869 0.011069 0.022138 115091
100 0.007374 0.037092 0.003061 0.012287 0.024574 216979
150 0.007757 0.043328 0.003128 0.013567 0.027133 309398
200 0.007795 0.037367 0.00368 0.01239 0.024781 410520
250 0.007967 0.030451 0.00363 0.011485 0.02297 502071
300 0.008077 0.043682 0.004004 0.01274 0.025481 594280
350 0.008221 0.038176 0.00398 0.012072 0.024143 681182
400 0.008623 0.041072 0.004008 0.012782 0.025565 742201
450 0.009021 0.063308 0.00419 0.014533 0.029065 798138
500 0.009058 0.040857 0.004214 0.013759 0.027518 883197
600 0.009419 0.038899 0.004465 0.01333 0.026661 1019216
700 0.010279 0.083165 0.004678 0.017752 0.035503 1089600
800 0.011781 0.080902 0.004999 0.022093 0.044186 1086495
900 0.011107 0.061182 0.005582 0.018299 0.036598 1296480
図1:1~10byte時の分布
図2:10~100byte時の分布
図3:100~500byte時の分布
図4:500~900byte時の分布
  • 考察
    • 通信時間のバラつき
    • 通信自体のオーバーヘッド(1byte送信時)
      • ワースト29.7.3ms(ave:6.7ms 6σ:23ms) 但し、頻度としては10ms以内が多い。
    • ESP32高温時の動作異常について(温度はtemperatureRead()関数で観測)
      • CPU温度が49℃を超えた際に以下現象が発生した。
        ESP32側はパケット送信を実施しているが、クライアント側が受信できない。
        この時CPUはフリーズやシャットダウンは起きていない。
        クライアント側からのパケット受信はできている。
        温度が下がれば動作は再度安定する。
    • ESP32のSoftAPモードについて
      • 上記高温時の不具合内容が、温度との依存関係なく頻発した。
        電圧ドロップ等も確認したが、3.3~3.4Vは維持出来ており、原因は不明。
  • 結論
    • 制御指令に対する遅延:1指令が100byte以内であれば、ワースト32ms
    • 通信速度:1Mbps以上出る
  • 所感
    • BluetoothのSPP通信と同様の比較を行ったが、性能面だけを比較するのであれば、間違いなくWifiによるUDP通信の方が優れていた。
  • ESP32ソース
void wifi_info(){
  uint8_t wifiMac[6];
  esp_read_mac(wifiMac, ESP_MAC_WIFI_STA);
  char wifiMacChar[18] = {0};
  sprintf(wifiMacChar,"%02X:%02X:%02X:%02X:%02X:%02X", wifiMac[0],wifiMac[1],wifiMac[2],wifiMac[3],wifiMac[4],wifiMac[5]);
  Serial.printf("MAC:%s\n",wifiMacChar);
  Serial.print("ip:");
  Serial.println(WiFi.localIP());
  Serial.print("subnet:");
  Serial.println(WiFi.subnetMask());
  Serial.printf("wifiTxPower:%d\n",WiFi.getTxPower());
}

void wifiNonAP_setup(){
  const char ssid[]= "ssid";
  const char pass[]= "sspass";
  WiFi.begin(ssid,pass);
  while (WiFi.status() != WL_CONNECTED);{
    delay(500);
    Serial.println("wifi_connect_ok");
  }
void udp_setup(){
  const int localport = 10000;
  WiFi.setTxPower(WIFI_POWER_19_5dBm);
  wifiNonAP_setup();
  esp_wifi_set_ps(WIFI_PS_NONE);
  wifi_info();
  udp.begin(localport);
}

void setup() {
  Serial.begin(115200);
  Serial.printf("cpuFreq(Mhz):%d\n",ets_get_cpu_frequency());
  Serial.printf("cputemp:%f\n",temperatureRead());
  udp_setup();
  Serial.println("bootUpFin");
}
void loop() {
  const char *remoteIp = "xxx.xxx.xxx.xxx";
  const int remoteUdpPort = 10001;
  String btRxBuff;
  String udpRxBuff;
  while(true){
    if (udp.parsePacket()){
      udpRxBuff = udp.readStringUntil('\n');
      udp.beginPacket(remoteIp, remoteUdpPort);
      udp.print(udpRxBuff + '\n');
      udp.endPacket();
    }
  }
}
  • ラズパイ
import time
import socket
import csv

serv_ip = ("xxx.xxx.xxx.xxx", 10000)
local_ip = ("xxx.xxx.xxx.xxx", 10001)

tx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
rx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
rx_sock.bind(local_ip)


def udp_tx(tx_data="", chk=1):
    tx_data = tx_data + '\n'
    tx_data_b = tx_data.encode('utf-8')
    tx_sock.sendto(tx_data_b, serv_ip)
    if chk == 1:
        rx_data_b, add = rx_sock.recvfrom(1024)
        rx_data = rx_data_b.decode('utf-8')
        if tx_data_b == rx_data_b:
            return True
        else:
            print('err')
            return None
    else:
        return True


def udp_benchmark(loop=10, byte=1):
    result_ls = [byte]
    data = 0
    send_data = ""
    for n in range(byte):
        send_data += str(data)
        data += 1
        if data >= 9:
            data = 0
    tx_result = udp_tx(tx_data=send_data)
    for n in range(loop):
        start = time.time()
        tx_result = udp_tx(tx_data=send_data)
        ct = time.time() - start
        if tx_result is None:
            result_ls.append(-1)
        else:
            result_ls.append(round(ct, 6))
    return result_ls


for n in [1, 2, 3, 4, 5, 10, 20, 30, 40, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 600, 700, 800, 900, 1000]:
    result_ls = udp_benchmark(loop=1000, byte=n)
    print(result_ls)
    with open('udp_benchmark_pc.csv', 'a', newline='') as f:
        writer = csv.writer(f)
        writer.writerow(result_ls)
        f.close()


tx_sock.close()
rx_sock.close()

ESP32とラズパイでBluetoothによるSPP通信のベンチマーク

  • 目的
    • ESP32とラズパイでBluetoothのSPP通信を行った場合
      遅延と実効通信レートがどの程度になるか調査をする。
  • 試験環境
    • サーバー側:ESP-WROOM-32
    • クライアント側:Raspberrypi3b+(内臓Bluetooth使用)
      • OS:bullseye Release11(CLI運用)
      • ソフト:python3.9.2_64bit pyserial
    • "サーバー"・"クライアント"配置
      • 距離:1000mm(遮蔽物無し)
  • 試験内容(ソースコードは記事巻末参照)
    • 通信手順
      • ASCIIコードを用い、LFで送受信における完了トリガーとする。
    • 通信サイクル
      • ラズパイよりデータを送信し、ESP32が受信データと同一データを送信。
      • ラズパイ上で送受信データの一致確認をもってサイクル完了とした。
    • 測定方法
      • ラズパイ側でtime関数を使用し、上記サイクル時間を計測。
      • 1サイクル内の各種オーバーヘッドは含む。但しloopのオーバーヘッドは除く。
      • シリアルポートopen後の初回通信は評価対象としない。(時間が長い為)
      • 通信レートは1000サイクルにおける1サイクル平均値にて算出



  • 試験結果
    • 各送受信データ量における、N=1000の試験結果を示す。
      • 表1:試験結果概要
      • 各転送データ量における通信時間の分布は以下の通り。
        • 図1:1~10byte時の分布
        • 図2:10~100byte時の分布
        • 図3:100~500byte時の分布
        • 図4:500~900byte時の分布

表1:試験結果一覧

送受信データ
(byte)
通信時間
AVE(s)
通信時間
MAX(s)
通信時間
MIN(s)
通信時間
3σ(s)
通信時間
6σ(s)
実効通信レート
(bps)
1 0.013288 0.071321 0.006205 0.032500 0.065000 1204
2 0.013523 0.081167 0.006226 0.033617 0.067233 2366
3 0.013145 0.058818 0.007148 0.032047 0.064094 3652
4 0.01379 0.063877 0.006835 0.033823 0.067645 4641
5 0.013136 0.057554 0.006931 0.031898 0.063795 6090
10 0.013178 0.057569 0.007301 0.031447 0.062894 12141
20 0.015249 0.065444 0.008145 0.033261 0.066522 20985
30 0.017178 0.066215 0.008599 0.035967 0.071934 27943
40 0.017413 0.090023 0.009469 0.035429 0.070858 36754
50 0.019243 0.08304 0.009867 0.035136 0.070273 41574
100 0.033166 0.085077 0.014676 0.036745 0.073490 48242
150 0.036418 0.096614 0.015962 0.032540 0.065080 65901
200 0.036001 0.082626 0.018734 0.031361 0.062723 88886
250 0.037932 0.087403 0.022059 0.027688 0.055376 105452
300 0.039324 0.093719 0.024318 0.025414 0.050828 122063
350 0.041409 0.076375 0.026813 0.024264 0.048528 135236
400 0.044113 0.086197 0.028701 0.028282 0.056563 145082
450 0.057283 0.098023 0.032105 0.038603 0.077206 125692
500 0.063707 0.110676 0.034387 0.034159 0.068317 125575
600 0.067726 0.137092 0.040482 0.032502 0.065004 141748
700 0.075531 0.173518 0.044626 0.033677 0.067354 148283
800 0.079246 0.128892 0.049892 0.030826 0.061653 161522
900 0.095119 0.166947 0.056227 0.037209 0.074418 151389


図1:1~10byte時の分布

図2:10~100byte時の分布
図3:100~500byte時の分布
図4:500~900byte時の分布
  • 考察
    • 通信時間のバラつき
      • 正規分布ではなく、2山になっている。6σでの想定が妥当である。
    • 通信自体のオーバーヘッド(1byte送信時)
      • ワースト78.3ms(ave:13.3ms 6σ:65ms) 但し、頻度としては10msが多い。
  • 結論(一回辺り100byte以内の送受信による制御を想定)
    • 制御指令に対する遅延:ワースト106ms
  • ESP32ソース
#include <Arduino.h>
#include <BluetoothSerial.h>

BluetoothSerial SerialBT;
String btrx_buff;

void spp_setup(){
  uint8_t macBT[6];
  char btname[11];
  esp_read_mac(macBT, ESP_MAC_BT);
  sprintf(btname, "ESP32-%02X%02X", macBT[4], macBT[5]);
  if (SerialBT.begin(btname)) {
    Serial.println("start_spp");
  }
}

void setup() {
  Serial.begin(115200);
  spp_setup();
}

void loop() {
    btrx_buff = SerialBT.readStringUntil('\n');
    SerialBT.print(btrx_buff + '\n');
    //Serial.println(btrx_buff);
}
  • ラズパイ
import serial
import time
import csv


bt_spp = serial.Serial(port='/dev/rfcomm0', baudrate=115200, timeout=None)

def spp_tx(tx_buff=None,chk=1):
    tx_buff = tx_buff + '\n'
    tx_buff_b = tx_buff.encode('utf-8')
    bt_spp.write(tx_buff_b)
    if chk == 1:
        rx_data_b = bt_spp.readline()
        rx_data = rx_data_b.decode('utf-8')
        if tx_buff == rx_data:
            return True
        else:
            return None
    elif chk == 2:
        rx_data_b = bt_spp.readline()
        rx_data = rx_data_b.decode('utf-8')
        if rx_data == '\n':
            return True
        else:
            return None
    else:
        return True


def spp_benchimark(loop=10,byte=1):
    ct = 0
    result_ls =[byte]
    data = 0
    send_data =""
    for n in range(byte):
        send_data += str(data)
        data +=1
        if data >=9:
            data = 0
    tx_result = spp_tx(tx_buff=send_data)
    for n in range(loop):
        start = time.time()
        tx_result = spp_tx(tx_buff=send_data)
        ct = time.time()-start
        if tx_result is None:
            result_ls.append(-1)
        else:
            result_ls.append(round(ct,6))
    return result_ls

for n in [1,2,3,4,5,10,20,30,40,50,100,150,200,250,300,350,400,450,500,600,700,800,900]:
    result_ls = spp_benchimark(loop=1000,byte=n)
    with open('data.csv','a') as f:
        writer = csv.writer(f)
        writer.writerow(result_ls)

bt_spp.close()

Nucleo64_F446RE用シールド基板完成

Nucleo64をそのまま使い、ラジコン用のシールド基板を作成。

主電源:12~13V前後のバッテリーを使用。

5V電源:村田製降圧型DCDCコンバータ OKL-T/6-W12N-C

モータードライブIC:DRV8871

6軸ジャイロ:MPU-6050(GY-521)

 

f:id:smellbomb:20220409145721j:plainf:id:smellbomb:20220409115623j:plainf:id:smellbomb:20220409145256j:plainf:id:smellbomb:20220409150521j:plainf:id:smellbomb:20220409150613j:plain

 

■通信関係

メインCPU通信用SPI(SPI2)・・・J3端子台・ラズパイと繋ぎこみ

メインCPU通信用UART(UART6)・・・J3端子台・SPIとジャンパで切替

ジャイロセンサ通信用I2C(I2C1)・・・基板上MPU-6050と接続

予備SPI(SPI3)・・・J10ピンソケット出力

予備I2C(I2C2/I2C3)・・・J8/J9ピンソケット出力

 

■モーター制御

AB相エンコーダ・・・J2ピンソケット 5Vトレラント接続

サーボモータ・・・J4,J5ピンソケット

 

■センサー

バッテリー電圧監視・・・抵抗分圧でADC1入力(R1:47kΩ R3:10kΩ)

5V電圧監視・・・抵抗分圧でADC2入力(R2,R3:10kΩ)

超音波距離センサーSR04・・・J6,J7ピンソケット入力(5Vトレラント)

 

 

 

モーター制御基板設計変更 STM32F303K8にて

モーター制御基板をラズパイPicoからSTM32F303K8へ変更

 

理由

1.高速通信制御可能なスレーブデバイスにしたい

2.100LPR程度のエンコーダーを使えるようにしたい

3.そもそも処理能力がカツカツになった

 

STM32F303K8へ変更後

■通信

>SPI通信10MHzのスレーブデバイス

>DMA転送で通信処理負荷低減

>設定値等の通信は、レジスタマップとして複数同時処理できるようにした

エンコーダー

ペリフェラルのエンコーダを使用

RTOS

>オーバーヘッドのデメリットの方が多く、

 STM32F303K8ではメモリがギリギリになりそうなので不採用とした。

 

■構成

以下の通り、ギリギリ足ります。現状空きPINは2点。

・I2C×1・・・ジャイロ用

・SPI全二重×1・・・主制御装置通信用(ラズパイ)

・動力モーター用PWM(TIM1、CH1,2)

サーボモータ用PWM(TIM2、CH1,2)

エンコーダー用タイマ(TIM3)・・・入力は5V耐圧端子にて

・GPIO×3・・・超音波センサーSR04用、TRIGは共用

・制御系5V及びバッテリー電圧監視用ADC×2(ADC1,IN4,IN11)

 

注意点:Nucleoを使用する場合、I2CとSPIを同時使用する場合

裏面ショートジャンパ SB16とSB18は取り除く必要があります。

これに気づくのに一週間位溶かしました。

f:id:smellbomb:20211115005835j:plain

SB16,SB18

f:id:smellbomb:20211114235121j:plain

STM32F303K8ピン配置

完成はしてませんが、概ね形になってきたのでソースご参考迄に

最初はLLで書こうかと思いましたが、後でF4に変更したりの可能性もあったので

HALで書いてあります。

 

SPI通信は1フレーム8bit固定×任意フレーム数として通信できるように

マスターから送信時に以下のようにしました。

「ヘッダ、開始アドレス0xXX、R/Wアドレス数」

ヘッダは、読み込み0x10、書き込み0x20。

将来的に、このヘッダを拡張してバースト処理できるようにしようと思います。

 

 

smellbomb.hatenablog.com

 

STM32F303K8 作りかけソース20211115

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "string.h"
#include "math.h"
#include "inttypes.h"

#define ADDR_ERR_1      0x00
#define ADDR_ERR_2      0x01
#define ADDR_ERR_3      0x02
#define ADDR_ERR_4      0x03
#define ADDR_ERR_5      0x04
#define ADDR_ERR_6      0x05
#define ADDR_ERR_7      0x06
#define ADDR_ERR_8      0x07
#define ADDR_ERR_9      0x08
#define ADDR_ERR_10     0x09


#define ADDR_RPS_SET_H  0x0A
#define ADDR_RPS_SET_L  0x0B
#define ADDR_TRQ_OVER   0x0C
#define ADDR_ROT_RST    0x0D
#define ADDR_STP_ROT_H  0x0E
#define ADDR_STP_ROT_L  0x0F

#define ADDR_ROT_SUM_H  0x10
#define ADDR_ROT_SUM_L  0x11

#define ADDR_RPS_RAW_H  0x12
#define ADDR_RPS_RAW_L  0x13
#define ADDR_RPS_AVE_H  0x14
#define ADDR_RPS_AVE_L  0x15
#define ADDR_RPS_MAX_H  0x16
#define ADDR_RPS_MAX_L  0x17
#define ADDR_RPS_MIN_H  0x18
#define ADDR_RPS_MIN_L  0x19

#define ADDR_DTY_RAW_H  0x1A
#define ADDR_DTY_RAW_L  0x1B
#define ADDR_DTY_AVE_H  0x1C
#define ADDR_DTY_AVE_L  0x1D
#define ADDR_DTY_MAX_H  0x1E
#define ADDR_DTY_MAX_L  0x1F
#define ADDR_DTY_MIN_H  0x20
#define ADDR_DTY_MIN_L  0x21
#define ADDR_DRV_MEM    0x22
#define ADDR_AVE_TIM    0x23

#define ADDR_PID_KP_H   0x24
#define ADDR_PID_KP_L   0x25
#define ADDR_PID_KI_H   0x26
#define ADDR_PID_KI_L   0x27
#define ADDR_PID_KD_H   0x28
#define ADDR_PID_KD_L   0x29

#define ADDR_DT1_REQ    0x30
#define ADDR_DT1_DST_H  0x31
#define ADDR_DT1_DST_L  0x32
#define ADDR_DT2_REQ    0x33
#define ADDR_DT2_DST_H  0x34
#define ADDR_DT2_DST_L  0x35



/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;

I2C_HandleTypeDef hi2c1;

RTC_HandleTypeDef hrtc;

SPI_HandleTypeDef hspi1;
DMA_HandleTypeDef hdma_spi1_rx;
DMA_HandleTypeDef hdma_spi1_tx;

TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim6;
TIM_HandleTypeDef htim7;
TIM_HandleTypeDef htim17;

UART_HandleTypeDef huart2;

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_DMA_Init(void);
static void MX_SPI1_Init(void);
static void MX_I2C1_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM6_Init(void);
static void MX_TIM7_Init(void);
static void MX_TIM2_Init(void);
static void MX_RTC_Init(void);
static void MX_ADC1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM17_Init(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int _write(int file, char *ptr, int len){
  HAL_UART_Transmit(&huart2,(uint8_t *)ptr,len,10);
  return len;
}


uint8_t param_arry[200] = {0};
enum {
    SPI_STATE_READY,SPI_STATE_WAIT_COMMAND,SPI_STATE_GET_COMMAND,SPI_STATE_WAIT_READ,SPI_STATE_WAIT_WRITE,SPI_STATE_COMMAND_ERR
};

enum{
      DIST1_READY,DIST1_INIT_START,DIST1_INIT_END,DIST1_MES_START,DIST1_TRG_WAIT,DIST1_ECH_WAIT,DIST1_MES_END
};
enum{
      DIST2_READY,DIST2_INIT_START,DIST2_INIT_END,DIST2_MES_START,DIST2_TRG_WAIT,DIST2_ECH_WAIT,DIST2_MES_END
};
uint16_t dist1_st = 0, dist1_ed = 0;
uint16_t dist2_st = 0, dist2_ed = 0;
uint8_t dist1_state = DIST1_READY;
uint8_t dist2_state = DIST2_READY;

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
    setbuf(stdout, NULL);
    param_arry[ADDR_AVE_TIM] = 50;

    param_arry[ADDR_PID_KP_H] = ((uint16_t)(600)) >> 8;
    param_arry[ADDR_PID_KP_L] = ((uint16_t)(600)) & 0x00FF;
    param_arry[ADDR_PID_KI_H] = ((uint16_t)(2)) >> 8;
    param_arry[ADDR_PID_KI_L] = ((uint16_t)(2)) & 0x00FF;
    param_arry[ADDR_PID_KD_H] = ((uint16_t)(300)) >> 8;
    param_arry[ADDR_PID_KD_L] = ((uint16_t)(300)) & 0x00FF;


  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART2_UART_Init();
  MX_DMA_Init();
  MX_SPI1_Init();
  MX_I2C1_Init();
  MX_TIM1_Init();
  MX_TIM6_Init();
  MX_TIM7_Init();
  MX_TIM2_Init();
  MX_RTC_Init();
  MX_ADC1_Init();
  MX_TIM3_Init();
  MX_TIM17_Init();
  /* USER CODE BEGIN 2 */
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
  __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, 0);
  __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, 0);
  __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1, 0);
  __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2, 0);
  HAL_TIM_Encoder_Start( &htim3, TIM_CHANNEL_1);
  HAL_TIM_Encoder_Start( &htim3, TIM_CHANNEL_2);
  HAL_TIM_Base_Start_IT(&htim6);
  HAL_TIM_Base_Start_IT(&htim7);
  HAL_TIM_Base_Start_IT(&htim17);
  HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  uint8_t spiState = SPI_STATE_READY;




  while (1)
  {
      /*spi_start*/
      static uint8_t command_rxbuf[2] = {};
      static uint8_t spi_txbuf[128] = {0};
      static uint8_t spi_rxbuf[128] = {0};


      if (spiState == SPI_STATE_READY) {
          HAL_SPI_Receive_DMA(&hspi1, command_rxbuf,3);
          spiState = SPI_STATE_WAIT_COMMAND;
      }
      if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_COMMAND) {
          spiState = SPI_STATE_GET_COMMAND;
      }

      if (spiState == SPI_STATE_GET_COMMAND && command_rxbuf[0] == 0x10 && command_rxbuf[2] >=1) {
          spiState = SPI_STATE_WAIT_READ;
          for (int var = 0; var < command_rxbuf[2]; ++var) {
              spi_txbuf[var] = param_arry[command_rxbuf[1] + var];
          }
          HAL_SPI_Transmit_DMA(&hspi1, spi_txbuf, command_rxbuf[2]);
      }
      else if (spiState == SPI_STATE_GET_COMMAND && command_rxbuf[0] == 0x20 && command_rxbuf[2] >=1) {
          spiState = SPI_STATE_WAIT_WRITE;
          HAL_SPI_Receive_DMA(&hspi1, spi_rxbuf, command_rxbuf[2]);
      }
      else if (spiState == SPI_STATE_GET_COMMAND) {
          spiState = SPI_STATE_COMMAND_ERR;
          printf("command_err\r\n");
      }


      if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_READ) {
          spiState = SPI_STATE_READY;
      }


      if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_WRITE) {
          for (int var = 0; var < command_rxbuf[2]; ++var) {
              param_arry[command_rxbuf[1] + var] = spi_rxbuf[var];
          }
          spiState = SPI_STATE_READY;
      }
      /*spi_end*/

      /*dist_mes*/
      uint8_t dist_trg_pin_data = HAL_GPIO_ReadPin(DIST_TRG_GPIO_Port,DIST_TRG_Pin);
      uint8_t dist1_ech_pin_data = HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port,DIST1_ECH_Pin);
      uint16_t dist1_dist = (int16_t)(((param_arry[ADDR_DT1_DST_H] << 8) & 0xFF00) | (param_arry[ADDR_DT1_DST_H] & 0x00FF));
      uint16_t dist1_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
      if (param_arry[ADDR_DT1_REQ] != 0 && dist1_state == DIST1_READY && dist_trg_pin_data == GPIO_PIN_RESET && dist1_ech_pin_data == GPIO_PIN_RESET
              && dist2_state == DIST2_READY) {
          dist1_state = DIST1_INIT_START;
          param_arry[ADDR_DT1_DST_H] = 0;
          param_arry[ADDR_DT1_DST_L] = 0;
      }else if (dist1_state == DIST1_INIT_START) {
          dist1_state = DIST1_INIT_END;
          HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_SET);
          __HAL_TIM_SET_COUNTER(&htim17,0);
          dist1_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
      }else if (dist1_state == DIST1_INIT_END && dist_trg_pin_data == GPIO_PIN_SET) {
          dist1_state = DIST1_TRG_WAIT;
      }else if (dist1_state == DIST1_TRG_WAIT && dist1_cycle_time > 2) {
          HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
          dist1_state = DIST1_ECH_WAIT;
      }else if (dist1_state == DIST1_MES_END) {
          dist1_state = DIST1_READY;
          param_arry[ADDR_DT1_REQ] = 0;
      }else if (dist1_state >= DIST1_INIT_END && dist1_cycle_time > 3000) {
          dist1_dist = -1;
          HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
          param_arry[ADDR_DT1_DST_H] = (dist1_dist) >> 8;
          param_arry[ADDR_DT1_DST_L] = (dist1_dist) & 0x00FF;
          param_arry[ADDR_DT1_REQ] = 0;
          dist1_state = DIST1_READY;
      }

      /*dist2_mes*/

      uint8_t dist2_ech_pin_data = HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port,DIST2_ECH_Pin);
      uint16_t dist2_dist = (int16_t)(((param_arry[ADDR_DT2_DST_H] << 8) & 0xFF00) | (param_arry[ADDR_DT2_DST_H] & 0x00FF));
      uint16_t dist2_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
      if (param_arry[ADDR_DT2_REQ] != 0 && dist2_state == DIST2_READY && dist1_state == DIST1_READY
              && dist_trg_pin_data == GPIO_PIN_RESET && dist2_ech_pin_data == GPIO_PIN_RESET){
          dist2_state = DIST2_INIT_START;
          param_arry[ADDR_DT2_DST_H] = 0;
          param_arry[ADDR_DT2_DST_L] = 0;
      }else if (dist2_state == DIST2_INIT_START) {
          dist2_state = DIST2_INIT_END;
          HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_SET);
          __HAL_TIM_SET_COUNTER(&htim17,0);
          dist2_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
      }else if (dist2_state == DIST2_INIT_END && dist_trg_pin_data == GPIO_PIN_SET) {
          dist2_state = DIST2_TRG_WAIT;
      }else if (dist2_state == DIST2_TRG_WAIT && dist2_cycle_time > 2) {
          HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
          dist2_state = DIST2_ECH_WAIT;
      }else if (dist2_state == DIST2_MES_END) {
          dist2_state = DIST2_READY;
          param_arry[ADDR_DT2_REQ] = 0;
      }else if (dist2_state >= DIST2_INIT_END && dist2_cycle_time > 3000) {
          dist2_dist = -1;
          HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
          param_arry[ADDR_DT2_DST_H] = (dist2_dist) >> 8;
          param_arry[ADDR_DT2_DST_L] = (dist2_dist) & 0x00FF;
          param_arry[ADDR_DT2_REQ] = 0;
          dist2_state = DIST2_READY;
          }





    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_LSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_RTC
                              |RCC_PERIPHCLK_TIM1|RCC_PERIPHCLK_ADC12;
  PeriphClkInit.Adc12ClockSelection = RCC_ADC12PLLCLK_DIV1;
  PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI;
  PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
  PeriphClkInit.Tim1ClockSelection = RCC_TIM1CLK_PLLCLK;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief ADC1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC1_Init(void)
{

  /* USER CODE BEGIN ADC1_Init 0 */

  /* USER CODE END ADC1_Init 0 */

  ADC_MultiModeTypeDef multimode = {0};
  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC1_Init 1 */

  /* USER CODE END ADC1_Init 1 */
  /** Common config
  */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
  hadc1.Init.Resolution = ADC_RESOLUTION_12B;
  hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.NbrOfConversion = 1;
  hadc1.Init.DMAContinuousRequests = DISABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc1.Init.LowPowerAutoWait = DISABLE;
  hadc1.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure the ADC multi-mode
  */
  multimode.Mode = ADC_MODE_INDEPENDENT;
  if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_4;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SingleDiff = ADC_SINGLE_ENDED;
  sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
  sConfig.OffsetNumber = ADC_OFFSET_NONE;
  sConfig.Offset = 0;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */

  /* USER CODE END ADC1_Init 2 */

}

/**
  * @brief I2C1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x2000090E;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C1_Init 2 */

  /* USER CODE END I2C1_Init 2 */

}

/**
  * @brief RTC Initialization Function
  * @param None
  * @retval None
  */
static void MX_RTC_Init(void)
{

  /* USER CODE BEGIN RTC_Init 0 */

  /* USER CODE END RTC_Init 0 */

  /* USER CODE BEGIN RTC_Init 1 */

  /* USER CODE END RTC_Init 1 */
  /** Initialize RTC Only
  */
  hrtc.Instance = RTC;
  hrtc.Init.HourFormat = RTC_HOURFORMAT_24;
  hrtc.Init.AsynchPrediv = 127;
  hrtc.Init.SynchPrediv = 255;
  hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;
  hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;
  hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;
  if (HAL_RTC_Init(&hrtc) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN RTC_Init 2 */

  /* USER CODE END RTC_Init 2 */

}

/**
  * @brief SPI1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_SPI1_Init(void)
{

  /* USER CODE BEGIN SPI1_Init 0 */

  /* USER CODE END SPI1_Init 0 */

  /* USER CODE BEGIN SPI1_Init 1 */

  /* USER CODE END SPI1_Init 1 */
  /* SPI1 parameter configuration*/
  hspi1.Instance = SPI1;
  hspi1.Init.Mode = SPI_MODE_SLAVE;
  hspi1.Init.Direction = SPI_DIRECTION_2LINES;
  hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
  hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
  hspi1.Init.NSS = SPI_NSS_HARD_INPUT;
  hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi1.Init.CRCPolynomial = 7;
  hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
  hspi1.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
  if (HAL_SPI_Init(&hspi1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI1_Init 2 */

  /* USER CODE END SPI1_Init 2 */

}

/**
  * @brief TIM1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM1_Init(void)
{

  /* USER CODE BEGIN TIM1_Init 0 */

  /* USER CODE END TIM1_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};
  TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};

  /* USER CODE BEGIN TIM1_Init 1 */

  /* USER CODE END TIM1_Init 1 */
  htim1.Instance = TIM1;
  htim1.Init.Prescaler = 0;
  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim1.Init.Period = 2559;
  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim1.Init.RepetitionCounter = 0;
  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
  sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
  sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
  sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
  sBreakDeadTimeConfig.DeadTime = 0;
  sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
  sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
  sBreakDeadTimeConfig.BreakFilter = 0;
  sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
  sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
  sBreakDeadTimeConfig.Break2Filter = 0;
  sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
  if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM1_Init 2 */

  /* USER CODE END TIM1_Init 2 */
  HAL_TIM_MspPostInit(&htim1);

}

/**
  * @brief TIM2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM2_Init(void)
{

  /* USER CODE BEGIN TIM2_Init 0 */

  /* USER CODE END TIM2_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM2_Init 1 */

  /* USER CODE END TIM2_Init 1 */
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 4294967295;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */

  /* USER CODE END TIM2_Init 2 */
  HAL_TIM_MspPostInit(&htim2);

}

/**
  * @brief TIM3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM3_Init(void)
{

  /* USER CODE BEGIN TIM3_Init 0 */

  /* USER CODE END TIM3_Init 0 */

  TIM_Encoder_InitTypeDef sConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM3_Init 1 */

  /* USER CODE END TIM3_Init 1 */
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 0;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 65535;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
  sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC1Filter = 0;
  sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
  sConfig.IC2Filter = 0;
  if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM3_Init 2 */

  /* USER CODE END TIM3_Init 2 */

}

/**
  * @brief TIM6 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM6_Init(void)
{

  /* USER CODE BEGIN TIM6_Init 0 */

  /* USER CODE END TIM6_Init 0 */

  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM6_Init 1 */

  /* USER CODE END TIM6_Init 1 */
  htim6.Instance = TIM6;
  htim6.Init.Prescaler = 64;
  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim6.Init.Period = 9999;
  htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM6_Init 2 */

  /* USER CODE END TIM6_Init 2 */

}

/**
  * @brief TIM7 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM7_Init(void)
{

  /* USER CODE BEGIN TIM7_Init 0 */

  /* USER CODE END TIM7_Init 0 */

  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM7_Init 1 */

  /* USER CODE END TIM7_Init 1 */
  htim7.Instance = TIM7;
  htim7.Init.Prescaler = 64000;
  htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim7.Init.Period = 100;
  htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM7_Init 2 */

  /* USER CODE END TIM7_Init 2 */

}

/**
  * @brief TIM17 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM17_Init(void)
{

  /* USER CODE BEGIN TIM17_Init 0 */

  /* USER CODE END TIM17_Init 0 */

  /* USER CODE BEGIN TIM17_Init 1 */

  /* USER CODE END TIM17_Init 1 */
  htim17.Instance = TIM17;
  htim17.Init.Prescaler = 639;
  htim17.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim17.Init.Period = 65535;
  htim17.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim17.Init.RepetitionCounter = 0;
  htim17.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim17) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM17_Init 2 */

  /* USER CODE END TIM17_Init 2 */

}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  huart2.Instance = USART2;
  huart2.Init.BaudRate = 38400;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART2_Init 2 */

  /* USER CODE END USART2_Init 2 */

}

/**
  * Enable DMA controller clock
  */
static void MX_DMA_Init(void)
{

  /* DMA controller clock enable */
  __HAL_RCC_DMA1_CLK_ENABLE();

  /* DMA interrupt init */
  /* DMA1_Channel2_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
  /* DMA1_Channel3_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOF_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin : TEST_Pin */
  GPIO_InitStruct.Pin = TEST_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(TEST_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pins : DIST2_ECH_Pin DIST1_ECH_Pin */
  GPIO_InitStruct.Pin = DIST2_ECH_Pin|DIST1_ECH_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pin : DIST_TRG_Pin */
  GPIO_InitStruct.Pin = DIST_TRG_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(DIST_TRG_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pin : LED_Pin */
  GPIO_InitStruct.Pin = LED_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);

  /* EXTI interrupt init*/
  HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);

}

/* USER CODE BEGIN 4 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi1){
  //spiState = SPI_TXRX_COMP;
  //printf("spi_txrx_comp\r\n");
}

void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi1){
    //spiState = SPI_RX_COMP;
    //printf("spi_rx_comp\r\n");
}

void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi1){
    //spiState = SPI_TX_COMP;
    //printf("spi_tx_comp\r\n");
}

void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi1){
  //spiState = SPI_ERR;
  printf("spi_err\r\n");
}


void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
    if (GPIO_Pin == DIST1_ECH_Pin && HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port, DIST1_ECH_Pin) == GPIO_PIN_SET && dist1_state == DIST1_ECH_WAIT) {
        dist1_state = DIST1_MES_START;
        dist1_st = __HAL_TIM_GET_COUNTER(&htim17);
        //printf("%d",dist1_st);
    }
    if (GPIO_Pin == DIST1_ECH_Pin && HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port, DIST1_ECH_Pin) == GPIO_PIN_RESET && dist1_state == DIST1_MES_START) {
        dist1_ed = __HAL_TIM_GET_COUNTER(&htim17);
        int16_t dist1_dist = ((float)(dist1_ed - dist1_st) * 3.4f) / 2.0f;
        param_arry[ADDR_DT1_DST_H] = (dist1_dist) >> 8;
        param_arry[ADDR_DT1_DST_L] = (dist1_dist) & 0x00FF;
        dist1_state = DIST1_MES_END;


    }
    if (GPIO_Pin == DIST2_ECH_Pin && HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port, DIST2_ECH_Pin) == GPIO_PIN_SET && dist2_state == DIST2_ECH_WAIT) {
        dist2_state = DIST2_MES_START;
        dist2_st = __HAL_TIM_GET_COUNTER(&htim17);
            //printf("%d",dist1_st);
        }
        if (GPIO_Pin == DIST2_ECH_Pin && HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port, DIST2_ECH_Pin) == GPIO_PIN_RESET && dist2_state == DIST2_MES_START) {
            dist2_ed = __HAL_TIM_GET_COUNTER(&htim17);
            int16_t dist2_dist = ((float)(dist2_ed - dist2_st) * 3.4f) / 2.0f;
            param_arry[ADDR_DT2_DST_H] = (dist2_dist) >> 8;
            param_arry[ADDR_DT2_DST_L] = (dist2_dist) & 0x00FF;
            dist2_state = DIST2_MES_END;


        }
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
    //HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
    if (htim == &htim6) {
        HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_SET);
        const float dt = 10.0f;
        const float rot_pls = 52;

        const float pwm_period = 2559.0f;
        int16_t cnt_raw = 0;


        static float rps_arr[50] = {}, duty_arr[50] = {};
        static float rps_max=0, rps_min=0, duty_max=0, duty_min=0;
        static float rps_raw = 0,rps_ave = 0;
        static float duty_raw = 0,duty = 0,duty_ave = 0;
        float rps_sum = 0, duty_sum=0;
        static int64_t cnt_sum = 0;
        static int16_t rot_sum = 0;
        static uint8_t ave_count = 0;
        static uint16_t trq_over_cont = 0;
        static float P,I,D,preP=0;

        float ave_point = param_arry[ADDR_AVE_TIM];
        float set_rps = (float)(int16_t)(((param_arry[ADDR_RPS_SET_H] << 8) & 0xFF00) | (param_arry[ADDR_RPS_SET_L] & 0x00FF));
        int16_t stop_rot = (int16_t)(((param_arry[ADDR_STP_ROT_H] << 8) & 0xFF00) | (param_arry[ADDR_STP_ROT_L] & 0x00FF));
        float kp = (float)(int16_t)(((param_arry[ADDR_PID_KP_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KP_L] & 0x00FF)) * 0.01f;
        float ki = (float)(int16_t)(((param_arry[ADDR_PID_KI_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KI_L] & 0x00FF)) * 0.01f;
        float kd = (float)(int16_t)(((param_arry[ADDR_PID_KD_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KD_L] & 0x00FF)) * 0.01f;
        cnt_raw = __HAL_TIM_GET_COUNTER(&htim3);
        __HAL_TIM_SET_COUNTER(&htim3,0);

        if (ave_count > ave_point) {ave_count = 0;}

        if (param_arry[ADDR_ROT_RST] != 0x00) {
            cnt_sum = 0;
            param_arry[ADDR_ROT_RST] = 0x00;
        }
        cnt_sum += cnt_raw;
        rot_sum = cnt_sum / rot_pls;
        rps_raw = ((float)cnt_raw / rot_pls) * (1000.0f/dt);
        rps_arr[ave_count] = rps_raw;

        //PID

        P = set_rps - rps_raw;
        I += (P + preP) / 2.0f * dt;
        D = (P - preP) / dt;
        preP = P;
        if (fabs(rps_ave) < 0.001 && set_rps == 0) {
            P = 0;
            I = 0;
            D = 0;
        }
        duty_raw = kp * P + ki *I + kd * D;
        if (duty_raw >= 100) {
            duty_raw = 100;
        }else if (duty <= -100) {
            duty_raw = -100;
        }
        duty = duty_raw;
        duty_arr[ave_count] = duty;


        for (int var = 0; var < ave_point; ++var) {duty_sum += duty_arr[var];}
        duty_ave = duty_sum / ave_point;

        for (int var = 0; var < ave_point; ++var) {rps_sum += rps_arr[var];}
        rps_ave = rps_sum / ave_point;


        if (rps_max < rps_ave) {rps_max = rps_ave;}
        if (rps_min > rps_ave) {rps_min = rps_ave;}
        if (duty_max < duty) {duty_max = duty;}
        if (duty < duty_min) {duty_min = duty;}

        if (param_arry[ADDR_DRV_MEM] != 0) {
            duty_max = duty;
            duty_min = duty;
            rps_max = rps_ave;
            rps_min = rps_ave;
            param_arry[ADDR_DRV_MEM] = 0x00;
        }





        param_arry[ADDR_ROT_SUM_H] = (rot_sum >> 8);
        param_arry[ADDR_ROT_SUM_L] = (rot_sum >> 0);

        param_arry[ADDR_RPS_RAW_H] = ((int16_t)(rps_raw * 100.0f)) >> 8;
        param_arry[ADDR_RPS_RAW_L] = ((int16_t)(rps_raw * 100.0f)) >> 0;
        param_arry[ADDR_RPS_AVE_H] = ((int16_t)(rps_ave * 100.0f)) >> 8;
        param_arry[ADDR_RPS_AVE_L] = ((int16_t)(rps_ave * 100.0f)) >> 0;
        param_arry[ADDR_RPS_MAX_H] = ((int16_t)(rps_max * 100.0f)) >> 8;
        param_arry[ADDR_RPS_MAX_L] = ((int16_t)(rps_max * 100.0f)) >> 0;
        param_arry[ADDR_RPS_MIN_H] = ((int16_t)(rps_min * 100.0f)) >> 8;
        param_arry[ADDR_RPS_MIN_L] = ((int16_t)(rps_min * 100.0f)) >> 0;
        param_arry[ADDR_DTY_AVE_H] = ((int16_t)(duty_ave * 100.0f)) >> 8;
        param_arry[ADDR_DTY_AVE_L] = ((int16_t)(duty_ave * 100.0f)) >> 0;
        param_arry[ADDR_DTY_MAX_H] = ((int16_t)(duty_max * 100.0f)) >> 8;
        param_arry[ADDR_DTY_MAX_L] = ((int16_t)(duty_max * 100.0f)) >> 0;
        param_arry[ADDR_DTY_MIN_H] = ((int16_t)(duty_min * 100.0f)) >> 8;
        param_arry[ADDR_DTY_MIN_L] = ((int16_t)(duty_min * 100.0f)) >> 0;



        ave_count += 1;
        /*PWM*/
        int pwm_count = (1-(fabs(duty)/100)) * pwm_period;

        if (param_arry[ADDR_TRQ_OVER] != 0) {
            param_arry[ADDR_RPS_SET_H] = 0x00;
            param_arry[ADDR_RPS_SET_L] = 0x00;
        }

        if ((stop_rot > 0 && rot_sum >= stop_rot) || (stop_rot < 0 && rot_sum <= stop_rot)) {
            param_arry[ADDR_RPS_SET_H] = 0x00;
            param_arry[ADDR_RPS_SET_L] = 0x00;

        }

        if (duty >= 0) {
            __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, (int)pwm_period);
            __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, pwm_count);
        }else {
            __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, pwm_count);
            __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, (int)pwm_period);
        }
        if (fabs(duty) >= 99 && fabs(rps_raw) < 0.5f) {
            trq_over_cont += 1;
            if (trq_over_cont > 1000/dt) {
                param_arry[ADDR_TRQ_OVER] = 0x01;
                param_arry[ADDR_RPS_SET_H] = 0x00;
                param_arry[ADDR_RPS_SET_L] = 0x00;
            }
        }else{
            trq_over_cont = 0;
        }


        HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_RESET);
        //printf("%5.2f_%5.2f_%5.2f\r\n",P,I,D);

    }
    if (htim == &htim7){
        HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
        //printf("%5.2f_%5.2f_%5.2f_%5.2f\r\n",g_rps_raw,g_rps_ave,g_rps_max,g_rps_min);
        //int16_t rps_ave = (int16_t)(((param_arry[ADDR_RPS_AVE_H] << 8) & 0xFF00) | (param_arry[ADDR_RPS_AVE_L] & 0x00FF));



    }
}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

PWM制御の差異と出力トルク

現在作成中の猫を追いかける予定の車ですが、

概ねモーター制御側の雛形は完成してきました。

 

最終的にバッテリーで長時間駆動させるために、

適切なモーター駆動条件を確認しました。

確認した項目としては、以下の通り。

・PWM駆動周波数

・PWMOFF期間をFastDecay、SlowDecay

・減速側出力のON/OFF

 

 

■HブリッジのFastDecayとSlowDecayについて

概要だけ説明します。

FastDecay:PWMOFF期間を電源側へ回生

SlowDecay:ダイオードを介して逆起電力を再利用。

※SlowDecayは回生ブレーキと同位になります。

IndexPro様に分かりやすい解説があるので、紹介します。

www.indexpro.co.jp

 

尚、L298Nを使用する場合、

SlowDecay:EnablePinをONしたまま、InputPinをPWM制御

FastDecay:InputPinをCWかCCWで固定し、EnablePinをPWM制御

データシート抜粋し以下の通りです。

f:id:smellbomb:20211023135532j:plain

L298N_mode

■試験条件

機材詳細は以下の通り。

但し、以前は各種モータードライバの駆動はフォトカプラを介して駆動していたが

撤去し、マイコンTTL出力で直接駆動。

 

smellbomb.hatenablog.com

 

■試験方法

以下写真のように、駆動輪を空転させる形で実施。

f:id:smellbomb:20211023121739j:plain

トルク試験状態

マイコン内で5ms周期でPID制御を実施。

・回転数が規定回転数に到達後、指令値±1rpm時点のPWMのDuty平均値

※回転数、及びDutyは5msサンプリングの50回移動平均

・駆動電源は、DC12V/1.5Aスイッチング電源

 

 

■実験1

PID制御で加速側のみCW方向にPWM出力ON、減速側はDuty0%。

PWMOFF期間をFastDecay、SlowDecay

PWM駆動周波数は50Hz~100kHz

f:id:smellbomb:20211023131605p:plain

回転数とPWM駆動周波数ーDuty(出力は加速側のみ)

■実験2

実験結果1に付随して

・PID制御の減速側CCW出力有無で比較。

尚、駆動周波数は代表として10kHzで実施。

 

f:id:smellbomb:20211023151352p:plain

※CW,CCWの総和より時間平均としてある。

■実験結果まとめ

・基本的にFastDecayを採用するメリットはない。

・スイッチング損失等色々コミコミで考えたときに、SlowDecay運用であれば

 PWM駆動周波数1kHz以上は、ほぼ出力トルクは一定と考えられる。

・定速状態であれば、PID出力の減速側をCCW出力を行ったとしても

概ね5%程度でDutyが増加した。

 

■所感

特に細かく制御を切り替えず

PWM駆動周波数は10k~50kHz程度で、SlowDecacy運用。

急停止による駆動系への負荷を気にしなければ

終始加減速を考慮したPID出力で良さそう。