Hexapod_Simulation_Unity/Assets/Serial3.cs

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)));

                    float rotationabs = Math.Abs(walk.centerRotation);
                    float speed2 = ((6 / 3) * walk.walkspeed) + ((6/5) * rotationabs);
                    speed2 = speed2 < 6 ? speed2 : 6;
                    walk.speed2 = (int)(10 - speed2);
                }
            }
        }
    }

    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);
    }

}