161 lines
4.5 KiB
C#
161 lines
4.5 KiB
C#
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);
|
|
}
|
|
|
|
}
|