私は、ロボット アームを含む遠隔制御アプリケーションに取り組んでいます。ウェブページから (mySQL 経由で) 受信した座標を受け取り、その座標をモーター ステップに変換し、シリアル コマンドを使用してロボットに出力する VB.net プログラムを作成しました。
ロボット (MICROBOT TeachMover) のマニュアルには、シリアル コマンドがロボットに送信されるたびに、ロボットは操作の成功または失敗を示す文字 (0、1、または 2) を返すと記載されています。マニュアルには、この「ハンドシェイク」文字をプログラムで受信する必要があると記載されています。
プログラムを実行すると、ロボット アームが意図したとおりに動作しません。ロボットがまったく動かない長い初期ラグがあり、その後、最終的に正の x 方向に移動します。その後、再び完全に動かなくなります。COM ポートがタイムアウトしたという例外を継続的に受け取ります。これは、ハンドシェイク文字が正しく読み取られていないことを示しています。シリアル コマンドが正しく送信されていることはわかっています (フォームの読み込み中にグリッパーを閉じるコマンドを使用してこれをテストしました) が、タイムアウトに設定した秒数に関係なく、シリアル受信コマンドは常にタイムアウト例外で終了します。現在のコードを以下に含めました。私はシリアル通信についてよく知らないので、私の誤解を指摘していただけると助かります。
VB.net コード:
' Libraries
Imports MySql.Data.MySqlClient ' Enables connection with MySQL
Imports System.IO.Ports ' Enables communcation with ports
Imports System.Threading.Tasks ' Enables use of Timer class
Imports System.Timers ' Enables use of Timer class
Public Class Form1
' MySQL Variables
Private connectionString As String = "server=localhost;user id=root; password=;database=coordinates"
Private commandText As String = "SELECT * FROM coordinatevals"
Private con As MySqlDataAdapter
Private table As DataTable
' COM Port Variables
Private com As SerialPort ' Port Variable
Private COMPortNumber As Integer = 1 ' Variable to hold COM port number
' Joystick/Robot Coordinates
Private X As Double ' Holds x-axis value received from mySQL
Private Y As Double ' Holds y-axis value received from mySQL
Private Z As Double ' Holds z-axis value received from mySQL
Private P As Double ' Holds pitch angle received from mySQL
Private R As Double ' Holds roll angle received from mySQL
Private thumbPressed As Boolean ' Holds thumb state value received from mySQL
' Robot Arm Constants
Private Const H As Double = 7.625 ' Shoulder height above table (in.)
Private Const L As Double = 7.0 ' Shoulder-to-elbow & elbow-to-wrist length (in.)
Private Const LL As Double = 3.8 ' Wrist-to-fingertip length (Gripper closed) (in.)
Private Const C As Double = 180 / Math.PI ' Degrees to Radians conversion constant (degrees in 1.0 radian)
Private Const R1 As Integer = 0 ' Roll is WRT Arm frame (Change to 1 if WRT Cartesian Frame)
' Variables to hold joint angles
Private T1 As Double
Private T2 As Double
Private T3 As Double
Private T4 As Double
Private T5 As Double
' Variables to hold distances and angles for moving robot
Private RR As Double ' Variable that holds radius
Private R0 As Double ' Variable that holds the distance from the shoulder to the wrist
Private Z0 As Double ' Variable that holds the height of the wrist above the shoulder
Private B As Double ' Angle about which shoulder-elbow-wrist must be pivoted
Private A As Double ' Angle in shoulder-elbow-wrist triangle
' Variables to hold robot arm scale factors
Private Const S1 As Double = 1125
Private Const S2 As Double = -S1
Private Const S3 As Double = -661.2
Private Const S4 As Double = -244.4
Private Const S5 As Double = S4
' Joint Steps from zeroed joint angles to initialization position
Private Const P1 As Integer = 0
Private Const P2 As Integer = -508
Private Const P3 As Integer = 1162
Private Const P4 As Integer = 384
Private Const P5 As Integer = P4
' Variables to hold correct coordinates
Private W1 As Integer
Private W2 As Integer
Private W3 As Integer
Private W4 As Integer
Private W5 As Integer
' Variables to hold previous coordinates
Private oldW1 As Integer = 0
Private oldW2 As Integer = 0
Private oldW3 As Integer = 0
Private oldW4 As Integer = 0
Private oldW5 As Integer = 0
' Variable to hold speed of robot
Private robotSpeed As Integer = 50
' String for command
Private cmdString As String = Nothing ' String to hold command to be sent to robot
' Timer
Dim Timer1 As New Timer(5000)
' Form Load Event
Private Sub Form1_Load(sender As Object, e As EventArgs) Handles MyBase.Load
' Set up the COM port.
com = My.Computer.Ports.OpenSerialPort("COM" & COMPortNumber.ToString)
com.BaudRate = 9600
com.DataBits = 8
com.Parity = IO.Ports.Parity.None
com.StopBits = IO.Ports.StopBits.One
' Reset the robot.
SendSerialData("@RESET")
' Start the timer.
Timer1.Start()
' Add handler for receiving handshake from the TeachMover.
AddHandler com.DataReceived, AddressOf DataReceivedHandler
' Add handler for ticks of clock.
AddHandler Timer1.Elapsed, AddressOf Timer1_Tick_1
End Sub
' Handler for Timer1 Object
Private Sub Timer1_Tick_1(sender As Object, e As EventArgs)
' Disable the timer.
Timer1.Enabled = False
' Fill DataTable object with MySQL data.
Try
con = New MySqlDataAdapter(commandText, connectionString)
table = New DataTable
con.Fill(table)
Catch ex As Exception
MsgBox(ex.ToString)
End Try
' Update form with mySQL data
' Trigger
' x-axis
X = table.Rows(0).Item(0)
' y-axis
Y = table.Rows(0).Item(1)
' z-Axis
Z = table.Rows(0).Item(2)
' Roll angle
R = table.Rows(0).Item(3)
' Pitch angle
P = table.Rows(0).Item(4)
' Thumb button state
thumbPressed = table.Rows(0).Item(5)
' If the thumb button on the joystick is pressed, close the program.
If thumbPressed = True Then
Me.Close()
End If
moveTeachMover()
' Re-enable the timer.
Timer1.Enabled = True
End Sub
' convertToRadians takes an angle in degrees and returns the equivalent angle in radians
Function convertToRadians(ByVal angleInDeg As Double) As Double
Return (angleInDeg / C)
End Function
' sendSerialData takes a string and sends it to the COM port.
Sub SendSerialData(ByVal data As String)
com.WriteLine(data & vbCrLf)
End Sub
Private Shared Sub DataReceivedHandler(sender As Object, e As SerialDataReceivedEventArgs)
Dim sp As SerialPort = CType(sender, SerialPort)
Try
sp.ReadTimeout = 20000
Dim indata As String = sp.ReadLine()
Dim handshake As Integer = CType(indata, Integer) ' Variable that holds the data recieved from the robot
If handshake = 0 Or handshake = 2 Then
MsgBox("ERROR: The command was not executed correctly.")
End If
Catch ex As TimeoutException
MsgBox("ERROR: Serial Port read timed out.")
End Try
End Sub
'' ReceiveSerialData receives data from the TeachMover
'Sub ReceiveSerialData()
' ' Receive strings from a serial port.
' Dim returnStr As String = ""
' Try
' com.ReadTimeout = 10000
' handshake = CType(com.ReadLine(), Integer)
' If handshake = 0 Or handshake = 2 Then
' MsgBox("ERROR: The command was not executed correctly.")
' End If
' Catch ex As TimeoutException
' MsgBox("ERROR: Serial Port read timed out.")
' End Try
'End Sub
Private Sub moveTeachMover()
' Convert angles to radians.
P = convertToRadians(P)
R = convertToRadians(R)
' theta1
' Special case where x-coordinate is 0
If X = 0 Then
T1 = Math.Sign(Y) * Math.PI / 2
Else
T1 = Math.Atan(Y / X)
End If
' ERROR: theta1 is out of range.
If T1 < 0 Then
Exit Sub
End If
' radius
RR = Math.Sqrt((X * X) + (Y * Y))
' ERROR: Hand too close to body
If RR < 2.25 And Z < 15 Then
Exit Sub
End If
' ERROR: Reach out of range
If RR > 17.8 Then
Exit Sub
End If
' Distance from shoulder to wrist
R0 = RR - LL * Math.Cos(P)
If X < 2.25 And Z < 1.25 And R0 < 3.5 Then
' ERROR: Hand interference with base
If P < convertToRadians(-90) Then
Exit Sub
End If
End If
' Height of the wrist above the shoulder
Z0 = Z - LL * Math.Sin(P) - H
' Angles
If R0 = 0 Then
B = (Math.Sign(Z0)) * Math.PI / 2
Else
B = Math.Atan(Z0 / R0)
End If
A = R0 * R0 + Z0 * Z0
A = 4 * L * L / A - 1
' ERROR: Reach out of range for shoulder
If A < 0 Then
Exit Sub
End If
A = Math.Atan(Math.Sqrt(A))
' theta2 and theta3
T2 = A + B
T3 = B - A
' ERROR: Shoulder out of range
If T2 > convertToRadians(144) Or T2 < convertToRadians(-35) Then
Exit Sub
End If
' ERROR: Elbow out of range
If T2 - T3 < 0 Or T2 - T3 > convertToRadians(149) Then
Exit Sub
End If
' ERROR: Pitch out of range
If (R > convertToRadians(270) Or R < convertToRadians(-270)) Then
If (P > ((convertToRadians(90) + T3) - (R + convertToRadians(270))) Or
P < ((convertToRadians(-90) + T3) + (R - convertToRadians(270)))) Then
Exit Sub
End If
End If
' ERROR: Pitch out of range
If P > (convertToRadians(90) + T3) Or P < (convertToRadians(-90) + T3) Then
Exit Sub
End If
' ERROR: Roll out of range
If (R > (convertToRadians(360) - Math.Abs(P - T3)) Or R < (convertToRadians(-360) + Math.Abs(P - T3))) Then
Exit Sub
End If
' theta4
T4 = P - R - R1 * T1
' theta5
T5 = P + R + R1 * T1
' Get correct coordinates.
W1 = CType((S1 * T1 + 0.5), Integer) - P1
W2 = CType((S2 * T2 + 0.5), Integer) - P2
W3 = CType((S3 * T3 + 0.5), Integer) - P3
W4 = CType((S4 * T4 + 0.5), Integer) - P4
W5 = CType((S5 * T5 + 0.5), Integer) - P5
' Send command to robot via serial port.
cmdString = "@STEP " & robotSpeed.ToString & "," & (W1 - oldW1).ToString & "," & (W2 - oldW2).ToString & "," & (W3 - oldW3).ToString & "," & (W4 - oldW4).ToString & "," & (W5 - oldW5).ToString
SendSerialData(cmdString)
'ReceiveSerialData()
oldW1 = W1
oldW2 = W2
oldW3 = W3
oldW4 = W4
oldW5 = W5
End Sub
Private Sub Form1_FormClosed(sender As Object, e As FormClosedEventArgs) Handles MyBase.FormClosed
If com IsNot Nothing Then
com.Close()
End If
Timer1.Stop()
End Sub
End Class
(ReceiveSerialData サブルーチンは、私が最初に実装したものでした。私が試した両方のアプローチの例として、コメント アウトしてプログラムに残しました。)
私はすでに独自にテストしているので、mySQL から座標を取得するのに遅延がないことを知っています。ただし、これがシリアル通信の遅延に寄与している可能性があるかどうかはわかりません。
シリアル通信コードの信頼性と速度を向上させる方法についてアドバイスや提案があれば、ぜひお寄せください。ハンドシェイク プロパティを使用する必要があるかどうかも疑問でしたが、TeachMover アームは標準インターフェイス信号 (DTR、CTS、RTS など) を使用しないとマニュアルに記載されているため、まだ試していません。これがソフトウェアまたはハードウェアの問題であるかどうか。
ありがとう、ゴピカ
更新: いくつかの変更を加え、シリアル通信をテストしています。とりあえずタイムアウトを無限に変更しました。また、ハンドシェイク プロパティと RTSEnable/DTREnable を設定しようとしましたが、プログラムはまだ TeachMover からハンドシェイクを受信しておらず、無限に待機しています。