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