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