カメラ付きのキャタピラーBluetoothロボットを構築しています。 パート3

前のシリーズ：
パート1
パート2
さて、誰もがすでにスペアパーツと組み立てられたロボットを注文しましたか？ ロボットが復活する時が来ました。
今日はソフトウェアを分析します。
私が提供するオプションはできるだけ簡単です。 彼にユニークな能力を期待しないでください。 彼の仕事はただ仕事に行くことです。 フォールトトレランス、スムーズな制御、追加機能-これは創造性の範囲です。この喜びを奪わないように、私はすべての人に任せています。 コードは非常にシンプルであるため、最適とは言えず、安全ではなく、一般に美しくありません。 それを改善するための提案がある場合-独自のオプションを提供し、なぜそれがなぜ良いのかを説明するコードの断片を直接提供します。
不十分なことに対する非建設的な批判は特に必要ありません:)私はすでに欠点を知っています。 しかし、何かはっきりしないことがあれば、尋ねて、説明します。
さあ、行こう！


タスクを単純なステップに分割します。
オンボードコントローラー

• 前進/後退およびコーナリング用のキャタピラモーターの制御
• カメラサーボ制御
• Bluetooth経由で受信し、モーションコマンドを実行し、カメラのサーボを制御する

PC /ラップトップ

• 移動方向を設定するためのモーターの速度の計算
• Bluetooth経由で制御パケットを転送する
• 簡単に制御できるジョイスティック接続

前進/後退およびコーナリング用のキャタピラモーターの制御

むき出しのHブリッジやL293D / L298Nではなく、既製のMotorShieldを使用しているため、発明するのに特に複雑なものはありません。 AFMotorライブラリーを使用します。 Motorshield V3があり、SPIバスが必要な場合は、 修正版を使用してください 。
コメントはより簡潔で短いため、通常は英語でコメントを書きます。
モーターを制御するための変数を宣言します。 右側のモーターは4番目のポートに接続され、左側のモーターは3番目のポートに接続されます。

AF_DCMotor rMotor(4); //Right motor AF_DCMotor lMotor(3); //Left motor 

与えられた方向と速度に応じて、モーターに回転を命じます（左のモーターの場合）：

 switch (lDirection){ case 0: lMotor.run(RELEASE); break; case 1: lMotor.run(FORWARD); lMotor.setSpeed(lSpeed); break; case 2: lMotor.run(BACKWARD); lMotor.setSpeed(lSpeed); break; } 

適切なエンジンの場合：

 switch (rDirection){ case 0: rMotor.run(RELEASE); break; case 1: rMotor.run(FORWARD); rMotor.setSpeed(rSpeed); break; case 2: rMotor.run(BACKWARD); rMotor.setSpeed(rSpeed); break; } 

lDirection（またはrDirection）は次の値を取ります。
0-モーターを停止します
1-正転
2-回転バック。

カメラサーボ制御

サーボを制御するために、2つのオブジェクトpanServo（カメラの回転を担当）とtiltServo（チルトを担当）を宣言します。 サーボは機械的であり、即座に回転しないため、ドライブが回転コマンドを実行するために必要な遅延の変数を導入します（15ミリ秒で十分です）

 Servo panServo, tiltServo; long interval = 15; // interval at which to control servo long previousMillis = 0; unsigned long currentMillis; 

previousMillisとcurrentMillisは、サーボの実行中に制御ループで愚かに待機することを避けるために使用されます。 確認します-最後のコマンドから15ミリ秒が経過していない場合、サーボにコマンドを送信しても無駄です-まだビジーです。
カメラの回転を担当する部分：

  //Rotate camera currentMillis = millis(); if(currentMillis - previousMillis > interval) { previousMillis = currentMillis; if (lastPan!=pan) panServo.write(pan); // tell pan servo to go to position if (lastTilt!=tilt) tiltServo.write(tilt); // tell tilt servo to go to position lastPan=pan; lastTilt=tilt; } 

Bluetooth経由で受信し、モーションコマンドを実行し、カメラのサーボを制御する

