joystick

2019-05-19 00:18:28 +02:00
parent aa1f537137
commit c91b885b17
8 changed files with 305 additions and 1 deletions
@@ -0,0 +1,160 @@
+using System;
+using UnityEngine;
+using System.Collections;
+using System.IO.Ports;
+using System.Linq;
+using UnityEngine.UI;
+
+public class Serial3 : MonoBehaviour{
+
+    public RotationControl bot;
+    public Walk3 walk;
+
+    public float a1, a2, a3, b1, b2, b3;
+
+    ~Serial3() {
+        serialPort.Close();
+    }
+
+    public bool enabled = false;
+
+    SerialPort serialPort;
+
+    public void Update()
+    {
+        if (serialPort == null)
+        {
+            serialPort = new SerialPort(@"\\.\" + "COM7", 115200);
+
+            try
+            {
+                serialPort.NewLine = "\r\n";
+
+//                serialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
+
+                serialPort.Open();
+                serialPort.DiscardOutBuffer();
+                serialPort.DiscardInBuffer();
+
+                enabled = true;
+            }
+            catch
+            {
+                enabled = false;
+            }
+        }
+
+        if (enabled)
+        {
+            if (serialPort is SerialPort)
+            {
+                string received = serialPort.ReadLine();
+                string[] data = received.Split(',');
+
+                foreach (string d in data)
+                {
+
+                    string[] ch = d.Split('_');
+
+                    int channel = int.Parse(ch.First());
+                    float value = float.Parse(ch.Last());
+
+                    switch (channel)
+                    {
+                        case 0:
+                            a1 = value - 127;
+                            break;
+                        case 1:
+                            a2 = value - 127;
+                            break;
+                        case 2:
+                            a3 = value - 127;
+                            break;
+                        case 3:
+                            b1 = value - 127;
+                            break;
+                        case 4:
+                            b2 = value - 127;
+                            break;
+                        case 5:
+                            b3 = value - 127;
+                            break;
+                    }
+
+                    bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
+                        1);
+
+                    float speed = (float) (Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
+                    walk.walkspeed = speed < 0.5f ? 0 : speed;
+
+                    float rotation = -1 * limit(b3/4, -5, 5);
+                    walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
+
+                    walk.walkDirection = (-1 * ConvertRadiansToDegrees((float) Math.Atan2(b2, b1)));
+                }
+            }
+        }
+    }
+
+    private void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
+    {
+        SerialPort serialPort = (SerialPort)sender;
+
+        if (enabled)
+        {
+            if (serialPort is SerialPort)
+            {
+                string received = serialPort.ReadLine();
+                string[] data = received.Split(',');
+
+                foreach (string d in data)
+                {
+                    //                    if (data == "\r")
+                    //                        continue;
+
+                    string[] ch = d.Split('_');
+
+                    int channel = int.Parse(ch.First());
+                    float value = float.Parse(ch.Last());
+
+                    switch (channel)
+                    {
+                        case 0:
+                            a1 = value;
+                            break;
+                        case 1:
+                            a2 = value;
+                            break;
+                        case 2:
+                            a3 = value;
+                            break;
+                        case 3:
+                            b1 = value;
+                            break;
+                        case 4:
+                            b2 = value;
+                            break;
+                        case 5:
+                            b3 = value;
+                            break;
+                    }
+                }
+
+                //                bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
+            }
+        }
+    }
+
+    public float limit(float input, float min, float max)
+    {
+        input =  input / 6;
+        return (input < max & input > min) ? input : input < min ? min : max;
+    }
+
+    public float ConvertRadiansToDegrees(float radians)
+    {
+        float degrees = (float)((180 / Math.PI) * radians);
+        return (degrees);
+    }
+
+}