はじめに
QRコードを読んでいただきありがとうございます。
ギリギリでプログラム配布のために書いた記事なので至らぬところがあるかもしれませんが、ご了承ください。
今回はあくまでもプログラムの配布をメインにするので、シミュレーションの中身の計算式とかが知りたい方は以下の記事を参照してください。
(https://qiita.com/Gomachan_2021/items/3f01e44417a648708ac9)
システム構成
本作品は図1に示すようなシステム構成となっています。
Arduino UNOでハンドル・アクセル・ブレーキ・シフトノブから値を取得して、自動車およびエンジンの運動方程式を逐次解いています。上記の計算によって得られたエンジンの回転数をタコメータに反映するのもArduino UNOで処理しています。
Arduino UNOで計算および取得した、車両速度・シフト・エンジン回転数・計測タイム・走行距離・実機の制御入力の値をUSBシリアル通信でPCへ送信し、Processingを使って、メータへ反映しています。実機の制御入力の値はPCのBluetoothを使ってESP32へ送信しています。ESP32はPCから受け取った制御入力の値をもとに実機を制御しています。
バージョン情報
開発環境
・Arduino IDE 1.8.19
・Processing 4.3
Arduino IDEは少し古い(丸いアイコンの頃の)バージョンのものを使用しています。それほど深い理由はありませんが、使い慣れていることと、最新のバージョンのArduino IDEはそれ本体のインストールとは別にドライバみたいなものが必要っぽいという記事を見たことが要因です。Processingは当時最新だったものを使用しました。特に理由はないです。
使用製品
・Arduino UNO
・ESP32-WROOM-DA Module
・dynabook RJ74/KU
Arduino UNOは純正品です。ESP32は先輩からいただいた怪しいやつで、BOOTボタン押しっぱにしないと書き込めないくせ者です。
ライブラリなど
ESP32をArduino IDEで使うためにesp32ライブラリみたいなものをダウンロードすると思いますが、これのバージョンは3.0以上にしてください。3.0以降からpwm出力の構文が変わったので、古いバージョンのライブラリを使用しているとコンパイルが通りません。古いバージョンの場合は、「esp32 pwm」でググって出てきた記事を参考にして適宜コードを修正してください。
Processingでエンジン音を出力するために、car.mp3という音声ファイルを使用していますが、著作権的な問題で、音声ファイルの配布は致しません。車のアイドリング音を1分ほど録音したものになります。
Arduinoのコード
//ピンの設定------------------------------------------------------------------------------
#define handle_pin A0 //ハンドルのピン
#define accelerator_pin A1 //アクセルのピン
#define brake_pin A2 //ブレーキのピン
#define reverse_pin A3 //シフトノブ(z軸)のピン(使わない)
#define lateral_pin A4 //シフトノブ(x軸)のピン
#define Longitudinal_pin A5 //シフトノブ(y軸)のピン
#define mode_switch 3 //AT、MTの変更用スイッチのピン(1：MT、0：AT)
#define game_switch 4 //0-200のタイム計測モード変更用スイッチのピン(1：有効、0：無効)
#define tacho_pin 5 //タコメータ制御用のピン
//車両定数の定義----------------------------------------------------------------------------
#define pi 3.14159 //円周率
#define g 9.80665 //重力加速度
float torque_max = 22; //エンジン最高トルク[kgf・m]
float omega_max = 9000; //エンジン最高回転数[rpm]
float idling = 800; //アイドリング回転数[rpm]
float ke = 0; //逆起電力定数
float kt_R = 0; //トルク定数/抵抗値
float m_fly = 7.6; //フライホイール質量[kg]
float D_fly = 350; //フライホイール直径[mm]
float m_wheel = 21; //タイヤ質量[kg]
float D_wheel = 19; //タイヤ直径[インチ]
float I = 0; //フライホイール慣性モーメント[kg・m^2]
float J = 0; //車体慣性モーメント[kg・m^2]
float M = 1350; //車体質量[kg]
//float theta = 0; //傾斜[rad]
float r = 0; //タイヤ半径
float Tb_max = 4000; //ブレーキの最大制動トルク[N・m]
float Tf = 10; //タイヤの転がり摩擦トルク[N・m]
float handle_angle_max = 150; //舵角の最大値[deg]
float V_max = 0; //最大車輪速度[m/s](正規化に使用)
float W = 50; //ロボットの横幅[mm]
float L = 100; //ロボットの縦幅[mm]
float kG[7] = {1, 15.5, 9.0, 6.2, 4.4, 3.6, 14.6}; //ギア比[N,1,2,3,4,5,R=14.6]
int dt = 5; //制御周波数[ms]
float shift_up_omega = 7500; //シフトアップの目安回転数
float shift_down_omega = 4000; //シフトダウンの目安回転数
//変数の定義---------------------------------------------------------------------------------------
int Gear_No = 0; //ギア番号
int AT_state = 0; //AT状態表示
float omega = 0; //エンジン回転数[rad/s]
float OMEGA = 0; //タイヤ回転数[rad/s]
float V = 0; //車両速度[m/s]
int R_flag = 1; //R判定用フラグ
float handle_angle = 0; //舵角[rad]
int pwm_r = 0; //右車輪移動速度[m/s]
int pwm_l = 0; //左車輪移動速度[m/s]
float accel_value = 0; //アクセルの値(0~1)
float Tb = 0; //ブレーキの制動トルク[N・m]
float odometry = 0; //走行距離を保持するための変数 [km]
//0-200ゲームの変数の定義----------------------------------------------------------------------------
unsigned long start_time = 0;
bool flag_measure_time = 0;
bool flag_measure_finish = 0;
unsigned long measured_time = 0;
float ref_speed = 180; //ゲームの目標速度[km/h]
//セットアップ---------------------------------------------------------------------------------------
void setup() {
//ピンのセット-----------------------------------------------------
pinMode(handle_pin,INPUT);
pinMode(accelerator_pin,INPUT);
pinMode(brake_pin,INPUT);
pinMode(reverse_pin,INPUT);
pinMode(lateral_pin,INPUT);
pinMode(Longitudinal_pin,INPUT);
pinMode(mode_switch,INPUT_PULLUP);
pinMode(game_switch,INPUT_PULLUP);
Serial.begin(115200);
//定数計算・単位変換------------------------------------------------
torque_max = torque_max*g; //エンジン最高トルク[N]
omega_max = omega_max*pi/30; //エンジン最高回転数[rad/s]
idling = idling*pi/30; //アイドリング回転数[rad/s]
ke = 1/omega_max; //逆起電力定数
kt_R = torque_max; //定数kt/R トルク定数/抵抗値
I = m_fly*pow(D_fly/1000,2)/8; //フライホイール慣性モーメント[kgm^2](質量7.6kg、直径0.35m)I=質量*(直径)^2/8
r = D_wheel*0.0254/2; //タイヤ半径
J = M*r*r + 4*0.5*m_wheel*r*r; //タイヤ慣性モーメント[kgm^2](タイヤ4本を仮定)
W = W*0.001; //車輪間距離[m]
L = L*0.001; //ロボットの縦幅[m]
V_max = (1 + W*tan(handle_angle_max/2)/L)*omega_max*r/kG[5];
}
//ループ---------------------------------------------------------------------------------------
void loop() {
if(digitalRead(mode_switch)){
MT_mode();
}else{
AT_mode();
}
}
//デューティー比の計算-------------------------------------------------------------
void calc_pwm(){
pwm_r = (1 + W*tan(handle_angle/2)/L)*V*130/V_max + R_flag*55;
pwm_l = (1 - W*tan(handle_angle/2)/L)*V*130/V_max + R_flag*55;
if(pwm_r>255)pwm_r = 255;
if(pwm_l>255)pwm_l = 255;
}
//ATモード----------------------------------------------------------------------
void AT_mode(){
if(millis() >= dt){
get_accel(); //アクセルの値を取得
get_brake(); //ブレーキの値を取得
get_handle(); //ハンドルの値を取得
if(get_gear_AT()){
shock();
}
if(Gear_No){
G_mode();
}else{
N_mode();
}
calc_pwm();
meter();
}
}
//MTモード-----------------------------------------------------------------------
void MT_mode(){
if(millis() >= dt){
get_accel();
get_brake();
get_handle();
if(get_gear()){
shock();
}
if(Gear_No){
G_mode();
}else{
N_mode();
}
calc_pwm();
meter();
}
}
//ニュートラルモード---------------------------------------------------------------
void N_mode(){
//if(flag_measure_finish) measured_time = 0;
omega += (accel_value - ke*omega)*0.001*dt*kt_R/I; //エンジン回転数の計算
if(omega < idling){ //アイドリング判定
omega = idling;
}
OMEGA -= (Tf + Tb)*dt*0.001/J;//OMEGA -= (Tf + Tb)*dt*0.001/r/M; //タイヤ回転数の計算
if(OMEGA < 0){ //停止判定
OMEGA = 0;
}
V = OMEGA*r; //車両速度の計算
}
//ギアードモード------------------------------------------------------------------
void G_mode(){
omega += ((accel_value - ke*omega)*kt_R*kG[Gear_No] - Tb - Tf)*kG[Gear_No]*dt*0.001/J; //エンジン回転数の計算
if(omega < idling){ //アイドリング判定
omega = idling;
}
OMEGA = R_flag*omega/kG[Gear_No];
V = OMEGA*r;
if(3.6*V >= ref_speed) {
flag_measure_time = 0;
flag_measure_finish = 1;
}
}
/*ATギアーモード------------------------------------------------------------------
void Auto_Gear_Change_mode(){
omega += ((accel_value - ke*omega)*kt_R*kG[Gear_No] - Tb - Tf)*kG[Gear_No]*dt*0.001/J; //エンジン回転数の計算
if(omega < idling){ //アイドリング判定
omega = idling;
}
OMEGA = R_flag*omega/kG[Gear_No];
V = OMEGA*r;
}
*/
//シフトチェンジ-----------------------------------------------------------------------------------------------------------------------------------------------------
void shock(){
omega = (I*kG[Gear_No]*omega + J*OMEGA)*kG[Gear_No]/(I*kG[Gear_No]*kG[Gear_No] + J);
OMEGA = omega/kG[Gear_No];
V = OMEGA*r;
if(Gear_No == 6){
omega = 0;
V = 0;
}
}
//メーターの表示------------------------------------------------------------------
void meter(){
float omega_print = 30*omega/pi; //エンジン回転数[rpm]
float V_print = V*3.6; //車両速度[km/h]
float tacho_print = abs(omega_print)/20; //タコメータ表示用周波数[Hz]
if(tacho_print > 500) tacho_print = 500; //メータの振れの限界値設定
tone(tacho_pin,tacho_print); //タコメータの表示
float freq = omega/idling; //再生速度
odometry += V*dt/1000000;
//processingとの通信
Serial.print(pwm_r);
Serial.print(",");
Serial.print(pwm_l);
Serial.print(",");
Serial.print(abs(V_print));
Serial.print(",");
if(Gear_No == 0)Serial.print('N'); //シフトの表示
else if(Gear_No == 6)Serial.print('R');
else{
if(digitalRead(mode_switch))Serial.print(Gear_No);
else Serial.print('D');
}
Serial.print(",");
Serial.print(freq);
Serial.print(",");
if(flag_measure_time*digitalRead(game_switch))measured_time = millis() - start_time;
Serial.print(measured_time);
Serial.print(",");
Serial.println(odometry);
}
//ハンドルの値を取得---------------------------------------------------------------
void get_handle(){
handle_angle = (-analogRead(handle_pin) + 500)*0.00003878*handle_angle_max;
}
//アクセルの値を取得----------------------------------------------------------------
void get_accel(){
accel_value = (analogRead(accelerator_pin) - 142)*0.00138;
return 0;
}
//ブレーキの値を取得-----------------------------------------------------------------
void get_brake(){
Tb = (analogRead(brake_pin) - 200)*0.00145*Tb_max;
if(Tb < 0){
Tb = 0;
}
return 0;
}
//シフト番号を取得-------------------------------------------------------------------
int get_gear(){
R_flag = 1;
int x = (analogRead(lateral_pin) - 500)/150;
int y = (analogRead(Longitudinal_pin) - 500)/300;
int last_Gear_No = Gear_No;
Gear_No = abs(y)*(2*(x + 2)-(y > 0)); //Nの場合は0を返す
if(Gear_No == 6){ //Rに入っていたらR_flagを-1にする
R_flag = -1;
}
int pulse = (abs(Gear_No - last_Gear_No) > 0&&Gear_No > 0); //ギアがNから入ったら1を返す
if(pulse*Gear_No == 1){
measured_time = 0;
flag_measure_time = 1;
flag_measure_finish= 0;
start_time = millis();
}
return pulse;
}
//ATモードのシフト番号を取得-------------------------------------------------------------------
int get_gear_AT(){
R_flag = 1;
int x = (analogRead(lateral_pin) - 500)/150;
int y = (analogRead(Longitudinal_pin) - 500)/300;
int last_AT_state = AT_state; //シフトノブの本来の値を保持する変数
int last_Gear_No = Gear_No; //制御用のギア比の値を保持する変数
AT_state = abs(y)*(2*(x + 2)-(y > 0)); //シフトノブの本来の値を格納する変数。Nの場合は0を返す。
if(AT_state == 6){
R_flag = -1; //シフトノブがRに入っていたらR_flagを-1にする
Gear_No = 6; //制御用ギア比をRに統一
}
if(AT_state == 0){
Gear_No = 0; //Nに入っていたら、制御用ギアもNに統一する
//measured_time = 0; //計測タイムのリセット
}
int pulse_AT = (abs(AT_state - last_AT_state) > 0&&AT_state > 0); //ギアがNから入ったら1を返す。ここではどこに入ってもDとする。
if(pulse_AT){
Gear_No = 1; //ギアがDに入ったら、内部では1速に統一される
measured_time = 0; //計測タイムのリセット
flag_measure_time = 1; //タイム計測を行うフラグ
flag_measure_finish = 0;
start_time = millis(); //スタートしたときのタイムを記憶する
}
//Rに入っていない時に限り、オートシフトチェンジを行う
if(R_flag == 1 && Gear_No > 0){
if(Gear_No < 5 && 30*omega/pi > shift_up_omega)Gear_No++; //シフトアップ
if(Gear_No > 1 && 30*omega/pi < shift_down_omega)Gear_No--; //シフトダウン
}
int pulse = (abs(Gear_No - last_Gear_No) > 0&&Gear_No > 0); //ソフトチェンジが発生したら1を返す
return pulse;
}
Processingのコード
//Arduinoからシリアル通信でカンマで区切られた2つのデータを読み取り、ESP32へ送信するコード
//セットアップ関数のポート番号は変わる可能性があるので注意!
//デバッグ用にESPポートの部分はコメントアウトしてます(セットアップとdrawの1行目)
import processing.serial.*;
import processing.sound.*;
SoundFile player;
SoundFile player2;
SoundFile player3;
SoundFile player4;
SoundFile player5;
SoundFile player6;
SoundFile player7;
SoundFile player8;
Serial arduinoPort; //Arduino
Serial espPort; // espとのbluetooth通信ポート
String pwm_R; //データ格納用グローバル変数
String pwm_L; //データ格納用グローバル変数
float Angle;
float V;
float idling_rate = 1;
String Gear = "N";
String measured_time = "0";
String odometry = "0";
String V_text = "0";
//セットアップ関数--------------------------------------------------------------------------------------
void setup() {
espPort = new Serial(this, "COM9", 115200); // "COMx" はespのbluetoothポート番号に置き換える
arduinoPort = new Serial(this, "COM10", 115200 ); // "COMx" はArduinoのURATポート番号に置き換える
size(1500, 900); //メータ表示用ウィンドウを生成
smooth(); //呪文
frameRate(100); //フレームレートの設定
//音重ねて再生
player = new SoundFile(this, "car.mp3");
player.play();
player.loop();
player2 = new SoundFile(this, "car.mp3");
player2.play();
player2.loop();
player3 = new SoundFile(this, "car.mp3");
player3.play();
player3.loop();
player4 = new SoundFile(this, "car.mp3");
player4.play();
player4.loop();
player5 = new SoundFile(this, "car.mp3");
player5.play();
player5.loop();
player.amp(0.8);
player2.amp(0.6);
player3.amp(0.5);
player4.amp(0.8);
player5.amp(0.6);
}
//描画用関数--------------------------------------------------------------------------------------------
void draw() {
//音職人
player.rate(idling_rate);
player2.rate(0.6*idling_rate);
player3.rate(0.1*idling_rate);
player4.rate(V*0.04+0.01);
player5.rate(V*0.1+0.01);
background(50); //背景を黒色で描画
// メーター軸の描画-------------------------------------------------------------
stroke(0); //線の色を黒に設定
fill(0); //黒で塗りつぶす
ellipse(width/2, height - 40, 150, 150); //黒線・黒塗りの円の描画
noFill(); //塗りつぶしを解除
// メータの円弧の描画-----------------------------------------------------------
strokeWeight(200); //線の太さを50pに設定
stroke(80); //線の色を灰色に設定
arc(width/2, height, 500*2, 500*2, PI+radians(0), PI+HALF_PI+radians(90)); //内側の円弧を描画
strokeWeight(10); //線の太さを10pに設定
stroke(250); //線の色を白色に設定
arc(width/2, height, 600*2, 600*2, PI+radians(0), PI+HALF_PI+radians(90));//arc(width/2, height-40, 100*2, 100*2, PI+radians(10), PI+HALF_PI+radians(80));
// 目盛の描画------------------------------------------------------------------
strokeWeight(10); //線のサイズを10pに設定
stroke(250); //線の色を白に設定
textSize(70); //文字サイズを70pに設定
for(int i = 170; i >= 10; i -= 20){ //20[deg]ごとに刻む
line(560*cos(radians(i))+width/2, 560*sin(-radians(i))+height, 600*cos(radians(i))+width/2, 600*sin(-radians(i))+height); //メータの刻みの表示
fill(250); //塗りつぶし(文字)色を白に設定
text(int(1.5*(170 - i)), 480*cos(radians(i))+width/2 - 35, 480*sin(-radians(i))+height); //メータの数字の描画
noFill(); //塗りつぶしを解除
}
// 細かい目盛の描画--------------------------------------------------------------
strokeWeight(3); //線のサイズを3pに設定
stroke(250); //線の色を白に設定
for(int i = 170; i >= 10; i -= 2){ //20[deg]ごとに刻む
line(580*cos(radians(i))+width/2, 580*sin(-radians(i))+height, 600*cos(radians(i))+width/2, 600*sin(-radians(i))+height); //メータの刻みの表示
}
// シフトの表示-------------------------------------------------------------------
textSize(200);
text(Gear, 150, 250); //メータの数字の描画
//タイムの表示
textSize(180);
text(measured_time, 900, 200); //タイムの描画
textSize(140);
text("s", 1400, 200); //タイムの描画
//走行距離の表示
textSize(80);
text(odometry, 1100, 350); //タイムの描画
textSize(40);
text("km", 1400, 350); //タイムの描画
//速度の表示
fill(180); //線の色を白に設定
textSize(90);
text(V_text, 600, 700); //速度の描画
textSize(40);
text("km/h", 850, 700); //タイムの描画
// 針の動作の描画-----------------------------------------------------------------
stroke(255, 0, 0, 230); //線の色を赤色に設定
strokeWeight(20); //線の太さを20pに設定
Angle = 174 - V*0.7; //針の角度を計算
line(width/2, height-40, 580*cos(radians(Angle))+width/2, 580*sin(-radians(Angle))+height-40); //針の描画
espPort.write(pwm_R + ',' + pwm_L + ';'); //ESP32へデータを送信
}
//Arduinoからのシリアル通信によって発生する処理---------------------------------------------------------
void serialEvent(Serial arduinoPort) {
if ( arduinoPort.available() > 0 ) { //データが存在する場合処理を続行
String datas = arduinoPort.readStringUntil('\n'); //改行されるまでデータを取得
if ( datas != null ) { //データがNULLでない場合に処理を続行
String[] val = split(trim(datas), ','); //データをカンマごとに区切って配列valに格納
if (val.length == 7) { //左右の車輪のpwm(データ量が2つ)が格納されていることを確認し続行
pwm_R = val[0]; //0番目の値をpwm_Rに格納
pwm_L = val[1]; //1番目の値をpwm_Lに格納
V = float(val[2]); //2番目の値をVに格納
Gear = val[3]; //3番目の値をGearに格納
idling_rate = float(val[4]); //4番目の値をidling_rateに格納
measured_time = nf(float(val[5])*0.001,2,3); //5番目の値をmeasured_timeに格納
odometry = nf(float(val[6]),5,2); //6番目の値をodometryに格納
V_text = nf(V,3,2);
//println(odometry);
}
}
}
}
/*player stop----------------------------------------------------------------------------------------------
void stop() {
player.stop();
super.stop();
}*/
ESP32のコード
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;
//マクロ定義---------------------------------------------
#define R_A_pin 25
#define R_B_pin 26
#define L_A_pin 27
#define L_B_pin 12
//データ格納用変数----------------------------------------
String datas = ""; //データ読み込み用
String pwm_R = ""; //右車輪デューティー比
String pwm_L = ""; //左車輪デューティー比
int duty_R = 0;
int duty_L = 0;
void setup() {
Serial.begin(115200);
SerialBT.begin("ESP32_car"); //Bluetooth device name を設定
//pwmのピン、周波数、分解能を設定
ledcAttach(R_A_pin, 490, 8);
ledcAttach(R_B_pin, 490, 8);
ledcAttach(L_A_pin, 490, 8);
ledcAttach(L_B_pin, 490, 8);
//SerialBT.println("setup_done!");
}
void loop() {
//SerialBT.println("aa");
/*
ledcWrite(R_A_pin, 100);
ledcWrite(R_B_pin, 0);
ledcWrite(L_A_pin, 250);
ledcWrite(L_B_pin, 0);
delay(100);
*/
//SerialBT.println("aa");
if (SerialBT.available() > 0) { //PCとの通信が成立している場合に処理を行う
datas = SerialBT.readStringUntil(';'); //';'までのデータを1セットとして読み取り、datasに格納
if (datas.length() > 0) { //datasに格納されたデータがNULLでない場合(長さを持つ場合)に処理を続行
processData(datas); //制御に関する自作関数
}
}
}
//'separator'でdatasを区切って、index番目のデータを取り出す自作関数(GPT)-----------------------------
String getValue(String datas, char separator, int index) {
int found = 0;
int strIndex[] = {0, -1};
int maxIndex = datas.length() - 1;
for (int i = 0; i <= maxIndex && found <= index; i++) {
if (datas.charAt(i) == separator || i == maxIndex) {
found++;
strIndex[0] = strIndex[1] + 1;
strIndex[1] = (i == maxIndex) ? i + 1 : i;
}
}
return found > index ? datas.substring(strIndex[0], strIndex[1]) : "";
}
//実機の制御を行う関数---------------------------------------------------------------------
void processData(String datas){
pwm_R = getValue(datas, ',', 0); //カンマでデータを区切って、0番目にあたるデータ(右pwm)を取得
pwm_L = getValue(datas, ',', 1); //カンマでデータを区切って、1番目にあたるデータ(左pwm)を取得
duty_R = pwm_R.toInt();
duty_L = pwm_L.toInt();
//右車輪の制御(channel 0,1:右車輪)
if(duty_R > 0){
ledcWrite(R_A_pin, 0);
ledcWrite(R_B_pin, abs(duty_R));
}else{
ledcWrite(R_A_pin, abs(duty_R));
ledcWrite(R_B_pin, 0);
}
//左車輪の制御(channel 2,3:左車輪)
if(duty_L > 0){
ledcWrite(L_A_pin, 0);
ledcWrite(L_B_pin, abs(duty_L));
}else{
ledcWrite(L_A_pin, abs(duty_L));
ledcWrite(L_B_pin, 0);
}
delay(2);
}