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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
|
Imports System
Imports System.Collections.Generic
Imports System.ComponentModel
Imports System.Data
Imports System.Drawing
Imports System.Text
Imports System.Windows.Forms
Imports System.IO.Ports
Imports System.Threading
Imports System.Reflection
Imports RoboticsConnection.Serializer
Imports RoboticsConnection.Serializer.Components
Imports RoboticsConnection.Serializer.Controllers
Imports RoboticsConnection.Serializer.Ids
Imports RoboticsConnection.Serializer.Sensors
Namespace PIDMotorControlExample
Public Partial Class Form1
Inherits Form
Private serializer As Serializer
Private pmc As PIDMotorController
Private batteryVoltage As AnalogSensor
Private checkPid As Boolean = False
Private connected As Boolean = False
Public Sub New()
InitializeComponent()
serializer = New Serializer()
serializer.BaudRate = 19200
AddHandler serializer.CommunicationStarted, AddressOf serializer_CommunicationStarted
pmc = New PIDMotorController(serializer)
batteryVoltage = New AnalogSensor(serializer)
batteryVoltage.Pin = AnalogPinId.Pin5
batteryVoltage.UpdateFrequency = 1000
AddHandler batteryVoltage.ValueChanged, AddressOf batteryVoltage_ValueChanged
For Each port As String In SerialPort.GetPortNames()
ComListBox.Items.Add(port)
ComListBox.Text = port
Next
End Sub
Private Sub batteryVoltage_ValueChanged(ByVal sender As SerializerComponent)
' Calculate actual battery voltage:
Dim voltage As Double = batteryVoltage.Value 0.01459
BatteryVoltageLabel.Text = [String].Format("Serializer Battery Voltage: {0:0.0} volts", voltage)
End Sub
' CommunicationStarted Event Handler
Private Sub serializer_CommunicationStarted(ByVal sender As Serializer)
Console.WriteLine("Communication established with Serializer")
ConnectionStatusLabel.ForeColor = Color.Green
ConnectionStatusLabel.Text = "Connection Status: Connected"
EnableDriveControls()
connected = True
SerializerFirmwareVersionLabel.Text = "Serializer Firmware Version: " & serializer.GetFirmwareVersion()
Dim a As Assembly = Assembly.GetAssembly(GetType(RoboticsConnection.Serializer.Serializer))
Dim AssemblyString As String = a.GetTypes()(0).Assembly.FullName
Dim version As String = AssemblyString.Substring(AssemblyString.IndexOf("Version=") + 8, 7)
SerializerLibVersionLabel.Text = "Serializer .NET Library Version: " & version
' Set velocity and distance PID parameters:
pmc.VelProportional = 10
pmc.VelIntegral = 0
pmc.VelDerivative = 5
pmc.VelLoop = 20
pmc.DistProportional = 1
pmc.DistIntegral = 0
pmc.DistDerivative = 0
pmc.DistAcceleration = 1
pmc.DistDeadband = 5
pmc.DeadbandEnabled = True
' Set Physical drivetrain configuration parameters (for Stinger).
If StingerRadioButton.Checked Then
Console.WriteLine("Using Stinger Drivetrain Configuration")
pmc.EncoderResolution = 12
pmc.GearReduction = 0.0185
' 1/54
pmc.TicksPerRevolution = 624
' Gear Reduction Encoder Resolution = 52 12
pmc.WheelDiameter = 2.25
pmc.WheelTrack = 8.6
' Distance between centerline of drive wheels
pmc.VelocityDivider = 2.857
'3.57; // = 100/Max PID Velocity
Else
Console.WriteLine("Using Traxster Drivetrain Configuration")
pmc.EncoderResolution = 12
pmc.GearReduction = 0.0185
' 1/54
pmc.TicksPerRevolution = 624
' Gear Reduction Encoder Resolution = 52 12
pmc.WheelDiameter = 2.899
pmc.WheelTrack = 7.25
' Distance between centerline of drive tracks
pmc.VelocityDivider = 2.857
'3.57; // = 100/Max PID Velocity
End If
timer1.Enabled = True
End Sub
' Form1 Timer1 Tick Event Handler
Private Sub timer1_Tick(ByVal sender As Object, ByVal e As EventArgs)
If connected Then
serializer.PumpEvents()
If checkPid Then
Dim busy As Boolean = pmc.QueryStatus()
If busy Then
PidStatusLabel.ForeColor = Color.Red
PidStatusLabel.Text = "PID Status: Busy"
Else
PidStatusLabel.ForeColor = Color.Green
PidStatusLabel.Text = "PID Status: Complete"
checkPid = False
End If
End If
End If
End Sub
Private Sub ConnectButton_Click(ByVal sender As Object, ByVal e As EventArgs)
serializer.PortName = ComListBox.Text
serializer.StartCommunication()
End Sub
Private Sub DisconnectButton_Click(ByVal sender As Object, ByVal e As EventArgs)
ConnectionStatusLabel.ForeColor = Color.Black
ConnectionStatusLabel.Text = "Connection Status: Disconnected"
serializer.ShutDown()
DisableDriveControls()
connected = False
End Sub
Private Sub DriveButton_Click(ByVal sender As Object, ByVal e As EventArgs)
If connected Then
pmc.Speed = MotorSpeed.Value
pmc.Distance = CDbl(MotorDistance.Value)
pmc.TravelDistance()
checkPid = True
End If
End Sub
Private Sub RotateButton_Click(ByVal sender As Object, ByVal e As EventArgs)
If connected Then
pmc.Speed = MotorSpeed.Value
pmc.RotationAngle = CDbl(RotationAngle.Value)
pmc.Rotate()
checkPid = True
End If
End Sub
Private Sub MotorSpeed_Scroll(ByVal sender As Object, ByVal e As EventArgs)
MotorSpeedLabel.Text = MotorSpeed.Value.ToString()
End Sub
Private Sub StopButton_Click(ByVal sender As Object, ByVal e As EventArgs)
If connected Then
pmc.[Stop]()
checkPid = False
Thread.Sleep(100)
PidStatusLabel.ForeColor = Color.Green
PidStatusLabel.Text = "PID Status: Cancelled"
End If
End Sub
Private Sub ExitButton_Click(ByVal sender As Object, ByVal e As EventArgs)
serializer.ShutDown()
Application.[Exit]()
End Sub
Private Sub quitToolStripMenuItem_Click(ByVal sender As Object, ByVal e As EventArgs)
serializer.ShutDown()
Application.[Exit]()
End Sub
Private Sub EnableDriveControls()
DriveButton.Enabled = True
RotateButton.Enabled = True
StopButton.Enabled = True
MotorSpeed.Enabled = True
MotorDistance.Enabled = True
RotationAngle.Enabled = True
End Sub
Private Sub DisableDriveControls()
DriveButton.Enabled = False
RotateButton.Enabled = False
StopButton.Enabled = False
MotorSpeed.Enabled = False
MotorDistance.Enabled = False
RotationAngle.Enabled = False
End Sub
Private Sub instructionsToolStripMenuItem_Click(ByVal sender As Object, ByVal e As EventArgs)
Dim instrux As String = "PID Motor Control Example Application" & vbLf & vbLf
instrux += "MAKE SURE YOU HAVE THE LATEST FIRMWARE FLASHED TO THE SERIALIZER!!!" & vbLf & vbLf
instrux += "Choose a Robot Configuration (Stinger/Traxster)." & vbLf & vbLf
instrux += "Select Appropriate COM Port, and click 'Connect'." & vbLf & vbLf
instrux += "Set a distance (in inches) that you wish the robot to drive." & vbLf & vbLf
instrux += "Set a velocity (0-100) for the robot." & vbLf & vbLf
instrux += "Click the 'Drive Distance' button. The robot should travel that exact distance and stop." & vbLf & vbLf
instrux += "If you have troubles stopping the robot, press the Red 'STOP!!!' button." & vbLf & vbLf
instrux += "Command the robot to rotate a specified angle by entering that angle, and pressing the 'Rotate Angle' button." & vbLf & vbLf
instrux += "If you have troubles stopping the robot, press the Red 'STOP!!!' button." & vbLf & vbLf
instrux += "The status of the PID algorithm status updates while it is executing, and it will let you know when it's complete." & vbLf & vbLf
instrux += "General information about the Serializer is displayed at the bottom of the app," & vbLf & "including Serializer Firmware Version, Serializer .NET Lib Version, and Current Battery Voltage" & vbLf & vbLf
instrux += "This app works easily with the RoboticsConnection.com Stinger and/or Traxster Robot Kits," & vbLf & "since the PID parameters are pre-configured for the motors and quadrature encoders included in the kit." & vbLf
MessageBox.Show(instrux, "Instructions", MessageBoxButtons.OK, MessageBoxIcon.Information)
End Sub
End Class
End Namespace |
