Tanoda
SerialControllerBytes.cs
Go to the documentation of this file.
1
9using System;
10using System.Collections;
11using System.Collections.Generic;
12using System.IO;
13using System.Linq;
14using System.Runtime.InteropServices;
15using System.Threading;
16using NaughtyAttributes;
17using Newtonsoft.Json;
18using UnityEngine;
19
34public class SerialControllerBytes : MonoBehaviour
35{
36#if !UNITY_WEBGL
37 [Tooltip("Port name with which the SerialPort object will be created.")]
38 public string portName = "COM7";
39
40 [Tooltip("Baud rate that the serial device is using to transmit data.")]
41 public int baudRate = 115200;
42
43 [Tooltip("Reference to an scene object that will receive the events of connection, " +
44 "disconnection and the messages from the serial device.")]
45 public GameObject messageListener;
46
47 [Tooltip("After an error in the serial communication, or an unsuccessful " +
48 "connect, how many milliseconds we should wait.")]
49 public int reconnectionDelay = 1000;
50
51 [Tooltip("Maximum number of unread data messages in the queue. " +
52 "New messages will be discarded.")]
53 public int maxUnreadMessages = 1;
54
55 // Constants used to mark the start and end of a connection. There is no
56 // way you can generate clashing messages from your serial device, as I
57 // compare the references of these strings, no their contents. So if you
58 // send these same strings from the serial device, upon reconstruction they
59 // will have different reference ids.
60 public const string SERIAL_DEVICE_CONNECTED = "__Connected__";
61 public const string SERIAL_DEVICE_DISCONNECTED = "__Disconnected__";
62
63 // Internal reference to the Thread and the object that runs in it.
64 protected Thread thread;
65 protected SerialThreadBytes serialThread;
66
67
68 // ------------------------------------------------------------------------
69 // Invoked whenever the SerialController gameobject is activated.
70 // It creates a new thread that tries to connect to the serial device
71 // and start reading from it.
72 // ------------------------------------------------------------------------
73 void OnEnable()
74 {
75 serialThread = new SerialThreadBytes(portName,
79 thread = new Thread(serialThread.RunForever);
80 thread.Start();
81 }
82
83 // ------------------------------------------------------------------------
84 // Invoked whenever the SerialController gameobject is deactivated.
85 // It stops and destroys the thread that was reading from the serial device.
86 // ------------------------------------------------------------------------
87 void OnDisable()
88 {
89 // If there is a user-defined tear-down function, execute it before
90 // closing the underlying COM port.
91 if (userDefinedTearDownFunction != null)
92 userDefinedTearDownFunction();
93
94 // The serialThread reference should never be null at this point,
95 // unless an Exception happened in the OnEnable(), in which case I've
96 // no idea what face Unity will make.
97 if (serialThread != null)
98 {
99 serialThread.RequestStop();
100 serialThread = null;
101 }
102
103 // This reference shouldn't be null at this point anyway.
104 if (thread != null)
105 {
106 thread.Join();
107 thread = null;
108 }
109 }
110
111 // ------------------------------------------------------------------------
112 // Polls messages from the queue that the SerialThread object keeps. Once a
113 // message has been polled it is removed from the queue. There are some
114 // special messages that mark the start/end of the communication with the
115 // device.
116 // ------------------------------------------------------------------------
117 void Update()
118 {
119 // If the user prefers to poll the messages instead of receiving them
120 // via SendMessage, then the message listener should be null.
121 if (messageListener == null)
122 return;
123
124 // Read the next message from the queue
125 var m = serialThread.ReadMessage();
126 var message = "";
127 var s = m as string;
128 var b = m as byte[];
129
130 if (b != null)
131 if (b[3] == 10)
132 {
133 var pose = GetPoseFromArray(b);
134 //Debug.Log($"Current pose: X:{pose.x} Y:{pose.y} Z:{pose.z} R:{pose.r}");
135 var pos = new Vector3(pose.x, pose.y, pose.z);
136 /*if (!savedPos.Contains(pos))*/ savedPos.Add(pos);
137 //Debug.Log(ByteArrayToString(b));
138 //Debug.Log($"Current pose: X:{pose.x} Y:{pose.y} Z:{pose.z} R:{pose.r}");
139 //File.AppendAllText("E:\\robotcoords.log", $"X:{pose.x} Y:{pose.y} Z:{pose.z}");
140 return;
141 }
142
143 message = s ?? m?.ToString();
144
145 if (string.IsNullOrEmpty(message))
146 return;
147
148 // Check if the message is plain data or a connect/disconnect event.
149 if (ReferenceEquals(message, SERIAL_DEVICE_CONNECTED))
150 messageListener.SendMessage("OnConnectionEvent", true);
151 else if (ReferenceEquals(message, SERIAL_DEVICE_DISCONNECTED))
152 messageListener.SendMessage("OnConnectionEvent", false);
153 else
154 messageListener.SendMessage("OnMessageArrived", message);
155 }
156
157 public static string ByteArrayToString(byte[] ba)
158 {
159 return BitConverter.ToString(ba).Replace("-", " ");
160 }
161
162 public float x, y, z, r;
163
164 [Button]
165 private void SendMoveCommand()
166 {
167 var header = new byte[] {0xAA, 0xAA, 0x13};
168 var MoveCommand = new byte[] {0x54, 0x03, 0x01};
169 var payload = ConcatArrays(MoveCommand, BitConverter.GetBytes(x), BitConverter.GetBytes(y),
170 BitConverter.GetBytes(z), BitConverter.GetBytes(r));
171 var message = ConcatArrays(header, payload, new[] {CalculateChecksum(payload)});
172
173 SendSerialMessage(message);
174 }
175
176 internal void SendMoveCommand(Vector3 v, bool queue = false)
177 {
178 SendMoveCommand(new Vector4(v.x, v.y, v.z, 0), queue);
179 }
180
181 internal void SendMoveCommand(Vector4 v, bool queue = false)
182 {
183 //Debug.Log($"Setting position: {v}");
184 var header = new byte[] {0xAA, 0xAA, 0x13};
185 var MoveCommand = new byte[] {0x54, (byte) (queue ? 0x03 : 0x01), 0x01};
186 var payload = ConcatArrays(MoveCommand, BitConverter.GetBytes(v.x), BitConverter.GetBytes(v.y),
187 BitConverter.GetBytes(v.z), BitConverter.GetBytes(v.w));
188 var message = ConcatArrays(header, payload, new[] {CalculateChecksum(payload)});
189
190 SendSerialMessage(message);
191 }
192
193 internal void SendRelativeMoveCommand(Vector3 v, bool queue = false)
194 {
195 //Debug.Log($"Setting position: {v}");
196 var header = new byte[] {0xAA, 0xAA, 0x13};
197 var MoveCommand = new byte[] {0x54, (byte) (queue ? 0x03 : 0x01), 0x07};
198 var payload = ConcatArrays(MoveCommand, BitConverter.GetBytes(v.x), BitConverter.GetBytes(v.y),
199 BitConverter.GetBytes(v.z), BitConverter.GetBytes(0));
200 var message = ConcatArrays(header, payload, new[] {CalculateChecksum(payload)});
201
202 SendSerialMessage(message);
203 }
204
205 public static T[] ConcatArrays<T>(params T[][] list)
206 {
207 var result = new T[list.Sum(a => a.Length)];
208 var offset = 0;
209 for (var x = 0; x < list.Length; x++)
210 {
211 list[x].CopyTo(result, offset);
212 offset += list[x].Length;
213 }
214
215 return result;
216 }
217
218 public static byte CalculateChecksum(byte[] payload)
219 {
220 byte checksum = 0;
221 foreach (var b in payload) checksum += b;
222
223 return (byte) (256 - checksum);
224 }
225
226 // ------------------------------------------------------------------------
227 // Returns a new unread message from the serial device. You only need to
228 // call this if you don't provide a message listener.
229 // ------------------------------------------------------------------------
230 public string ReadSerialMessage()
231 {
232 // Read the next message from the queue
233 return (string) serialThread.ReadMessage();
234 }
235
236 // ------------------------------------------------------------------------
237 // Puts a message in the outgoing queue. The thread object will send the
238 // message to the serial device when it considers it's appropriate.
239 // ------------------------------------------------------------------------
240 public void SendSerialMessage(byte[] message)
241 {
242 serialThread.SendMessage(message);
243 }
244
245 // ------------------------------------------------------------------------
246 // Executes a user-defined function before Unity closes the COM port, so
247 // the user can send some tear-down message to the hardware reliably.
248 // ------------------------------------------------------------------------
249 public delegate void TearDownFunction();
250
251 private TearDownFunction userDefinedTearDownFunction;
252
253 public void SetTearDownFunction(TearDownFunction userFunction)
254 {
255 userDefinedTearDownFunction = userFunction;
256 }
257
258 [Button]
259 private void SendGetPosePayload()
260 {
261 SendSerialMessage(new byte[] {0xAA, 0xAA, 0x02, 0x0A, 0x00, 0xF6});
262 }
263
264 //[Button]
265 //private void StartLogPose()
266 //{
267 // StartCoroutine(work());
268 //}
269 //
270 //IEnumerator work()
271 //{
272 // while (true)
273 // {
274 // yield return new WaitForSeconds(0.1f);
275 // SendSerialMessage(new byte[] { 0xAA, 0xAA, 0x02, 0x0A, 0x00, 0xF6 });
276 // }
277 //}
278
279 [Button]
280 public void SendGripperOff()
281 {
282 SendSerialMessage(new byte[] {0xAA, 0xAA, 0x04, 0x3F, 0x03, 0x00, 0x00, 0xBE});
283 }
284
285 [Button]
286 internal void SendGripperRelease()
287 {
288 SendSerialMessage(new byte[] {0xAA, 0xAA, 0x04, 0x3F, 0x01, 0x01, 0x00, 0xBF});
289 }
290
291 [Button]
292 internal void SendGripperGrip()
293 {
294 SendSerialMessage(new byte[] {0xAA, 0xAA, 0x04, 0x3F, 0x01, 0x01, 0x01, 0xBE});
295 }
296
297 internal readonly List<Vector3> savedPos = new List<Vector3>();
298
300 {
301 savedPos.Clear();
302 }
303
304 [Button]
305 public void SavePosition()
306 {
307 SendSerialMessage(new byte[] {0xAA, 0xAA, 0x02, 0x0A, 0x00, 0xF6});
308 }
309
310 [Button]
312 {
313#if !UNITY_WEBGL
314 File.WriteAllText("DOBOT_POSITIONS.json", JsonConvert.SerializeObject(savedPos));
315#endif
316 }
317
318 [Button]
319 private void ReplayPosition()
320 {
321 StartCoroutine(replayer());
322 }
323
324 IEnumerator replayer()
325 {
326 foreach (var vector3 in savedPos)
327 {
328 SendMoveCommand(vector3, true);
329 yield return new WaitForSeconds(0.1f);
330 }
331 }
332
333
334 public static unsafe PoseReturn GetPoseFromArray(byte[] buffer)
335 {
336 var result = new PoseReturn();
337
338 fixed (byte* bufferPtr = buffer)
339 {
340 Buffer.MemoryCopy(bufferPtr, &result, /*sizeof(PoseReturn)*/37, /*sizeof(PoseReturn)*/37);
341 }
342
343 return result;
344 }
345
346 [StructLayout(LayoutKind.Sequential, Pack = 1)]
347 [Serializable]
348 public unsafe struct PoseReturn
349 {
350 private readonly byte Header1;
351 private readonly byte Header2;
352 private readonly byte payloadLength;
353 private readonly byte ID;
354 private readonly byte Ctrl;
355 public float x; //Robotic arm coordinate system x
356 public float y; //Robotic arm coordinate system y
357 public float z; //Robotic arm coordinate system z
358 public float r; //Robotic arm coordinate system r
359 public fixed float jointAngle[4]; //Robotic arm 4 axis(The basement, rear arm, forearm,EndEffector) angles
360 }
361#endif
362}
UnityEngine.UI.Button Button
Definition: Pointer.cs:7
SerialThreadBytes serialThread
delegate void TearDownFunction()
void SetTearDownFunction(TearDownFunction userFunction)
static unsafe PoseReturn GetPoseFromArray(byte[] buffer)
static string ByteArrayToString(byte[] ba)
const string SERIAL_DEVICE_CONNECTED
static byte CalculateChecksum(byte[] payload)
const string SERIAL_DEVICE_DISCONNECTED
static T[] ConcatArrays< T >(params T[][] list)
void SendSerialMessage(byte[] message)