Arduinoの観点から見ると、Bluetoothモジュールは単にシリアル（UART）ポートです。
したがって、コンピューターから何かが来ているかどうかを確認するために、サイクルで問い合わせます。 バッファ内に何かが見つかった場合、ストリーム内のパケットの先頭を探しています-$ FFバイト（サーボの極端な位置と255のモーター速度は実質的に役に立たない-サーバントはより早く隣接し、250-255の速度は変わらないので、この値はストリームで非常に高くなりますまれに、これによりパッケージの先頭をキャッチできます。アルゴリズムを複雑にすることで信頼性を高めることができますが、これで十分です。
ヘッダーが見つかったら、エンジンの方向がエンジンごとに2ビットでエンコードされているバイトを受け入れます。 次に、エンジンの速度を読み取ります-エンジンあたり1バイト（lSpeed、rSpeed）およびカメラサーボの位置（パン、チルト）。

  if (Serial.available()>0) { Header=Serial.read(); //If header found then get and process Cmd if (Header==255){ while(Serial.available()<5){}; Direction=Serial.read(); lSpeed=Serial.read(); rSpeed=Serial.read(); pan=Serial.read(); tilt=Serial.read(); 

次に、左右のエンジンの方向を強調します

  lDirection=Direction & 0x03; rDirection=(Direction & 0x0C) >> 2; 

最後のコマンドを受信して​​から方向または速度が変更された場合は、エンジンの速度を設定してカメラを回転させます。
これがメインのワークサイクル全体です。

 void loop() { if (Serial.available()>0) { Header=Serial.read(); //If header found then get and process Cmd if (Header==255){ while(Serial.available()<5){}; Direction=Serial.read(); lSpeed=Serial.read(); rSpeed=Serial.read(); pan=Serial.read(); tilt=Serial.read(); lDirection=Direction & 0x03; rDirection=(Direction & 0x0C) >> 2; //Left if ((lastlDir!=lDirection) or (lastlSpeed!=lSpeed)){ switch (lDirection){ case 0: lMotor.run(RELEASE); break; case 1: lMotor.run(FORWARD); lMotor.setSpeed(lSpeed); break; case 2: lMotor.run(BACKWARD); lMotor.setSpeed(lSpeed); break; } lastlDir=lDirection; lastlSpeed=lSpeed; } //Right if ((lastrDir!=rDirection) or (lastrSpeed!=rSpeed)){ switch (rDirection){ case 0: rMotor.run(RELEASE); break; case 1: rMotor.run(FORWARD); rMotor.setSpeed(rSpeed); break; case 2: rMotor.run(BACKWARD); rMotor.setSpeed(rSpeed); break; } lastrDir=rDirection; lastrSpeed=rSpeed; } //Rotate camera currentMillis = millis(); if(currentMillis - previousMillis > interval) { previousMillis = currentMillis; if (lastPan!=pan) panServo.write(pan); // tell pan servo to go to position if (lastTilt!=tilt) tiltServo.write(tilt); // tell tilt servo to go to position lastPan=pan; lastTilt=tilt; } } } } 

ご覧のとおり、簡単なことはほとんどありません:)
Googleコードのプロジェクトページからスケッチをダウンロードできます 。

シャーシにコマンドを実行するように教えました。 次に、それらの送信方法を学習する必要があります。
プログラミングを理解するのが面倒、またはDelphiのインストールに消極的である人は、 コンパイルされたバージョンをダウンロードできます

（Logitech Extreme 3D Proジョイスティックまたは中国のEasyTouchゲームパッドで動作します）。

残りの部分ではさらに進みます:)

必要なもの：

• Delphi 2010（Delphi 7で、プロジェクトファイルの数行を修正するだけで済みます）
• オープンComPortライブラリの TComPortコンポーネント（4.11がインストールされています）
• JEDI VCLのコンポーネントTjvHIDDevice、TjvHIDDeviceController。 私はv3.38を使用していますが、最新のものをダウンロードできます。 全部入れて、便利に

方向を設定するためのモーターの速度の計算

前後に移動しても問題はありません。同じ左右のモーター速度と同じ方向を設定するだけです。
動きの方向転換のために、直進からの逸脱の値であるステアの概念を紹介します。 エンジン速度は、次のように前後に移動するために計算されます。

  if Speed>0 then begin //Forward //Left/Right turn lSpeed:=Speed-Steer; rSpeed:=Speed+Steer; if lSpeed<0 then lSpeed:=0; if rSpeed<0 then rSpeed:=0; if lSpeed>MaxSpeed then lSpeed:=MaxSpeed; if rSpeed>MaxSpeed then rSpeed:=MaxSpeed; end else begin //Backward //Left/Right turn lSpeed:=Speed+Steer; rSpeed:=Speed-Steer; if lSpeed>0 then lSpeed:=0; if rSpeed>0 then rSpeed:=0; if lSpeed<(-MaxSpeed) then lSpeed:=-MaxSpeed; if rSpeed<(-MaxSpeed) then rSpeed:=-MaxSpeed; end; 

つまり、左側のエンジンの速度から前方に移動する場合、偏差を減算し、右側のエンジンの速度に加算します。 いずれかのトラックにブレーキをかけると、シャーシが完全に停止することなくスムーズに回転します。 後方に移動するとき、記号は単に変化します。
さて、速度が最大許容値を超えているかどうかを確認します。 特に、これは必要以上に高い電圧でモーターに電力を供給している人に役立ちます-最高速度を制限するだけでモーターは無傷になります。
管理例：
前進 -両方のモーターへの方向は「1」で、速度は同じです
戻る -両方のモーターの方向は「2」で、速度は同じです
動きを左右に変えるには 、方向を同じに設定し、速度は異なります。 速度が遅い側に向きます。
所定の位置に回すために -速度は同じで、モーターの方向は異なります-それは中心を回って回転します。
停止 -両方のモーターの方向「0」

Bluetooth経由で制御パケットを転送する

BluetoothモジュールをPCに追加すると、2つの仮想COMポートが形成されます（1つはインバウンド、もう1つはアウトバウンド）。
ロボットに接続するには、発信ポートを開くだけです。 Bluetooth設定のポートのリストで、またはブルートフォース方式で指定できます-正しいポートに接続すると、プログラムは誓わず、モジュールのLEDは点滅を停止します-接続が確立され、ロボットに直接接続されていると想定できます。

 procedure TfTank.bConnectClick(Sender: TObject); begin if Tank.Connected then begin Tank.Disconnect; bConnect.Caption:='Connect'; end else begin Tank.Port:=cbPort.Text; Tank.Connect; bConnect.Caption:='Disconnect'; MessageBeep(MB_ICONINFORMATION); end; end; 

便宜上、ロボットとの通信に関連するすべてのアクションを実装するTRCTankという小さなクラスを作成しました。

  TRCTank=class private fPort:string; ComPort:TComPort; Cmd, lastCmd:TControlPacket; fConnected:Boolean; function isConnected: boolean; protected public constructor Create; destructor Destroy;override; procedure Connect; procedure Disconnect; procedure SendCommand(lDir,left, rDir, right, pan, tilt:Byte); property Port:string read fPort write fPort; property Connected:boolean read isConnected; end; 

接続と切断は、基本的にポートを開閉するだけで、現在の状態を確認して、開いているポートを開いたり閉じたりしないようにします。

ロボットにコマンドを送信するには、ロボットがキャッチするヘッダーを作成します（コード255のバイトがあります）。 そして、ロボットがそれらを待っている順序でコマンドを書き留めます。 そのような構造が判明

  TControlPacket=record Header, Direction, lSpeed, //left motor speed rSpeed :Byte;//right motor speed pan, tilt :Byte; //Camera pan & tilt end; 

コマンドを送信する機能では、言及する価値のある唯一のことは、2つのオフセットで1バイトに両方のエンジンの方向をパッキングすることです。 残りは明らかです。

 procedure TRCTank.SendCommand; begin if not fConnected then Exit; Cmd.Header:=255; Cmd.Direction:=lDir + rDir shl 2; Cmd.lSpeed:=left; Cmd.rSpeed:=right; Cmd.pan:=pan; Cmd.tilt:=tilt; if (lastCmd.Direction=Cmd.Direction) and (lastCmd.lSpeed=Cmd.lSpeed) and (lastCmd.rSpeed=Cmd.rSpeed) and (lastCmd.pan=Cmd.pan) and (lastCmd.tilt=Cmd.tilt) then Exit; ComPort.Write(cmd, SizeOf(cmd)); lastCmd:=Cmd; end; 

簡単に制御できるジョイスティック接続

残念ながら、インターネット上で賢明なHIDデバイスの操作に関するドキュメントはあまりありません。 その結果、MIDIポートを介して動作するように送信するか、ジョイスティックを2軸と4つのボタンを備えたデバイスと見なす多数の古いコードを調べました。 このオプションは私には向いていませんでした。 TjvJoystickコンポーネントに関する情報はどこにもなかったため、偶然偶然見つけました。 残念ですが、この時点で既にコンポーネントを作成しました:)だから、もしあなたがそれを理解するのが面倒でなければ、JEDI VCLの既製のコンポーネントを使用できます。
HIDデバイスを直接操作し、そのレポートをバイト単位で分析します。 ただし、すべてのジョイスティック軸（EasyTouchには4つあります）とすべてのボタン（10〜12個のジョイスティック）が使用可能です。
これは次のように機能します。フォーム上のTjvHIDDeviceControllerコンポーネントを使用して、システム内のHIDデバイスのリストを取得し、コンボボックスに出力します。 SelectJoystickByID（VID、PID：Word）を呼び出して、選択したデバイスをTRjoystickクラスのオブジェクトに渡します。 （VendorIDおよびProductIDによって選択されます-たとえば、システムのデバイスマネージャーで表示できます）。
TRjoystickクラスはチェックアウトを実行し、ジョイスティックからレポートを受け取り、値を解読し、ボタンと軸のプロパティを設定し、ハンドラープロシージャを呼び出します。 プログラムでは、ハンドラーは次のようになります。

 procedure TfTank.OnJoyData; var Hat:THatPosition; CenterCamera:Boolean; begin Hat:=hCenter; CenterCamera:=False; //Easy touch joystick if (joyPID=6) and (joyVID=121) then begin scrPitch.Position:=TREasyTouchJoystick(Joy).rZ; scrAileron.Position:=TREasyTouchJoystick(Joy).Z; scrRudder.Position:=TREasyTouchJoystick(Joy).X; scrThrottle.Position:=TREasyTouchJoystick(Joy).Y; cbFire.Checked:=TREasyTouchJoystick(Joy).Btn1; cbAltFire.Checked:=TREasyTouchJoystick(Joy).Btn10; Hat:=TREasyTouchJoystick(Joy).Hat; CenterCamera:=TREasyTouchJoystick(Joy).Btn2; Speed:=Round(((TREasyTouchJoystick(Joy).rZ)-127)*2); Steer:=Round((TREasyTouchJoystick(Joy).Z)-127)*2; end; //Logitech Extreme 3D Pro if (joyPID=49685) and (joyVID=1133) then begin scrPitch.Position:=TRLogitechExtreme(Joy).Pitch; scrAileron.Position:=TRLogitechExtreme(Joy).Aileron; scrRudder.Position:=TRLogitechExtreme(Joy).Rudder; scrThrottle.Position:=TRLogitechExtreme(Joy).Throttle; cbFire.Checked:=TRLogitechExtreme(Joy).Btn1; cbAltFire.Checked:=TRLogitechExtreme(Joy).Btn2; Hat:=TRLogitechExtreme(Joy).Hat; CenterCamera:=TRLogitechExtreme(Joy).Btn1; Speed:=(TRLogitechExtreme(Joy).Pitch div 8)-255; //4096 to -256..256 Steer:=(TRLogitechExtreme(Joy).Aileron div 4)-127; //1024 to -127..128 end; ApplyDeadZone(Speed,DeadX); ApplyDeadZone(Steer,DeadY); if Speed>MaxSpeed then Speed:=MaxSpeed; if Speed<-MaxSpeed then Speed:=-MaxSpeed; if Speed>0 then begin //Forward //Left/Right turn lSpeed:=Speed-Steer; rSpeed:=Speed+Steer; if lSpeed<0 then lSpeed:=0; if rSpeed<0 then rSpeed:=0; if lSpeed>MaxSpeed then lSpeed:=MaxSpeed; if rSpeed>MaxSpeed then rSpeed:=MaxSpeed; end else begin //Backward //Left/Right turn lSpeed:=Speed+Steer; rSpeed:=Speed-Steer; if lSpeed>0 then lSpeed:=0; if rSpeed>0 then rSpeed:=0; if lSpeed<(-MaxSpeed) then lSpeed:=-MaxSpeed; if rSpeed<(-MaxSpeed) then rSpeed:=-MaxSpeed; end; scrLeft.Position:=-lSpeed; scrRight.Position:=-rSpeed; if (cbAltFire.Checked) and (bConnect.Caption='Connect') then bConnect.OnClick(Self); case Hat of hUp: Inc(Tilt); hUpRight:begin Inc(Tilt);Dec(pan); end; hRight: Dec(pan); hRightDown: begin Dec(Pan); Dec(tilt); end; hDown: Dec(Tilt); hLeftDown: begin Inc(pan);Dec(tilt); end; hLeft: Inc(pan); hLeftUp: begin Inc(pan);Inc(tilt); end; hCenter: if CenterCamera then begin pan:=panCenter; tilt:=tiltCenter; end; end; //Limit Pan&Tilt range if pan<minPan then pan:=minPan; if tilt<minTilt then tilt:=minTilt; if pan>maxPan then pan:=maxPan; if tilt>maxTilt then tilt:=maxTilt; //Show info lJoy.Caption:='S:'+IntToStr(Speed)+' D:'+InttoStr(Steer)+' L:'+InttoStr(lSpeed)+' R:'+InttoStr(rSpeed); lhat.Caption:=THatPosString[Integer(Hat)]; //Show camera position on sliders scrPan.Position:=pan; scrTilt.Position:=tilt; //Send command to tank Command2Tank; end; 

