I have been working on a telecontrol application involving a robotic arm. I have created a VB.net program which takes the coordinates received from a webpage (via mySQL) and converts the coordinates into motor steps, which it then outputs to the robot using a serial command.
The manual for the robot (a MICROBOT TeachMover) states that whenever a serial command is sent to the robot, the robot will send a character back (a 0, 1, or 2) indicating success or failure of the operation. The manual states that this "handshake" character must be received in the program.
Whenever I run my program, the robotic arm does not operate as intended. There is a long initial lag in which the robot does not move at all, after which it eventually moves in the positive x-direction. After that, it stops moving completely once more. I continuously receive the exception that the COM port timed out, which indicates that the handshake character is not being read correctly. I do know the serial commands are being sent correctly (I tested this using the close gripper command during form load) but my serial receive command always ends in a timeout exception, regardless of the number of seconds I set for timeout. I have included my current code below. I do not know much about serial communication, so if someone could point out any misunderstandings on my part, that would be great.
VB.net Code:
' 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
(The ReceiveSerialData sub was what I had originally implemented; I have left it in the program commented out as an example of both the approaches I had tried.)
I know there is no lag in getting the coordinates from mySQL, as I have already tested it on its own. However, I do not know if this could be contributing to the lag in the serial communication.
If anyone has any advice or suggestions about how to make my serial communication code more reliable and faster, I would greatly appreciate it. I was also wondering whether I should use the handshake property, but I have not tried it yet as the manual states that the TeachMover arm does not use standard interface signals (DTR, CTS, RTS, etc.) I would be interested in finding out whether this is a software or hardware issue.
Thank you, Gopika
UPDATE: I have been making several changes and testing the serial communication. I changed the timeout to infinite for the time being. I also tried setting the handshake property and RTSEnable/DTREnable, but the program is still not receiving the handshake from the TeachMover and is waiting infinitely.
I did further testing on my project, and I figure out that the program is actually receiving the handshake now. I changed the timeout to infinite, and I reverted back to the ReceiveSerialData sub, and the program now receives the handshake correctly. Thank you to all for your comments and suggestions.