/// <summary>
/// Sends the pwm information of this robot to the server and other clients.
/// </summary>
/// <param name="pwm"></param>
private void RemoteUpdateRobotInfo(float[] pwm)
{
if (RootNode != null)
{
DriveJoints.UpdateAllMotors(RootNode, pwm, emuList);
}
}
/// <summary>
/// Called once every physics step (framerate independent) to drive motor joints as well as handle the resetting of the robot
/// </summary>
void FixedUpdate()
{
if (rootNode != null && ControlsEnabled)
{
if (Packet != null)
{
DriveJoints.UpdateAllMotors(rootNode, Packet.dio, controlIndex, MixAndMatchMode.GetMecanum());
}
else
{
DriveJoints.UpdateAllMotors(rootNode, emptyDIO, controlIndex, MixAndMatchMode.GetMecanum());
}
int isMixAndMatch = PlayerPrefs.GetInt("mixAndMatch", 0); //0 is false, 1 is true
//If the robot is in Mix and Match mode and has a manipulator, update the manipulator motors
if (robotHasManipulator == 1 && isMixAndMatch == 1)
{
Debug.Log("Manipulator should be moving");
DriveJoints.UpdateManipulatorMotors(manipulatorNode, emptyDIO, controlIndex, MixAndMatchMode.GetMecanum());
}
}
if (IsResetting)
{
Resetting();
}
UpdateStats();
}
/// <summary>
/// Called once every physics step (framerate independent) to drive motor joints as well as handle the resetting of the robot
/// </summary>
void FixedUpdate()
{
if (rootNode != null && ControlsEnabled)
{
if (Packet != null)
{
DriveJoints.UpdateAllMotors(rootNode, Packet.dio, ControlIndex, RobotIsMecanum);
}
else
{
DriveJoints.UpdateAllMotors(rootNode, emptyDIO, ControlIndex, RobotIsMecanum);
}
if (RobotHasManipulator)
{
DriveJoints.UpdateManipulatorMotors(manipulatorNode, emptyDIO, ControlIndex);
}
}
if (IsResetting)
{
Resetting();
}
UpdateStats();
}
void FixedUpdate()
{
if (Input.GetKey(KeyCode.M))
{
SceneManager.LoadScene("MainMenu");
}
if (rootNode != null)
{
UnityPacket.OutputStatePacket packet = unityPacket.GetLastPacket();
DriveJoints.UpdateAllMotors(rootNode, packet.dio);
}
BRigidBody rigidBody = robotObject.GetComponentInChildren <BRigidBody>();
if (Input.GetKey(KeyCode.R))
{
if (!resetting)
{
foreach (GameObject g in extraElements)
{
Destroy(g);
}
BeginReset();
resetting = true;
}
Vector3 transposition = new Vector3(
Input.GetKey(KeyCode.RightArrow) ? RESET_VELOCITY : Input.GetKey(KeyCode.LeftArrow) ? -RESET_VELOCITY : 0f,
0f,
Input.GetKey(KeyCode.UpArrow) ? RESET_VELOCITY : Input.GetKey(KeyCode.DownArrow) ? -RESET_VELOCITY : 0f);
if (!transposition.Equals(Vector3.zero))
{
TransposeRobot(transposition);
}
}
else
{
EndReset();
resetting = false;
}
if (!rigidBody.GetCollisionObject().IsActive)
{
rigidBody.GetCollisionObject().Activate();
}
}
/// <summary>
/// Steps the world
/// </summary>
/// <param name="elapsedTime">elapsed time</param>
public virtual void Update(float elapsedTime, KeyboardKeyEventArgs args)
{
DriveJoints.UpdateAllMotors(Skeleton, args);
cachedArgs = args;
if (Controls.GameControls[Controls.Control.ResetRobot] == args.Key)
{
ResetRobot();
}
World.StepSimulation(elapsedTime, 1000 /*, 1f / 300f*/);
OnUpdate?.Invoke();
}
/// <summary>
/// Called once every physics step (framerate independent) to drive motor joints as well as handle the resetting of the robot
/// </summary>
void FixedUpdate()
{
if (rootNode != null && ControlsEnabled)
{
if (Packet != null)
{
DriveJoints.UpdateAllMotors(rootNode, Packet.dio, controlIndex, MixAndMatchMode.GetMecanum());
}
else
{
DriveJoints.UpdateAllMotors(rootNode, new UnityPacket.OutputStatePacket.DIOModule[2], controlIndex, MixAndMatchMode.GetMecanum());
}
if (MixAndMatchMode.hasManipulator)
{
DriveJoints.UpdateAllMotors(manipulatorNode, new UnityPacket.OutputStatePacket.DIOModule[2], controlIndex, MixAndMatchMode.GetMecanum());
}
}
if (IsResetting)
{
Resetting();
}
}
/// <summary>
/// Updates all motors on the robot.
/// </summary>
protected virtual void UpdateMotors(float[] pwm = null)
{
DriveJoints.UpdateAllMotors(RootNode, pwm ?? DriveJoints.GetPwmValues(Packet == null ? emptyDIO : Packet.dio, ControlIndex, IsMecanum()), emuList);
}
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>
/// Updates all motors on the robot.
/// </summary>
protected virtual void UpdateMotors()
{
DriveJoints.UpdateAllMotors(RootNode, ControlIndex, emuList);
}
/// <summary>
/// Default Constructor
/// </summary>
public Physics()
{
DantzigSolver mlcp = new DantzigSolver();
collisionConf = new SoftBodyRigidBodyCollisionConfiguration();
dispatcher = new CollisionDispatcher(new DefaultCollisionConfiguration());
solver = new DefaultSoftBodySolver();
ConstraintSolver cSolver = new MultiBodyConstraintSolver();
broadphase = new DbvtBroadphase();
World = new SoftRigidDynamicsWorld(dispatcher, broadphase, cSolver, collisionConf, solver);
//Actual scaling is unknown, this gravity is probably not right
World.Gravity = new Vector3(0, -98.1f, 0);
World.SetInternalTickCallback(new DynamicsWorld.InternalTickCallback((w, f) => DriveJoints.UpdateAllMotors(Skeleton, cachedArgs)));
//Roobit
RigidNode_Base.NODE_FACTORY = (Guid guid) => new BulletRigidNode(guid);
string RobotPath = @"C:\Program Files (x86)\Autodesk\Synthesis\Synthesis\Robots\";
string dir = RobotPath;
GetFromDirectory(RobotPath, s => { Skeleton = (BulletRigidNode)BXDJSkeleton.ReadSkeleton(s + "skeleton.bxdj"); dir = s; });
List <RigidNode_Base> nodes = Skeleton.ListAllNodes();
for (int i = 0; i < nodes.Count; i++)
{
BulletRigidNode bNode = (BulletRigidNode)nodes[i];
bNode.CreateRigidBody(dir + bNode.ModelFileName);
bNode.CreateJoint();
if (bNode.joint != null)
{
World.AddConstraint(bNode.joint, true);
}
World.AddCollisionObject(bNode.BulletObject);
collisionShapes.Add(bNode.BulletObject.CollisionShape);
}
//Field
string fieldPath = @"C:\Program Files (x86)\Autodesk\Synthesis\Synthesis\Fields\";
GetFromDirectory(fieldPath, s => f = BulletFieldDefinition.FromFile(s));
foreach (RigidBody b in f.Bodies)
{
World.AddRigidBody(b);
collisionShapes.Add(b.CollisionShape);
}
}
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
}
}