【Rover C ProをObnizで動かす】遠隔操作できる自宅監視カメラ(コントローラー作成)
- 2021.06.14
- IoT

前回からの続きです.
Obnizサーバー上のコントローラーを操作→その入力値を取得→値を16進数に変換後、ラズパイ側(ペリフェラル)にBLE通信で送る→ラズパイ側で10進数へ再変換→モーターを動かす.
上記の流れについてその都度メモしていたものを雑記ですが今回まとめました.
スティックの押し具合をどのように値として操作機器(ラズパイ)に送るか、その考え方の参考にでもなれば幸いです.
コントローラーについて

x,y座標を以下のようにf、r、l、bとして細分化する.

コントローラーから取得した入力値を係数としてどのホイールをどのように回転(正転、逆転)させるか決める.
また、上図の座標においてマイナスの範囲が存在するものの、係数として使用する際には絶対値として用いる.

上記については以下で既に説明しております.
コントローラスティックで入力した値を4方向(f,l,r,b)の値へと変換する
プログラムを以下に記す.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 |
/** * 入力値をf,l,r,bに変換する * param {number} tempX:移動後x座標 * param {number} tempY:移動後x座標 */ function speedParameter(speedX, speedY){ var f, l, r, b = 0; var mapSpeedX = Math.floor(speedX / 10); var mapSpeedY = Math.floor(speedY/ 10); console.log("speedParameter mapSpeedX:" + mapSpeedX) console.log("speedParameter mapSpeedY:" + mapSpeedY) if (mapSpeedX == 0) { l = 0; r = 0; } else if(mapSpeedX < 0) { l = mapSpeedX; r = 0; } else if(mapSpeedX > 0) { l = 0; r = mapSpeedX; } if (mapSpeedY == 0) { f = 0; b = 0; } else if(mapSpeedY < 0) { f = 0; b = mapSpeedY; } else if(mapSpeedY > 0) { f = mapSpeedY; b = 0; } console.log("parseInt32ToByte(f, l, r, b)" + parseInt32ToByte(f, l, r, b)); return parseInt32ToByte(f, l, r, b); } |
また、ラズパイ側へ送る際は以下の関数で符号付きの16進数へと変換する.
1 2 3 4 5 6 7 8 9 10 11 12 |
/** * Uint型をバイナリデータに変換する */ function parseInt32ToByte(f, l, r, b){ var dataArray = new Array(4); dataArray[0] = "0x" + (f & 0xff).toString(16), 2; dataArray[1] = "0x" + (l & 0xff).toString(16), 2; dataArray[2] = "0x" + (r & 0xff).toString(16), 2; dataArray[3] = "0x" + (b & 0xff).toString(16), 2; console.log("parseInt32ToByte:" + f.toString(16)); return dataArray; } |
試しにラズパイ側へ送送ってみると以下のような結果を得ることができた.
(←:ラズパイ側、→:Obniz側)