まず、軸の座標の生の値を速度範囲-256..256および方向-127..128に指定します。
低速での線形制御では、モーターにはその場所からロボットを動かす力がないため、（経験により）小さな不感帯を導入します-特定の速度値からのみ動きます。 （ApplyDeadZone（Speed、DeadX）; ApplyDeadZone（Steer、DeadY）;）
ラダーを考慮した後、速度が範囲外になっていないことを確認し、スライダーを使用してフォームのモーターの速度を表示します。
次に、帽子の位置に応じて、カメラの方向を変更するか中央に配置し、制限も確認します（通常、デジタル制御の制限に達する前にサーボが機械的に停止します）。 別のスライダーペアでカメラの位置を表示し、速度を表示し、戦車にコマンドを送信します。

 procedure TfTank.Command2Tank; begin lDir:=0; rDir:=0; //prepare rDir, lDir data based on tracks speed case lSpeed of 0:lDir:=0; //stop 1..255: lDir:=1; //forward -255..-1:lDir:=2; //backward end; case rSpeed of 0:rDir:=0; //stop 1..255: rDir:=1; //forward -255..-1:rDir:=2; //backward end; Tank.SendCommand(lDir,Abs(lSpeed),rDir,Abs(rSpeed), pan, tilt); end; 

コードにはさまざまな実験があり、選択したポートとジョイスティックの保存、カメラの制御とセンタリングの制限の保存と読み込みを担当する部分があり、ジョイスティックではなく速度と方向のスライダーを直接引いて制御を戻すことができます。 しかし、これは管理の主なタスクには関係ありません。 好きなようにコードを使用できます。必要に応じて「BolgenOSを配置」することもできます。
R BT RC Tankのソースは 、プロジェクトのWebサイトでGoogleコードでダウンロードできます。
私は最初の人から制御プロセスを削除しようとしましたが、カメラで画面を撮影することは恩知らずな仕事であり、かなり平凡なものでした。 しかし、一般的な意味は明確です。


PS現在、シャーシは再加工のために分解されているため、ハードウェア上のコードの変更をすばやく確認することはできません。 ただし、ビデオで見られるように、ダウンロード可能なバージョンは完全に機能します。