Team4388/2019-Virtual-Field
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Virtual-Field.git synced 2026-06-09 00:28:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Compatability with Robopipe v2.0.0

Browse Source
This commit is contained in:
Keenan D. Buckley
2019-01-28 16:16:40 -07:00
parent 2b26a07105
commit b2ef9af37a
6 changed files with 1468 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
FRC 2019 Virtual Camera/Assets/Scripts/robotCommunication.cs
+26 -8
View File
@@ -9,16 +9,22 @@ using UnityEngine;
public class robotCommunication : MonoBehaviour
{
public bool isData = false;
public static int portNumber = 4388;
public int bytesAvailable;
List<string> keys = new List<string>();
public SortedList<string, float> robotValues =
new SortedList<string, float>
{
{ "encoder", 0.0f },
{ "navX", 0.0f },
{ "Left Pos Inches", 0.0f },
{ "Yaw Angle Deg", 0.0f },
{ "Right Pos Inches", 0.0f },
{ "Elevator Pos Ticks", 0.0f },
};
public string input;
public float encoder, navX;
public string input, request = "";
IPEndPoint serverAddress = new IPEndPoint(IPAddress.Parse("127.0.0.1"), portNumber);
Socket clientSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
@@ -29,24 +35,36 @@ public class robotCommunication : MonoBehaviour
// Start is called before the first frame update
void Start()
{
Console.WriteLine("Connecting...");
clientSocket.Connect(serverAddress);
Console.WriteLine("Connected");
foreach (var valuePair in robotValues)
{
request += valuePair.Key;
keys.Add(valuePair.Key);
request += ",";
}
request = request.Substring(0,request.Length-1);
byte[] byteRequest = Encoding.UTF8.GetBytes(request);
clientSocket.Send(byteRequest);
}
// Update is called once per frame
void Update()
{
bytesAvailable = clientSocket.Available;
if (bytesAvailable == 12)
if (bytesAvailable > 10)
{
byte[] byteInput = new byte[100];
clientSocket.Receive(byteInput);
input = Encoding.UTF8.GetString(byteInput);
string[] values = input.Split('|');
robotValues["encoder"] = float.Parse(values[0], CultureInfo.InvariantCulture.NumberFormat);
robotValues["navX"] = float.Parse(values[1], CultureInfo.InvariantCulture.NumberFormat);
int i = 0;
foreach (string key in keys)
{
robotValues[keys[i]] = float.Parse(values[i], CultureInfo.InvariantCulture.NumberFormat);
i++;
}
clientSocket.Send(new byte[] { 100 });
isData = true;
}
}
}
FRC 2019 Virtual Camera/Assets/Scripts/robotControl.cs
+28 -19
View File
@@ -6,37 +6,46 @@ using UnityEngine;
public class robotControl : MonoBehaviour
{
public float yRot, yAng;
public float avgEncoder, lastEncoder;
public Vector3 pos;
public Vector3 rot;
public float initialRot, yRot, yAng;
public float avgEncoder, lastEncoder, leftEncoder, rightEncoder;
public Vector3 pos, rot;
public int button;
robotCommunication roboCom;
bool start = false;
// Start is called before the first frame update
void Start()
{
GameObject Robot = GameObject.Find("Robot");
roboCom = Robot.GetComponent<robotCommunication>();
pos = new Vector3(0, 10, -260);
pos = new Vector3(0, 10, -257.5f);
rot = new Vector3(0.0f, 0.0f, 0.0f);
gameObject.transform.position = pos;
roboCom.robotValues.TryGetValue("encoder", out avgEncoder);
lastEncoder = avgEncoder;
}
// Update is called once per frame
void Update()
{
roboCom.robotValues.TryGetValue("encoder", out yRot);
roboCom.robotValues.TryGetValue("encoder", out avgEncoder);
yAng = (yRot + 90) * Mathf.Deg2Rad;
pos.x += ((avgEncoder - lastEncoder) * Mathf.Cos(yAng));
pos.z += ((avgEncoder - lastEncoder) * Mathf.Sin(yAng));
///TODO: Adjust yRot to fall within -180 to 180
rot.y = yRot;
transform.rotation = Quaternion.Euler(rot);
transform.position = pos;
lastEncoder = avgEncoder;
roboCom.robotValues.TryGetValue("Yaw Angle Deg", out yRot);
roboCom.robotValues.TryGetValue("Left Pos Inches", out leftEncoder);
roboCom.robotValues.TryGetValue("Right Pos Inches", out rightEncoder);
if (!start && roboCom.isData)
{
avgEncoder = (leftEncoder + rightEncoder) / 2;
lastEncoder = avgEncoder;
initialRot = yRot;
transform.position = pos;
start = true;
} else if (roboCom.isData) {
avgEncoder = (leftEncoder + rightEncoder) / 2;
yAng = (yRot - initialRot) * Mathf.Deg2Rad;
float _yAng = yAng;
pos.x -= (avgEncoder - lastEncoder) * Mathf.Cos(yAng);
_yAng = yAng;
pos.z += (avgEncoder - lastEncoder) * Mathf.Sin(yAng);
rot.y = yAng * Mathf.Rad2Deg;
transform.rotation = Quaternion.Euler(rot);
transform.position = pos;
lastEncoder = avgEncoder;
}
}
}