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