/* ESP32-WROOM-32でVS-RC003HVから送られてくるV-SERVO用シリアル信号をKONDOサーボ用シリアル信号に変換しつつPS4コントローラで操縦できるようにするスケッチ ハードラックス向けカスタム仕様 2024/03/23 Release 1 by 666 PS4コントローラをESP32で使えるようにするライブラリはこちらを使っています。 https://github.com/aed3/PS4-esp32 ESP32をSPI通信のスレーブにするライブラリはこちらを使っています。 バージョンは0.3.0です。こちらはArduinoIDEのライブラリマネージャから追加できます。 https://github.com/hideakitai/ESP32DMASPI/releases 　　VS-RC003HVファームウェア（バージョン1.000、リビジョン19）以上がV-SERVO用シリアル信号に対応しています。 上記より古いファームウェアのVS-RC003HVにはRobovieMaker2から最新のファームウェアを書き込んでください。 https://www.vstone.co.jp/products/vs_rc003hv/download.html#04 VS-RC003HVのCH1-1に接続されているのがKONDOサーボのID:0という扱いなります。CH5-6がID:29です。 奇数IDと偶数IDで系統が分割されています。サーボ側のID設定に注意してください。 ※このスケッチで起こった事故・破損などについては一切の責任を追うことができませんので，ご了承ください。 */ #include #include #include "esp_bt_main.h" #include "esp_bt_device.h" #include "esp_gap_bt_api.h" #include "esp_err.h" //ESP32DMASPISlaveのスレーブ設定 ESP32DMASPI::Slave slave; static const int MSG_SIZE = 16; //送受信するバイト数の設定 uint8_t* ps2_tx; //SPI通信送信用データ格納用変数 uint8_t* ps2_rx; //SPI通信受信用データ格納用変数 #define GAIN -2.5968 //Vサーボの変化量をKRSサーボの変化量に変化させるときの比率 int pos[30]; //KRSサーボの位置情報格納変数 int last_pos[30]; //KRSサーボの最終位置情報格納変数 int last_flag[30]; //KRSサーボの最終位置一致情報格納変数 int serial_out_time; //KRSサーボのシリアル送信周期格納変数(us) byte send_id = 0; //送信IDの格納変数 hw_timer_t* ics_timer = NULL; //タイマーの宣言 byte color_flag = 127; //色情報ループ用変数 int count = 0; //送信管理カウンタ bool Last_PSButton = false; //PSボタンの押し情報格納変数 void IRAM_ATTR onTimer() { if (send_id <= 30) { //もし送信IDが30以下なら if (pos[send_id] == 1) sync_spd(send_id, 127); //スピード送信フラグが立っていたら強制的にスピード１２７を送信 else if (pos[send_id] == 2) sync_spd(send_id, 60); //ストレッチ送信フラグが立っていたら強制的にストレッチ６０を送信 else sync_pos(send_id, pos[send_id]); //位置情報を送信 } send_id++; //送信IDを次のIDにする if (send_id >= 31) { //もし送信IDが31以上になったら count++; //バッテリ状態管理カウンタを１増やす send_id = 0; //送信ID controll_data_make(); //コントローラ情報を組み立てる if (count >= 10) { //バッテリ状態管理カウンタが62以上になったら if (PS4.isConnected()) { //コントローラが接続されていてバッテリ情報が更新されたら if (color_flag == 127) { //色情ループ変数が１２７（電源投入時）なら PS4.setLed(44, 255, 164); //翡翠色を設定 PS4.sendToController(); //設定した情報をコントローラに送信 color_flag = 0; //色情ループ変数を０にする } if (PS4.PSButton()) { //もしPSボタンが押されていたら if (Last_PSButton == false) { //もしPSボタンが押さた瞬間だったら color_flag++; //色情報ループ変数を１増やす if (color_flag == 0) PS4.setLed(44, 255, 164); //色情報ループ変数が０なら翡翠色に設定 else if (color_flag == 1) PS4.setLed(255, 50, 50); //色情報ループ変数が１ならピンクゴールドに設定 else if (color_flag == 2) PS4.setLed(30, 30, 255); ///色情報ループ変数が２ならライトブルーに設定 else if (color_flag == 3) PS4.setLed(255, 255, 50); //色情報ループ変数が３ならパステルイエローに設定 else if (color_flag == 4) PS4.setLed(200, 200, 200); //色情報ループ変数が４ならペーパーホワイトに設定 if (color_flag >= 4) color_flag = -1; //もし色情報ループ変数が最後だったら最初に戻る PS4.sendToController(); //設定した情報をコントローラに送信 Last_PSButton = true; //押された瞬間を終えたことを記録する } } else { Last_PSButton = false; //PSボタンが離されたことを記憶する } } count = 0; //送信管理カウンタをリセットする } } } void setup() { //PS4コントローラと通信開始 PS4.begin(); //正常にPS4コントローラが接続されるようにESP32のペアリング情報をいったん全削除する uint8_t pairedDeviceBtAddr[20][6]; int count_bt = esp_bt_gap_get_bond_device_num(); esp_bt_gap_get_bond_device_list(&count_bt, pairedDeviceBtAddr); for (int i = 0; i < count_bt; i++) { esp_bt_gap_remove_bond_device(pairedDeviceBtAddr[i]); } //5Vを受けるピンをハイインピーダンスに設定 pinMode(21, INPUT); //VSRCと接触する未使用ピンをすべてハイインピーダンスに設定 pinMode(26, INPUT); pinMode(39, INPUT); //SVN pinMode(18, INPUT); pinMode(35, INPUT); pinMode(2, INPUT); //35はINPUTONLYなので2をバイパス pinMode(19, INPUT); pinMode(33, INPUT); pinMode(23, INPUT); pinMode(34, INPUT); pinMode(5, INPUT); pinMode(14, INPUT); //TMS //FET接続ピンをすべてハイインピーダンスに設定 pinMode(22, INPUT); pinMode(25, INPUT); pinMode(32, INPUT); //FET接続ピンをすべてハイインピーダンスに設定 pinMode(17, INPUT); pinMode(16, INPUT); pinMode(4, INPUT); //KONDOサーボ起動待ち delay(500); //FETを疑似的にソース接地させるためにピンをLOWに設定 pinMode(17, OUTPUT); digitalWrite(17, LOW); pinMode(25, OUTPUT); digitalWrite(25, LOW); //信号出力ピンをOUTPUTに設定 pinMode(16, OUTPUT); pinMode(22, OUTPUT); //SPI通信でDMAバッファを使うように設定 ps2_tx = slave.allocDMABuffer(MSG_SIZE); ps2_rx = slave.allocDMABuffer(MSG_SIZE); //SPI通信の送受信バッファをリセットする memset(ps2_tx, 0, MSG_SIZE); memset(ps2_rx, 0, MSG_SIZE); //SPI通信でVS-RCにPC2コントローラ情報を送るための変数の初期化 ps2_tx[0] = 0xFF; //0xFF固定 ps2_tx[1] = 0x73; //アナログモードで送信 ps2_tx[2] = 0x5A; //0x5A固定 ps2_tx[3] = 0b11111111; //左,下,右,,上,START,R3,L3,SELECT ps2_tx[4] = 0b11111111; //□,×,〇,△,R1,L1,R2,L2 ps2_tx[5] = 0x80; //右スティック左右 ps2_tx[6] = 0x80; //右スティック前後 ps2_tx[7] = 0x80; //左スティック左右 ps2_tx[8] = 0x80; //左スティック前後 //SPI通信の設定 slave.setDataMode(SPI_MODE0); //SPI通信のモード設定。DualShock2はMODE3のはずだが何故かVS-RCとはMODE0で通信する。 slave.setMaxTransferSize(MSG_SIZE); //SPI通信の最大送信バイト数 slave.setDMAChannel(2); //専用メモリの割り当て(1or2) slave.setQueueSize(1); //キューサイズ slave.setSlaveFlags(SPI_SLAVE_BIT_LSBFIRST); //LSBが最初になる並びで送受信するように設定。 slave.begin(HSPI, 19, 2, 18, 33); //HSPIを使って通信するように設定（VSPIでも可） //位置情報格納変数の初期化 for (byte i = 0; i < 30; i++) pos[i] = 0; //最終位置情報格納変数の初期化 for (byte i = 0; i < 30; i++) last_pos[i] = 0; //最終位置一致情報格納変数の初期化 for (byte i = 0; i < 30; i++) last_flag[i] = true; //VS-RC003HVからV-SERVO信号を受けるためのハードウェアシリアル設定 Serial2.begin(115200, SERIAL_8N1, 39, 27, false, 256); //（通信速度,パリティ設定,受信ピン,送信ピン,ロジック反転,リングバッファ） Serial1.begin(115200, SERIAL_8E1, 13, 22, true, 256); //（通信速度,パリティ設定,受信ピン,送信ピン,ロジック反転,リングバッファ） Serial.begin(115200, SERIAL_8E1, 1, 16, true, 256); //（通信速度,パリティ設定,受信ピン,送信ピン,ロジック反転,リングバッファ） //KONDOサーボへ定期的に信号を送るための割り込みタイマーの設定 ics_timer = timerBegin(0, getApbFrequency() / 1000000, true); //ESP32のタイマー0を使う,1カウント1マイクロ秒に設定,カウント方式を上昇に設定 timerAttachInterrupt(ics_timer, &onTimer, true); //ics_timerが条件を満たすとonTimer関数を割り込みで呼び出すように設定 timerAlarmWrite(ics_timer, 400, true); //400マイクロ秒ごとに割り込みが発生するようにする serial_out_time = 400; //シリアル送信周期が400であることを記憶する timerAlarmEnable(ics_timer); //タイマーを有効にする } void loop() { int i; //繰り返し用変数 int last_flag_count; int last_out_count = 0; byte id; //ID保存変数 byte rx[4]; //VS-RCから受信した値を格納する変数 while (1) { //無限ループ if (slave.remained() == 0) { // slave.queue(ps2_rx, ps2_tx, MSG_SIZE); //トランザクション(SPI通信ひとまとまり分)の開始 } if (Serial2.available() != 0) { //VS-RCからV-SERVO信号が到着するのを待つ rx[0] = Serial2.read(); //１バイト読み込む if (rx[0] >= 0xC0) { //もし配列の中身がID情報なら id = rx[0] - 0xC0; //idという変数にV-SERVOのID情報を格納する while (Serial2.available() == 0) {}; //VS-RCからV-SERVO信号が到着するのを待つ rx[1] = Serial2.read(); //rx[1]に変数にV-SERVOの位置情報（下位８ビット）を格納する while (Serial2.available() == 0) {}; //VS-RCからV-SERVO信号が到着するのを待つ rx[2] = Serial2.read(); //rx[2]に変数にV-SERVOの位置情報（上位８ビット）を格納する while (Serial2.available() == 0) {}; //VS-RCからV-SERVO信号が到着するのを待つ rx[3] = Serial2.read(); //rx[3]に変数にV-SERVOの脱力情報を格納する if (rx[3] == 0) { //そのIDが脱力状態なら pos[id] = 0; //一時変数に脱力を設定する } else { //そのIDが脱力状態でなければ pos[id] = (int)rx[2] * 128 + (int)rx[1]; //一時変数に位置情報を組み立てる pos[id] = pos[id] - 2048; //「サーボの左右中心がゼロ度」の時に「送信される値」がゼロになるように値を調整する pos[id] = 7500.0 + pos[id] * GAIN; //KONDOサーボに都合の良い値に倍率を調整し，それをKODNOサーボの左右中心の7500に加算する if (pos[id] > 11500) pos[id] = 1; //もし送信される値が上限限界を超えていたら，スピード送信フラグを立てて、脱力に設定する。 else if (pos[id] < 3500) pos[id] = 2; //もし送信される値が下限限界を超えていたら，ストレッチ送信フラグを立てて、脱力に設定する。 } if (last_pos[id] != pos[id]) { //もし最後に送った位置情報と今受け取った位置情報が異なるなら last_flag[id] = 1; //位置情報更新フラグを１（更新された）にする last_pos[id] = pos[id]; //最終位置情報を更新する } else { //そうでなければ last_flag[id] = 0; //位置情報更新フラグを０（更新されていない）にする } } } while (slave.available()) { slave.pop(); //トランザクションの終了 } if (!PS4.isConnected()) { //もしPS4コントローラが接続されていなければ last_flag_count = 0; //位置情報更新フラグを集計する変数を０に初期化する for (i = 0; i < 30; i++) last_flag_count += last_flag[i]; //位置情報が更新されたかどうか集計する if (last_flag_count == 0) { //すべてのサーボの位置情報が更新されていなければ last_out_count++; //位置情報が更新されなかった回数をカウントする変数に１を足す if (last_out_count >= 100) { //１００回連続で送信されなければ last_out_count = 100; //オーバーフロー防止のため１００に更新する if (serial_out_time != 30000) { //もしシリアル送信周期が30000でなければ timerAlarmWrite(ics_timer, 30000, true); //シリアル送信周期を30000usにする（コントローラ通信待機モード） serial_out_time = 30000; //シリアル送信周期が30000であることを記憶する } } } else { //位置情報が更新されていれば last_out_count = 0; //位置情報が更新されなかった回数をカウントする変数を初期化する if (serial_out_time != 400) { //もしシリアル送信周期が400でなければ timerAlarmWrite(ics_timer, 400, true); //シリアル送信周期を400usにする（通常送信モード） serial_out_time = 400; //シリアル送信周期が400であることを記憶する } } } else { //PS4コントローラが接続されていれば if (serial_out_time != 400) { ////もしシリアル送信周期が400でなければ timerAlarmWrite(ics_timer, 400, true); //シリアル送信周期を400usにする（通常送信モード） serial_out_time = 400; //シリアル送信周期が400であることを記憶する } } } } void sync_pos(byte target, int value) { //ポジション送信関数 byte tx[3]; //送信用配列の宣言 if (target < 0 || target > 31) target = 30; //範囲外のIDの場合，強制的にID30に信号送信 if (value < 3500 || value > 11500) value = 0; //範囲外のポジションの場合，強制的に脱力を送信 tx[0] = 0x80 + target; //ID情報の組み立て tx[1] = (byte)((value & 0x3F80) >> 7); //ポジション情報（上位）の組み立て tx[2] = (byte)(value & 0x007F); //ポジション情報（下位）の組み立て if (target % 2 == 0) { //送信する先が偶数であれば Serial1.write(tx, 3); //偶数セグメントにポジション情報を送信する。 } else { //そうでなければ Serial.write(tx, 3); //奇数セグメントにポジション情報を送信する。 } } void sync_str(byte target, int value) { //ストレッチ送信関数 byte tx[3]; //送信用配列の宣言 if (target < 0 || target > 31) target = 30; //範囲外のIDの場合，強制的にID30に信号送信 if (value < 1 || value > 128) value = 127; //範囲外のストレッチの場合，強制的にストレッチ最大を送信 tx[0] = 0xC0 + target; //ID情報の組み立て tx[1] = 0x01; //書き込みするパラメータをストレッチに指定 tx[2] = value; //ストレッチ情報の組み立て if (target % 2 == 0) { //送信する先が偶数であれば Serial1.write(tx, 3); //偶数セグメントにストレッチ情報を送信する。 } else { //そうでなければ Serial.write(tx, 3); //奇数セグメントにストレッチ情報を送信する。 } } void sync_spd(byte target, int value) { //スピード送信関数 byte tx[3]; //送信用配列の宣言 if (target < 0 || target > 31) target = 30; //範囲外のIDの場合，強制的にID30に信号送信 if (value < 1 || value > 128) value = 127; //範囲外のスピードの場合，強制的にスピード最大を送信 tx[0] = 0xC0 + target; //ID情報の組み立て tx[1] = 0x02; //書き込みするパラメータをスピードに指定 tx[2] = value; //スピード情報の組み立て if (target % 2 == 0) { //送信する先が偶数であれば Serial1.write(tx, 3); //偶数セグメントにスピード情報を送信する。 } else { //そうでなければ Serial.write(tx, 3); //奇数セグメントにスピード情報を送信する。 } } void controll_data_make() { //仮に格納する変数の初期化 byte button_data_1 = 0b11111111; //左,下,右,上,START,R3,L3,SELECT byte button_data_2 = 0b11111111; //□,×,〇,△,R1,L1,R2,L2 byte stick_data_RX = 0x80; //右スティック左右 byte stick_data_RY = 0x80; //右スティック前後 byte stick_data_LX = 0x80; //左スティック左右 byte stick_data_LY = 0x80; //左スティック前後 if (PS4.isConnected()) { //PS3の入力状況をPS2のコントローラ情報に成形 if (PS4.Share() || PS4.Touchpad()) button_data_1 -= 1; if (PS4.L3()) button_data_1 -= 2; if (PS4.R3()) button_data_1 -= 4; if (PS4.Options()) button_data_1 -= 8; if (PS4.Up() || PS4.UpRight() || PS4.UpLeft()) button_data_1 -= 16; if (PS4.Right() || PS4.UpRight() || PS4.DownRight()) button_data_1 -= 32; if (PS4.Down() || PS4.DownRight() || PS4.DownLeft()) button_data_1 -= 64; if (PS4.Left() || PS4.UpLeft() || PS4.DownLeft()) button_data_1 -= -128; if (PS4.L2()) button_data_2 -= 1; if (PS4.R2()) button_data_2 -= 2; if (PS4.L1()) button_data_2 -= 4; if (PS4.R1()) button_data_2 -= 8; if (PS4.Triangle()) button_data_2 -= 16; if (PS4.Circle()) button_data_2 -= 32; if (PS4.Cross()) button_data_2 -= 64; if (PS4.Square()) button_data_2 -= -128; if (abs(PS4.RStickX()) > 9) stick_data_RX = PS4.RStickX() - 128; if (abs(PS4.RStickY()) > 9) stick_data_RY = 127 - PS4.RStickY(); if (abs(PS4.LStickX()) > 9) stick_data_LX = PS4.LStickX() - 128; if (abs(PS4.LStickY()) > 9) stick_data_LY = 127 - PS4.LStickY(); } //PC2コントローラ情報の変数に書き込み ps2_tx[3] = button_data_1; //左,下,右,上,START,R3,L3,SELECT ps2_tx[4] = button_data_2; //□,×,〇,△,R1,L1,R2,L2 ps2_tx[5] = stick_data_RX; //右スティック左右 ps2_tx[6] = stick_data_RY; //右スティック前後 ps2_tx[7] = stick_data_LX; //左スティック左右 ps2_tx[8] = stick_data_LY; //左スティック前後 }