/* VS-RC003HVから送られてくるV-SERVO用シリアル信号をKONDOサーボ用シリアル信号に変換するスケッチ 2022/06/16 Release 5.1 by 666 VS-RC003HVファームウェア（バージョン1.000，リビジョン19）以上がV-SERVO用シリアル信号に対応しています。 上記より古いファームウェアのVS-RC003HVにはRobovieMaker2から最新のファームウェアを書き込んでください。 https://www.vstone.co.jp/products/vs_rc003hv/download.html#04 KODNOサーボにソフトウェアシリアルで信号を送るにはパリティが付加できるライブラリが必要です。 SoftwareSerialParityをダウンロードし，解凍してlibrariesに入れてください(「Arudino ライブラリ　入れ方」とかで検索すればOK) https://github.com/ljbeng/SoftwareSerialParity/archive/refs/heads/master.zip VS-RC003HVのCH1-1に接続されているのがKONDOサーボのID:0という扱いなります。CH5-6がID:29です。 奇数IDと偶数IDで系統が分割されています。サーボ側のID設定に注意してください。 ストレッチ・スピ―ドを変更できる関数も用意してありますが，このスケッチでは使っていません活用できそうなら使ってください。 ポジションとストレッチを調整する関数を別のスケッチに持っていく場合はプログラム上部の@〜Eの部分もも持っていってあげてください。 また，ＫＯＮＤＯサーボに「シリアル専用」「ＰＷＭ禁止」を設定していない場合は，void setupでF・G・Hを必ず入れてください。 ※このスケッチで起こった事故・破損などについては一切の責任を追うことができませんので，ご了承ください。 */ #include #define SERVO_PIN_1 4 //@KONDOサーボ用シリアル信号出力ピン設定 #define SERVO_PIN_2 5 //AKONDOサーボ用シリアル信号出力ピン設定 #define SERIAL_WAIT 124 //Bシリアル通信待ち時間（基本的には変更しないで下さい） SoftwareSerialParity puni_even(21, SERVO_PIN_1); //C偶数系統の宣言 SoftwareSerialParity puni_odd(21, SERVO_PIN_2); //D奇数系統の宣言 #define GAIN -2.5968 //VS-RC003HVの内部操作量とKONDOサーボの角度変化量の調整 void setup() { pinMode(SERVO_PIN_1, INPUT_PULLUP); //FKONDOサーボをシリアルモードで起動するために設定 pinMode(SERVO_PIN_2, INPUT_PULLUP); //GKONDOサーボをシリアルモードで起動するために設定 delay(1000); //HKONDOサーボをシリアルモードで起動するための待ち時間 Serial.begin(115200); //ハードウェアシリアルの開始 byte i; //繰り返し用変数 byte rx; //受信用変数 byte id; //ID情報用変数 byte POS_L; //位置情報下位８ビット用変数 byte POS_H; //位置情報上位８ビット用変数 byte FREE; //脱力情報用変数 int pos; //位置情報格納変数 for (i = 0 ; i <= 29; i++) sync_pos(i, 0); //全サーボに脱力送信 while (1) { while (Serial.available() == 0); //データが受信されるまで待つ rx = Serial.read(); //１バイト読み込む if (rx >= 0xC0) { //もし読み込んだデータがV-SERVOのID情報なら分岐 id = rx - 0xC0; //ID情報をIDそのものに変更 while (Serial.available() == 0); //データが受信されるまで待つ rx = Serial.read(); //１バイト読み込む if (rx < 0xC0) { //もし読み込んだデータがID情報じゃなければ（ID情報の次に位置情報が来てるなら）分岐 POS_L = rx; //読み込んだ位置情報を下位８ビットとして格納 while (Serial.available() == 0); //データが受信されるまで待つ rx = Serial.read(); //１バイト読み込む if (rx < 0xC0) {//もし読み込んだデータがID情報じゃなければ（ID情報の次に位置情報が来てるなら）分岐 POS_H = rx; //読み込んだ位置情報を上位８ビットとして格納 pos = (int)POS_H * 128 + (int)POS_L; //位置情報の上位ビットと下位ビットを結合 pos = pos - 2048; //「サーボの左右中心がゼロ度」の時に「送信される値」がゼロになるように値を調整する pos = 7500.0 + pos * GAIN; //KONDOサーボに都合の良い値に倍率を調整し，それをKODNOサーボの左右中心の7500に加算する while (Serial.available() == 0); //データが受信されるまで待つ rx = Serial.read(); //１バイト読み込む if (rx < 0xC0) {//もし読み込んだデータがID情報じゃなければ（ID情報の次に脱力情報が来てるなら）分岐 FREE = rx; //読み込んだ位置情報を脱力情報として格納 if (FREE == 0) pos = 0; //もし脱力状態ならば位置情報を脱力に設定 } } } } } } void loop() { } void sync_pos(byte target, int value) { //ポジション送信関数 byte tx[3]; //送信用配列の宣言 if (target < 0 || target > 31) target = 31; //範囲外のIDの場合，強制的にID31に信号送信 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) { //もし偶数IDなら pinMode(SERVO_PIN_1, OUTPUT); // 偶数サーボピンを出力に設定する puni_even.begin(123456, EVEN); //puni_evenという名前でパリティ対応ソフトウェアシリアルを開始する。123456bpsにするとなぜかうまくいく。EVENは偶数パリティ。 puni_even.write(tx, 3); //ポジション情報を送信する puni_even.flush(); //送信が完了するまで待つ puni_even.end(); //パリティ対応ソフトウェアシリアルを終了する pinMode(SERVO_PIN_1, INPUT_PULLUP); //偶数サーボピンをプルアップ抵抗が接続されている状態に設定 } else { //もし奇数IDなら pinMode(SERVO_PIN_2, OUTPUT); // 奇数サーボピンを出力に設定する puni_odd.begin(123456, EVEN); //puni_oddという名前でパリティ対応ソフトウェアシリアルを開始する。123456bpsにするとなぜかうまくいく。EVENは偶数パリティ。 puni_odd.write(tx, 3); //ポジション情報を送信する puni_odd.flush(); //送信が完了するまで待つ puni_odd.end(); //パリティ対応ソフトウェアシリアルを終了する pinMode(SERVO_PIN_2, INPUT_PULLUP); //奇数サーボピンをプルアップ抵抗が接続されている状態に設定 } delayMicroseconds(SERIAL_WAIT); //信号衝突を防ぐために一定時間待つ } void sync_strc(byte target, byte value) { //ストレッチ送信関数　例:　sync_strc(5,60)　/ID5のサーボをストレッチ60に設定する byte tx[3]; //送信用配列の宣言 if (target < 0 || target > 31) target = 31; //範囲外のIDの場合，強制的にID31に信号送信 if (value < 0 || value > 127) value = 60; //範囲外のストレッチの場合，強制的にストレッチ60を送信 tx[0] = 0xC0 + target; //ID情報の組み立て tx[1] = 0x01; //送信するパラメータをストレッチに設定 tx[2] = value; //送信するストレッチを設定 if (target % 2 == 0) { //もし偶数IDなら pinMode(SERVO_PIN_1, OUTPUT); // 偶数サーボピンを出力に設定する puni_even.begin(123456, EVEN); //puni_evenという名前でパリティ対応ソフトウェアシリアルを開始する。123456bpsにするとなぜかうまくいく。EVENは偶数パリティ。 puni_even.write(tx, 3); //パラメータ情報を送信する puni_even.flush(); //送信が完了するまで待つ puni_even.end(); //パリティ対応ソフトウェアシリアルを終了する pinMode(SERVO_PIN_1, INPUT_PULLUP); //偶数サーボピンをプルアップ抵抗が接続されている状態に設定 } else { //もし奇数IDなら pinMode(SERVO_PIN_2, OUTPUT); // 奇数サーボピンを出力に設定する puni_odd.begin(123456, EVEN); //puni_oddという名前でパリティ対応ソフトウェアシリアルを開始する。123456bpsにするとなぜかうまくいく。EVENは偶数パリティ。 puni_odd.write(tx, 3); //パラメータ情報を送信する puni_odd.flush(); //送信が完了するまで待つ puni_odd.end(); //パリティ対応ソフトウェアシリアルを終了する pinMode(SERVO_PIN_2, INPUT_PULLUP); //奇数サーボピンをプルアップ抵抗が接続されている状態に設定 } delayMicroseconds(SERIAL_WAIT); //信号衝突を防ぐために一定時間待つ } void sync_spd(byte target, byte value) { //スピード送信関数　例:　sync_spd(5,127)　/ID5のサーボをスピード127に設定する byte tx[3]; //送信用配列の宣言 if (target < 0 || target > 31) target = 31; //範囲外のIDの場合，強制的にID31に信号送信 if (value < 0 || value > 127) value = 127; //範囲外のスピードの場合，強制的にスピード127を送信 tx[0] = 0xC0 + target; //ID情報の組み立て tx[1] = 0x02; //送信するパラメータをスピードに設定 tx[2] = value; //送信するストレッチを設定 if (target % 2 == 0) { //もし偶数IDなら pinMode(SERVO_PIN_1, OUTPUT); // 偶数サーボピンを出力に設定する puni_even.begin(123456, EVEN); //puni_evenという名前でパリティ対応ソフトウェアシリアルを開始する。123456bpsにするとなぜかうまくいく。EVENは偶数パリティ。 puni_even.write(tx, 3); //パラメータ情報を送信する puni_even.flush(); //送信が完了するまで待つ puni_even.end(); //パリティ対応ソフトウェアシリアルを終了する pinMode(SERVO_PIN_1, INPUT_PULLUP); //偶数サーボピンをプルアップ抵抗が接続されている状態に設定 } else { //もし奇数IDなら pinMode(SERVO_PIN_2, OUTPUT); // 奇数サーボピンを出力に設定する puni_odd.begin(123456, EVEN); //puni_oddという名前でパリティ対応ソフトウェアシリアルを開始する。123456bpsにするとなぜかうまくいく。EVENは偶数パリティ。 puni_odd.write(tx, 3); //パラメータ情報を送信する puni_odd.flush(); //送信が完了するまで待つ puni_odd.end(); //パリティ対応ソフトウェアシリアルを終了する pinMode(SERVO_PIN_2, INPUT_PULLUP); //奇数サーボピンをプルアップ抵抗が接続されている状態に設定 } delayMicroseconds(SERIAL_WAIT); //信号衝突を防ぐために一定時間待つ }