// Get Angle between two up vectors
public static float GetAngleBetweenChildAndParent(UnityRigidNode child)
{
HingeJoint hinge = child.GetJoint <HingeJoint>();
if (hinge != null)
{
return(hinge.angle);
}
UnityRigidNode parent = (UnityRigidNode)child.GetParent();
return(MathfExt.ToDegrees(Mathf.Acos(Vector3.Dot(child.unityObject.transform.up, parent.unityObject.transform.up) / (child.unityObject.transform.up.magnitude * parent.unityObject.transform.up.magnitude))));
}
// Gets the linear position of a UnityRigidNode relative to its parent (intended to be used with pistons, but it could be used elsewhere)
public static float GetLinearPositionRelativeToParent(UnityRigidNode baseNode)
{
Vector3 baseDirection = baseNode.unityObject.transform.rotation * baseNode.GetJoint <Joint>().axis;
baseDirection.Normalize();
UnityRigidNode parentNode = (UnityRigidNode)(baseNode.GetParent());
// Vector difference between the world positions of the node, and the parent of the node
Vector3 difference = baseNode.unityObject.transform.position - parentNode.unityObject.transform.position;
// Find the magnitude of 'difference' along the baseDirection
float linearPositionAlongAxis = Vector3.Dot(baseDirection, difference);
// The dot product we get is inverted, so we need to invert it again before we return it.
return(-linearPositionAlongAxis);
}
// A function to handle solenoids
// We will have accurate velocity measures later, but for now, we need something that works.
public static void SetSolenoid(UnityRigidNode node, bool forward)
{
// Acceleration of the piston, whose value will be determined by the following try/catch statement
float acceleration = 0;
// Checks to make sure solenoid data was assigned. We can't really use a try/catch statement because if pressure and diameter data is left blank when the robot is created, Unity will still use its default values.
if (node.GetJoint <ConfigurableJoint>().xDrive.maximumForce > 0)
{
acceleration = node.GetJoint <ConfigurableJoint>().xDrive.maximumForce / node.GetJoint <ConfigurableJoint>().rigidbody.mass *(forward ? 1 : -1);
}
else
{
// Calculating an arbitrary maximum force. Assumes the piston diameter is .5 inches and that the PSI is 60psi.
float psiToNMm2 = 0.00689475728f;
float maximumForce = (psiToNMm2 * 60f) * (Mathf.PI * Mathf.Pow(6.35f, 2f));
acceleration = (maximumForce / node.GetJoint <ConfigurableJoint>().rigidbody.mass) * (forward ? 1 : -1);
throw new PistonDataMissing(node.ToString());
}
// Dot product is reversed, so we need to negate it
float velocity = acceleration * (Time.deltaTime) - Vector3.Dot(node.GetJoint <ConfigurableJoint>().rigidbody.velocity, node.unityObject.transform.TransformDirection(node.GetJoint <ConfigurableJoint>().axis));
node.GetJoint <ConfigurableJoint>().targetVelocity = new Vector3(velocity, 0, 0);
}
void FixedUpdate()
{
if (skeleton != null)
{
List <RigidNode_Base> nodes = skeleton.ListAllNodes();
unityPacket.OutputStatePacket packet = udp.GetLastPacket();
//stops robot while menu is open
if (gui.guiVisible)
{
mainNode.rigidbody.isKinematic = true;
}
else
{
mainNode.rigidbody.isKinematic = false;
}
DriveJoints.UpdateAllMotors(skeleton, packet.dio);
//TODO put this code in drivejoints, figure out nullreference problem with cDriver
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
if (uNode.GetSkeletalJoint() != null)
{
if (uNode.GetSkeletalJoint().GetJointType() == SkeletalJointType.LINEAR && uNode.GetSkeletalJoint().cDriver != null && uNode.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.ELEVATOR)
{
ElevatorScript es = uNode.unityObject.GetComponent <ElevatorScript>();
float[] pwm = packet.dio[0].pwmValues;
if (Input.anyKey)
{
pwm[3] += (Input.GetKey(KeyCode.Alpha1) ? 1f : 0f);
pwm[3] += (Input.GetKey(KeyCode.Alpha2) ? -1f : 0f);
}
if (es != null)
{
for (int i = 0; i < 8; i++)
{
if (uNode.GetSkeletalJoint().cDriver.portA == i)
{
es.currentTorque = pwm[i] * 2;
}
}
}
}
}
}
DriveJoints.UpdateSolenoids(skeleton, packet.solenoid);
#region HANDLE_SENSORS
InputStatePacket sensorPacket = new InputStatePacket();
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
if (node.GetSkeletalJoint() == null)
{
continue;
}
foreach (RobotSensor sensor in node.GetSkeletalJoint().attachedSensors)
{
int aiValue; //int between 0 and 1024, typically
InputStatePacket.DigitalState dioValue;
switch (sensor.type)
{
case RobotSensorType.POTENTIOMETER:
if (node.GetSkeletalJoint() != null && node.GetSkeletalJoint() is RotationalJoint_Base)
{
float angle = DriveJoints.GetAngleBetweenChildAndParent(uNode) + ((RotationalJoint_Base)uNode.GetSkeletalJoint()).currentAngularPosition;
sensorPacket.ai [sensor.module - 1].analogValues [sensor.port - 1] = (int)sensor.equation.Evaluate(angle);
}
break;
case RobotSensorType.ENCODER:
throw new NotImplementedException();
break;
case RobotSensorType.LIMIT:
throw new NotImplementedException();
break;
case RobotSensorType.GYRO:
throw new NotImplementedException();
break;
case RobotSensorType.MAGNETOMETER:
throw new NotImplementedException();
break;
case RobotSensorType.ACCELEROMETER:
throw new NotImplementedException();
break;
case RobotSensorType.LIMIT_HALL:
throw new NotImplementedException();
break;
}
}
}
udp.WritePacket(sensorPacket);
#endregion
//calculates stats of robot
if (mainNode != null)
{
speed = (float)Math.Abs(mainNode.rigidbody.velocity.magnitude);
weight = (float)Math.Round(mainNode.rigidbody.mass * 2.20462 * 860, 1);
angvelo = (float)Math.Abs(mainNode.rigidbody.angularVelocity.magnitude);
acceleration = (float)(mainNode.rigidbody.velocity.magnitude - oldSpeed) / Time.deltaTime;
oldSpeed = speed;
if (!time_stop)
{
time += Time.deltaTime;
}
}
}
}
/// <summary>
/// Loads a robot from file into the simulator.
/// </summary>
private void TryLoadRobot()
{
//resets rotation for new robot
rotation = Quaternion.identity;
if (activeRobot != null)
{
skeleton = null;
UnityEngine.Object.Destroy(activeRobot);
}
if (filePath != null && skeleton == null)
{
//Debug.Log (filePath);
List <Collider> meshColliders = new List <Collider>();
activeRobot = new GameObject("Robot");
activeRobot.transform.parent = transform;
List <RigidNode_Base> names = new List <RigidNode_Base>();
RigidNode_Base.NODE_FACTORY = delegate(Guid guid)
{
return(new UnityRigidNode(guid));
};
skeleton = BXDJSkeleton.ReadSkeleton(filePath + "skeleton.bxdj");
//Debug.Log(filePath + "skeleton.bxdj");
skeleton.ListAllNodes(names);
foreach (RigidNode_Base node in names)
{
UnityRigidNode uNode = (UnityRigidNode)node;
uNode.CreateTransform(activeRobot.transform);
if (!uNode.CreateMesh(filePath + uNode.ModelFileName))
{
UserMessageManager.Dispatch(node.ModelFileName + " has been modified and cannot be loaded.", 6f);
skeleton = null;
UnityEngine.Object.Destroy(activeRobot);
return;
}
uNode.CreateJoint();
Debug.Log("Joint");
meshColliders.AddRange(uNode.unityObject.GetComponentsInChildren <Collider>());
}
{ // Add some mass to the base object
UnityRigidNode uNode = (UnityRigidNode)skeleton;
uNode.unityObject.transform.rigidbody.mass += 20f * PHYSICS_MASS_MULTIPLIER; // Battery'
}
//finds main node of robot to use its rigidbody
mainNode = GameObject.Find("node_0.bxda");
//Debug.Log ("HELLO AMIREKA: " + mainNode);
auxFunctions.IgnoreCollisionDetection(meshColliders);
resetRobot();
}
else
{
Debug.Log("unityWheelData is null...");
}
HideGuiSidebar();
}
/// <summary>
/// Repositions the robot so it is aligned at the center of the field, and resets all the
/// joints, velocities, etc..
/// </summary>
private void resetRobot()
{
if (activeRobot != null && skeleton != null)
{
unityPacket.OutputStatePacket packet = null;
var unityWheelData = new List <GameObject>();
var unityWheels = new List <UnityRigidNode>();
// Invert the position of the root object
/**/
activeRobot.transform.localPosition = new Vector3(1f, 1f, -0.5f);
activeRobot.transform.rotation = Quaternion.identity;
activeRobot.transform.localRotation = Quaternion.identity;
mainNode.transform.rotation = Quaternion.identity;
/**/
var nodes = skeleton.ListAllNodes();
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
uNode.unityObject.transform.localPosition = new Vector3(0f, 0f, -5f);
uNode.unityObject.transform.localRotation = Quaternion.identity;
if (uNode.unityObject.rigidbody != null)
{
uNode.unityObject.rigidbody.velocity = Vector3.zero;
uNode.unityObject.rigidbody.angularVelocity = Vector3.zero;
}
if (uNode.HasDriverMeta <WheelDriverMeta>() && uNode.wheelCollider != null && uNode.GetDriverMeta <WheelDriverMeta>().isDriveWheel)
{
unityWheelData.Add(uNode.wheelCollider);
unityWheels.Add(uNode);
}
}
bool isMecanum = false;
if (unityWheelData.Count > 0)
{
auxFunctions.OrientRobot(unityWheelData, activeRobot.transform);
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
if (uNode.HasDriverMeta <WheelDriverMeta>() && uNode.wheelCollider != null)
{
unityWheelData.Add(uNode.wheelCollider);
if (uNode.GetDriverMeta <WheelDriverMeta>().GetTypeString().Equals("Mecanum"))
{
isMecanum = true;
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelType = (int)WheelType.MECANUM;
}
if (uNode.GetDriverMeta <WheelDriverMeta>().GetTypeString().Equals("Omni Wheel"))
{
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelType = (int)WheelType.OMNI;
}
}
}
auxFunctions.rightRobot(unityWheelData, activeRobot.transform);
if (isMecanum)
{
float sumX = 0;
float sumZ = 0;
foreach (UnityRigidNode uNode in unityWheels)
{
sumX += uNode.wheelCollider.transform.localPosition.x;
sumZ += uNode.wheelCollider.transform.localPosition.z;
}
float avgX = sumX / unityWheels.Count;
float avgZ = sumZ / unityWheels.Count;
foreach (UnityRigidNode uNode in unityWheels)
{
if (uNode.unityObject.GetComponent <BetterWheelCollider>().wheelType == (int)WheelType.MECANUM)
{
if ((avgX > uNode.wheelCollider.transform.localPosition.x && avgZ < uNode.wheelCollider.transform.localPosition.z) ||
(avgX < uNode.wheelCollider.transform.localPosition.x && avgZ > uNode.wheelCollider.transform.localPosition.z))
{
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelAngle = -45;
}
else
{
uNode.unityObject.GetComponent <BetterWheelCollider>().wheelAngle = 45;
}
}
}
}
}
mainNode.transform.rotation = rotation;
mainNode.rigidbody.inertiaTensorRotation = Quaternion.identity;
//makes sure robot spawns in the correct place
mainNode.transform.position = new Vector3(-2f, 1f, -3f);
mainNode.rigidbody.isKinematic = true;
StartCoroutine(FinishReset());
}
foreach (GameObject o in totes)
{
GameObject.Destroy(o);
}
totes.Clear();
}
void FixedUpdate()
{
if (skeleton != null)
{
List <RigidNode_Base> nodes = skeleton.ListAllNodes();
unityPacket.OutputStatePacket packet = udp.GetLastPacket();
DriveJoints.UpdateAllMotors(skeleton, packet.dio);
//TODO put this code in drivejoints, figure out nullreference problem with cDriver
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
if (uNode.GetSkeletalJoint() != null)
{
if (uNode.GetSkeletalJoint().GetJointType() == SkeletalJointType.LINEAR && uNode.GetSkeletalJoint().cDriver != null && uNode.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.ELEVATOR)
{
ElevatorScript es = uNode.unityObject.GetComponent <ElevatorScript>();
float[] pwm = packet.dio[0].pwmValues;
if (Input.anyKey)
{
pwm[3] += (Input.GetKey(KeyCode.Alpha1) ? 1f : 0f);
pwm[3] += (Input.GetKey(KeyCode.Alpha2) ? -1f : 0f);
}
if (es != null)
{
for (int i = 0; i < 8; i++)
{
if (uNode.GetSkeletalJoint().cDriver.portA == i)
{
es.currentTorque = pwm[i] * 2;
}
}
}
}
}
}
DriveJoints.UpdateSolenoids(skeleton, packet.solenoid);
#region HANDLE_SENSORS
InputStatePacket sensorPacket = new InputStatePacket();
foreach (RigidNode_Base node in nodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
if (node.GetSkeletalJoint() == null)
{
continue;
}
foreach (RobotSensor sensor in node.GetSkeletalJoint().attachedSensors)
{
int aiValue; //int between 0 and 1024, typically
InputStatePacket.DigitalState dioValue;
switch (sensor.type)
{
case RobotSensorType.POTENTIOMETER:
if (node.GetSkeletalJoint() != null && node.GetSkeletalJoint() is RotationalJoint_Base)
{
float angle = DriveJoints.GetAngleBetweenChildAndParent(uNode) + ((RotationalJoint_Base)uNode.GetSkeletalJoint()).currentAngularPosition;
sensorPacket.ai[sensor.module - 1].analogValues[sensor.port - 1] = (int)sensor.equation.Evaluate(angle);
}
break;
case RobotSensorType.ENCODER:
throw new NotImplementedException();
break;
case RobotSensorType.LIMIT:
throw new NotImplementedException();
break;
case RobotSensorType.GYRO:
throw new NotImplementedException();
break;
case RobotSensorType.MAGNETOMETER:
throw new NotImplementedException();
break;
case RobotSensorType.ACCELEROMETER:
throw new NotImplementedException();
break;
case RobotSensorType.LIMIT_HALL:
throw new NotImplementedException();
break;
}
}
}
udp.WritePacket(sensorPacket);
#endregion
}
}
// This function takes a skeleton and byte (a packet) as input, and will use both to check if each solenoid port is open.
public static void UpdateSolenoids(RigidNode_Base skeleton, unityPacket.OutputStatePacket.SolenoidModule[] solenoidModules)
{
byte packet = solenoidModules [0].state;
List <RigidNode_Base> listOfNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfNodes);
foreach (RigidNode_Base subBase in listOfNodes)
{
UnityRigidNode unityNode = (UnityRigidNode)subBase;
Joint joint = unityNode.GetJoint <Joint>();
// Make sure piston and skeletalJoint exist
// If the rigidNodeBase contains a bumper_pneumatic joint driver (meaning that its a solenoid)
if (joint != null && joint is ConfigurableJoint && subBase != null && subBase.GetSkeletalJoint() != null && subBase.GetSkeletalJoint().cDriver != null && (subBase.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.BUMPER_PNEUMATIC || subBase.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.RELAY_PNEUMATIC))
{
ConfigurableJoint cj = (ConfigurableJoint)joint;
// It will use bitwise operators to check if the port is open (see wiki for full explanation).
int stateA = packet & (1 << (subBase.GetSkeletalJoint().cDriver.portA - 1));
int stateB = packet & (1 << (subBase.GetSkeletalJoint().cDriver.portB - 1));
float linearPositionAlongAxis = GetLinearPositionRelativeToParent(unityNode);
// Error catching is done in the SetSolenoid function
if (stateA > 0 && stateB < 0)
{
// Do Nothing. Both ports should not be open
}
else if (stateA < 0 && stateB < 0)
{
// Again, do nothing. There will be no flow.
}
else if (stateA > 0)
{
SetSolenoid(unityNode, true);
}
else if (stateB > 0)
{
SetSolenoid(unityNode, false);
}
// If the piston hits its upper limit, stop it from extending any farther.
if (Mathf.Abs(cj.linearLimit.limit - linearPositionAlongAxis) < (.03f * cj.linearLimit.limit))
{
// Since we still want it to retract, however, we will only stop the piston if its velocity if positive. If its not, (its going backwards), we won't need to stop it
if (cj.targetVelocity.x > 0)
{
cj.targetVelocity = Vector3.zero;
}
// Otherwise, if the piston has reached its lower limit, we need to stop it from attempting retract farther.
}
else if (Mathf.Abs(-1 * cj.linearLimit.limit - linearPositionAlongAxis) < (.03f * cj.linearLimit.limit))
{
if (cj.targetVelocity.x < 0)
{
cj.targetVelocity = Vector3.zero;
}
}
}
}
}
// Drive All Motors Associated with a PWM port
public static void UpdateAllMotors(RigidNode_Base skeleton, unityPacket.OutputStatePacket.DIOModule[] dioModules)
{
float[] pwm = dioModules [0].pwmValues;
float[] can = dioModules [0].canValues;
if (Input.anyKey)
{
pwm [0] +=
(Input.GetKey(KeyCode.UpArrow) ? speedArrowPWM : 0.0f) +
(Input.GetKey(KeyCode.DownArrow) ? -speedArrowPWM : 0.0f) +
(Input.GetKey(KeyCode.LeftArrow) ? -speedArrowPWM : 0.0f) +
(Input.GetKey(KeyCode.RightArrow) ? speedArrowPWM : 0.0f);
pwm [1] +=
(Input.GetKey(KeyCode.UpArrow) ? -speedArrowPWM : 0.0f) +
(Input.GetKey(KeyCode.DownArrow) ? speedArrowPWM : 0.0f) +
(Input.GetKey(KeyCode.LeftArrow) ? -speedArrowPWM : 0.0f) +
(Input.GetKey(KeyCode.RightArrow) ? speedArrowPWM : 0.0f);
}
List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>();
skeleton.ListAllNodes(listOfSubNodes);
foreach (RigidNode_Base node in listOfSubNodes)
{
UnityRigidNode uNode = (UnityRigidNode)node;
//Debug.Log(uNode.GetSkeletalJoint().cDriver == null);
}
// Cycles through the packet
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
// Typcasting RigidNode to UnityRigidNode to use UnityRigidNode functions
UnityRigidNode unitySubNode = (UnityRigidNode)node;
if (unitySubNode.GetSkeletalJoint() != null && unitySubNode.GetSkeletalJoint().cDriver != null && unitySubNode.GetSkeletalJoint().cDriver.isCan)
{
pwm = can;
}
// Checking if there is a joint (and a joint driver) attatched to each joint
if (unitySubNode.GetSkeletalJoint() != null && unitySubNode.GetSkeletalJoint().cDriver != null && unitySubNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor())
{
// If port A matches the index of the array in the packet, (A.K.A: the packet index is reffering to the wheelCollider on the subNode0), then that specific wheel Collider is set.
if (unitySubNode.HasDriverMeta <WheelDriverMeta>() && unitySubNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
float OzInToNm = .00706155183333f * Init.PHYSICS_MASS_MULTIPLIER;
BetterWheelCollider bwc = unitySubNode.unityObject.GetComponent <BetterWheelCollider>();
bwc.currentTorque = OzInToNm * (pwm [i] * 271.1f);
bwc.brakeTorque = 343f * OzInToNm;
}
else if (unitySubNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
Joint joint = unitySubNode.GetJoint <Joint>();
// Something Arbitrary for now. 4 radians/second
float OzInToNm = (.00706155183333f / 3f) * Init.PHYSICS_MASS_MULTIPLIER;
float motorForce = OzInToNm * (Math.Abs(pwm [i]) < 0.05f ? 343f : (pwm [i] * pwm [i] * 171.1f));
float targetVelocity = 10000f * Math.Sign(pwm [i]);
#region Config_Joint
if (joint != null && joint is ConfigurableJoint)
{
ConfigurableJoint cj = (ConfigurableJoint)joint;
JointDrive jD = cj.angularXDrive;
jD.maximumForce = motorForce;
cj.angularXDrive = jD;
cj.targetAngularVelocity = new Vector3(targetVelocity, 0, 0);
// We will need this to tell when the joint is very near a limit
float angularPosition = GetAngleBetweenChildAndParent(unitySubNode);
// Stopping the configurable joint if it approaches its limits (if its within 5% of its limit)
if (cj.angularXMotion == ConfigurableJointMotion.Limited &&
(cj.highAngularXLimit.limit - angularPosition) <
(0.05f * cj.highAngularXLimit.limit))
{
// This prevents the motor from rotating toward its limit again after we have gotten close enough to the limit that we need to stop it.
// We will need it to be able to rotate away from the limit however (hence, the if-else statements)
// If the local up Vector of the unityObject is negative, the joint is approaching its positive limit (I am not sure if this will work in all cases, so its testing time!)
if (unitySubNode.unityObject.transform.up.x < 0 && cj.targetAngularVelocity.x > 0)
{
cj.targetAngularVelocity = Vector3.zero;
}
else if (unitySubNode.unityObject.transform.up.x > 0 && cj.targetAngularVelocity.x < 0)
{
cj.targetAngularVelocity = Vector3.zero;
}
}
}
#endregion
#region hinge joint
if (joint != null && joint is HingeJoint)
{
HingeJoint hj = (HingeJoint)joint;
JointMotor motor = hj.motor;
motor.force = motorForce;
motor.freeSpin = false;
motor.targetVelocity = targetVelocity;
if (hj.useLimits)
{
float limitRange = hj.limits.max - hj.limits.min;
if (Math.Min(Math.Abs(hj.angle - hj.limits.min), Math.Abs(hj.angle - hj.limits.max)) < 0.05 * limitRange)
{
// This prevents the motor from rotating toward its limit again after we have gotten close enough to the limit that we need to stop it.
// We will need it to be able to rotate away from the limit however (hence, the if-else statements)
// If the local up Vector of the unityObject is negative, the joint is approaching its positive limit (I am not sure if this will work in all cases, so its testing time!)
if (unitySubNode.unityObject.transform.up.x < 0 && motor.targetVelocity > 0)
{
motor.targetVelocity = 0;
}
else if (unitySubNode.unityObject.transform.up.x > 0 && motor.targetVelocity < 0)
{
motor.targetVelocity = 0;
}
}
}
hj.motor = motor;
#endregion
}
else if (unitySubNode.GetSkeletalJoint().cDriver.portA == i + 1)
{
// Should we throw an exception here?
Debug.Log("There's an issue: We have an active motor not set (even though it should be set).");
}
}
}
/* if(unitySubNode.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.ELEVATOR)
* {
* Debug.Log("got to 1");
* ElevatorScript es = unitySubNode.unityObject.GetComponent<ElevatorScript>();
* if(unitySubNode.GetSkeletalJoint().cDriver.portA == i + 1)
* {
* Debug.Log("got to 2");
* es.currentTorque = pwm[i]*.00706155183333f * Init.PHYSICS_MASS_MULTIPLIER;
* }
* }*/
}
}
}