上の結果におけるラズパイ側の処理(取得データの解析方法)を次に説明する
Obnizから取得したデータをラズパイ側で解析する
以下を参考にまずは符号つき16進数を10真数へと変換する
Pythonで符号付16進数データを正負の10進数に変換(データ解析前処理)
ただし、xor部分については0b11111110ではなく0b11111111、さらに+1する必要があった.(値が少し小さくなってしまうため)
以下のように訂正(変換したい値が4つあるため、dataOut_Decを配列にしている)
dataOut_Dec = (int(bin(int(dataIn_Hex, 0) ^ 0b11111110), 0)) * (-1)
↓
dataOut_dec.append((int(bin((int(data_hex, 0) ^ 0b11111111) + 1), 0)) * (-1))
変換したら、以下のように小数第一位の数値に変換およびf,b,l,rに代入する.
また、ここで更に10で割り、小数へと変換する.
1 2 3 4 5 6 7 8 9 10 11 12 13 |
# f,b,l,rに追加 def initDir(self, dataOut_dec): if len(dataOut_dec) == 4 : self.f = round(dataOut_dec[0] / 10, 1) self.l = round((dataOut_dec[1] / 10) * -1, 1) self.r = round(dataOut_dec[2] / 10, 1) self.b = round((dataOut_dec[3] / 10) * -1, 1) print("self.f :" + str(self.f)) print("self.l :" + str(self.l)) print("self.r :" + str(self.r)) print("self.b :" + str(self.b)) # モーター稼働 self.setSpeed() |
左前、右前、左後、右後の車輪のスピードを保持した「self.speed = [0,0,0,0]」に各方向のエネルギーの結果を代入する.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
def setSpeed(self): #各車輪のスピードを割り出す for index, item in enumerate(self.speed): # print("FORWARD :" + str(self.FORWARD[index])) self.speed[index] = self.FORWARD[index] * self.f self.speed[index] += self.BACKWARD[index] * self.b self.speed[index] += self.RIGHT[index] * self.r self.speed[index] += self.LEFT[index] * self.l self.speed[index] += self.ROTATE_R[index] * self.rl self.speed[index] += self.ROTATE_L[index] * self.rr # print("speed" + "[" + str(index) + "]:" + str(self.speed[index])) #スピードが100を越えないように丸め込む # self.speed[index] = self.speedParameter(self.speed[index]) # print("speed:" + str(self.speed)) speed_hex = [int(i) for i in self.speed] self.i2cWrite(speed_hex) |
そして最後に各モーターに書き込む.
1 2 3 4 5 6 7 8 9 10 11 |
#i2c書き込み def i2cWrite(self,data): # 左前 (7f = 127) self.i2c.writeReg8(self.roverC, 0x00, data[0]) # 右前 (00 = 0) self.i2c.writeReg8(self.roverC, 0x01, data[1]) # 左後 (9c = -127) self.i2c.writeReg8(self.roverC, 0x02, data[2]) # 右後 self.i2c.writeReg8(self.roverC, 0x03, data[3]) print("i2c_data :" + str(data)) |
バーチャルパッド描画
バーチャルパッドは以下のように描画.
(参考:https://qiita.com/Yamazin/items/2056deb0507d97dbcf1f )
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 |
/** * バーチャルパッドベース作画 * param {number} x X座標 * param {number} y Y座標 */ let drawVirtualPadBase = (x, y) => { $("#VirtualPad") .drawRect({ layer: true, name: "FrameRect", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", fillStyle: "rgba(0, 0, 0, 1)", strokeWidth: 2, x: x - $("#VirtualPad").offset().left - STICK_THRESHOLD - STICK_RADIUS, y: y - $("#VirtualPad").offset().top - STICK_THRESHOLD - STICK_RADIUS, width: STICK_THRESHOLD * 2 + STICK_RADIUS * 2, height: STICK_THRESHOLD * 2 + STICK_RADIUS * 2, }) // 枠の四角 .drawRect({ layer: true, name: "CenterCircle", groups: ["VirtualPadBase"], strokeStyle: "rgba(255, 255, 255, 1)", strokeWidth: 3, x: x - $("#VirtualPad").offset().left - STICK_RADIUS, y: y - $("#VirtualPad").offset().top - STICK_RADIUS, width: STICK_RADIUS * 2, height: STICK_RADIUS * 2, }) // 中央の四角 .drawLine({ layer: true, name: "TopLine", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", strokeWidth: 5, x1: x - $("#VirtualPad").offset().left, y1: y - $("#VirtualPad").offset().top - STICK_THRESHOLD, x2: x - $("#VirtualPad").offset().left, y2: y - $("#VirtualPad").offset().top - STICK_THRESHOLD - 40, }) // 上線 .drawLine({ layer: true, name: "BottomLine", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", strokeWidth: 5, x1: x - $("#VirtualPad").offset().left, y1: y - $("#VirtualPad").offset().top + STICK_THRESHOLD, x2: x - $("#VirtualPad").offset().left, y2: y - $("#VirtualPad").offset().top + STICK_THRESHOLD + 40, }) // 下線 .drawLine({ layer: true, name: "LeftLine", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", strokeWidth: 5, x1: x - $("#VirtualPad").offset().left - STICK_THRESHOLD, y1: y - $("#VirtualPad").offset().top, x2: x - $("#VirtualPad").offset().left - STICK_THRESHOLD - 40, y2: y - $("#VirtualPad").offset().top, }) // 左線 .drawLine({ layer: true, name: "RightLine", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", strokeWidth: 5, x1: x - $("#VirtualPad").offset().left + STICK_THRESHOLD, y1: y - $("#VirtualPad").offset().top, x2: x - $("#VirtualPad").offset().left + STICK_THRESHOLD + 40, y2: y - $("#VirtualPad").offset().top, }); // 右線 // バーチャルパッドスティック作画 drawVirtualPadStick(x, y); }; /** * バーチャルパッド(ローテーション)作画 * param {number} x X座標 * param {number} y Y座標 */ let drawVirtualPadRotation = (x, y) => { $("#VirtualPad") .drawRect({ layer: true, name: "FrameRect", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", fillStyle: "rgba(0, 0, 0, 1)", strokeWidth: 2, x: x - $("#VirtualPad").offset().left - STICK_THRESHOLD - STICK_RADIUS, y: y - $("#VirtualPad").offset().top - STICK_THRESHOLD - STICK_RADIUS, width: STICK_THRESHOLD * 2 + STICK_RADIUS * 2, height: STICK_THRESHOLD * 2 + STICK_RADIUS * 2, }) // 枠の四角 // バーチャルパッドスティック作画 drawVirtualPadStick(x, y); }; /** * バーチャルパッドスティック作画 * param {number} x X座標 * param {number} y Y座標 */ let drawVirtualPadStick = (x, y) => { $("#VirtualPad") .removeLayerGroup("VirtualPadStick") .drawLayers() .drawRect({ layer: true, name: "CenterCircle", groups: ["VirtualPadBase"], strokeStyle: "rgba(16, 220, 187, 1)", strokeWidth: 3, x: x - $("#VirtualPad").offset().left - STICK_RADIUS, y: y - $("#VirtualPad").offset().top - STICK_RADIUS, width: STICK_RADIUS * 2, height: STICK_RADIUS * 2, }) // スティックの四角 }; /** * バーチャルパットクリア */ let clearVirtualPad = () => { $("#VirtualPad") .removeLayerGroup("VirtualPadBase") .removeLayerGroup("VirtualPadStick") .drawLayers(); }; |
また、タッチイベントは以下の様にする.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 |
/** * バーチャルパッド用キャンバス上でタッチ開始、マウスダウン */ $("#VirtualPad").on("touchstart mousedown", () => { event.preventDefault(); // 初期座標取得 defaultX = isTouchDevice() ? event.changedTouches[0].pageX : event.pageX; defaultY = isTouchDevice() ? event.changedTouches[0].pageY : event.pageY; // console.log("初期座標取得"); // console.log("touchmove mousedown:defaultX:" + defaultX); // console.log("touchmove mousedown:defaultY:" + defaultY); // バーチャルパッドベース作画 drawVirtualPadBase(defaultX, defaultY); /** * バーチャルパッド用キャンバス上でタッチ移動、マウス移動 */ $("#VirtualPad").on("touchmove mousemove", async () => { event.preventDefault(); // 移動後の座標取得 let tempX = roundToThreshold( defaultX - (isTouchDevice() ? event.changedTouches[0].pageX : event.pageX), STICK_THRESHOLD ); let tempY = roundToThreshold( defaultY - (isTouchDevice() ? event.changedTouches[0].pageY : event.pageY), STICK_THRESHOLD ); event.changedTouches[0].pageY : event.pageY),STICK_THRESHOLD); // バーチャルパッドスティック作画 drawVirtualPadStick(defaultX - tempX, defaultY - tempY); //停止状態を解除する stopMode = false; //f,l,r,bへの入力値に変換する await moveMoter((tempX * -1), tempY); }); /** * バーチャルパッド用キャンバス上でタッチ終了、マウスアップ、マウスが離れた */ $("#VirtualPad").on("touchend mouseup mouseleave", async () => { event.preventDefault(); $("#VirtualPad").off("touchmove mousemove"); // バーチャルパッドクリア clearVirtualPad(); if (!stopMode){ //モーターを止める await moveMoter(0, 0); // 止めたらフラグをたてて必要以上に0が送られないようにする stopMode = true; } |
終わりに
バーチャルパッドを使ってのラジコン操作が可能となりました.
今回UIはJSで作りましたが、応用すればPS4コントローラーなどでも全然いけるかと思います.
また、制御関連にあまり手を加えていないこともあり、必要以上に進んでしまうなどオーバーシュートが目立ちます.
ここら辺のチューニングはガジェットの機能を一通り実装してから着手したいと考えています(ロボット工学について絶賛勉強中でございますゆえ).
-
前の記事
【Rover C ProをObnizで動かす】遠隔操作できる自宅監視カメラ(ラズパイを外部電源で動かしたい) 2021.05.14
-
次の記事
(レトロゲーム)ゲームボーイミクロ[ファミコンカラー]を買いました! 2021.06.18