/// <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> /// Updates pwm information and sends robot information to the server. /// </summary> private void FixedUpdate() { base.UpdatePhysics(); BRigidBody rigidBody = GetComponentInChildren <BRigidBody>(); if (rigidBody == null) { return; } if (isLocalPlayer) { float[] pwm = DriveJoints.GetPwmValues(Packet == null ? emptyDIO : Packet.dio, ControlIndex, false); if (isServer) { RpcUpdateRobotInfo(pwm); } else { CmdUpdateRobotInfo(pwm); } if (InputControl.GetButton(Controls.buttons[ControlIndex].resetRobot)) { CmdResetRobot(); } } if (isServer && serverCanSendUpdate) { RemoteUpdateTransforms(); } }
/// <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(); }
/// <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> /// Updates pwm information and sends robot information to the server. /// </summary> private void FixedUpdate() { base.UpdatePhysics(); BRigidBody rigidBody = GetComponentInChildren <BRigidBody>(); if (rigidBody == null) { return; } if (isLocalPlayer) { float[] pwm = DriveJoints.GetPwmValues(Packet == null ? emptyDIO : Packet.dio, ControlIndex, false); if (correctionEnabled) { if (isServer) { RpcUpdateRobotInfo(pwm); } else { CmdUpdateRobotInfo(pwm); } } } if (isServer && serverCanSendUpdate && correctionEnabled) { RemoteUpdateTransforms(); } }
/// <summary> /// Updates the motors of the robot. /// </summary> protected override void UpdateMotors(float[] pwm = null) { base.UpdateMotors(pwm); if (RobotHasManipulator) { DriveJoints.UpdateManipulatorMotors(manipulatorNode, emptyDIO, ControlIndex); } }
/// <summary> /// Updates the motors of the robot. /// </summary> protected override void UpdateMotors() { base.UpdateMotors(); if (RobotHasManipulator) { DriveJoints.UpdateManipulatorMotors(manipulatorNode, ControlIndex, emuList); } }
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 } }