/// <summary> /// Updates the motors on the manipulator in mix and match mode. Called every frame. /// </summary> /// <param name="skeleton"></param> /// <param name="dioModules"></param> /// <param name="controlIndex"></param> public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex) { float[] pwm; float[] can; if (dioModules[0] != null) { pwm = dioModules[0].pwmValues; can = dioModules[0].canValues; } else { pwm = new float[10]; can = new float[10]; } pwm[4] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SpeedArrowPwm); pwm[5] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SpeedArrowPwm); pwm[6] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SpeedArrowPwm); listOfSubNodes.Clear(); skeleton.ListAllNodes(listOfSubNodes); for (int i = 0; i < pwm.Length; i++) { foreach (RigidNode_Base node in listOfSubNodes) { RigidNode rigidNode = (RigidNode)node; BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>(); SkeletalJoint_Base joint = rigidNode.GetSkeletalJoint(); if (raycastWheel != null) { if (joint.cDriver.port1 == i + 1) { float output = pwm[i]; MotorType motorType = joint.cDriver.GetMotorType(); float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f; if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0) { torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear); } raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC); } } if (joint != null && joint.cDriver != null) { if (joint.cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null) { if (joint.cDriver.port1 == i + 1) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; friction = HingeCostFriction; MotorType motorType = joint.cDriver.GetMotorType(); Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType]; maxSpeed = motor.maxSpeed; impulse = motor.baseTorque - motor.slope * ((RigidBody)(rigidNode.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f; if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0) { float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear); impulse /= gearRatio; maxSpeed *= gearRatio; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = joint.cDriver.hasBrake ? motor.baseTorque : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse); } } else if (joint.cDriver.GetDriveType().IsElevator()) { if (joint.cDriver.port1 == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>()) { BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MaxSliderForce; sc.TargetLinearMotorVelocity = pwm[i] * MaxSliderSpeed; } } else if (joint.cDriver.GetDriveType().IsPneumatic() && rigidNode.HasDriverMeta <PneumaticDriverMeta>()) { BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); float output = motors[joint.cDriver.port1 - 1]; float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f; float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f; float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f; float force = psi * ((float)Math.PI) * width * width / 4f; float speed = stroke / 60f; sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = force; sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed; } } } } }
/// <summary> /// Updates all motor values from the given <see cref="RigidNode_Base"/>, pwm values, and emulation network info. /// </summary> /// <param name="skeleton"></param> /// <param name="pwm"></param> /// <param name="emuList"></param> public static void UpdateAllMotors(RigidNode_Base skeleton, float[] pwm, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList) { listOfSubNodes.Clear(); skeleton.ListAllNodes(listOfSubNodes); for (int i = 0; i < pwm.Length; i++) { motors[i] = pwm[i]; } if (Synthesis.GUI.EmulationDriverStation.Instance != null) { UpdateEmulationJoysticks(); UpdateEmulationMotors(pwm); UpdateEmulationSensors(emuList); } foreach (RigidNode node in listOfSubNodes.Select(n => n as RigidNode)) { SkeletalJoint_Base joint = node.GetSkeletalJoint(); if (joint == null || joint.cDriver == null) { continue; } BRaycastWheel raycastWheel = node.MainObject.GetComponent <BRaycastWheel>(); if (raycastWheel != null) { float output = motors[node.GetSkeletalJoint().cDriver.port1 - 1]; MotorType motorType = joint.cDriver.GetMotorType(); float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f; if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0) { torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear); } raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC); } else if (joint.cDriver.GetDriveType().IsMotor() && node.MainObject.GetComponent <BHingedConstraint>() != null) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; float output = !joint.cDriver.isCan ? motors[joint.cDriver.port1 - 1] : motors[joint.cDriver.port1 - 10]; friction = HingeCostFriction; MotorType motorType = joint.cDriver.GetMotorType(); Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType]; maxSpeed = motor.maxSpeed; impulse = motor.baseTorque - motor.slope * ((RigidBody)(node.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f; if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0) { float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear); impulse /= gearRatio; maxSpeed *= gearRatio; } BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? motor.baseTorque : output == 0f ? friction : Mathf.Abs(output * impulse); } else if (joint.cDriver.GetDriveType().IsElevator() && node.HasDriverMeta <ElevatorDriverMeta>()) { float output = motors[joint.cDriver.port1 - 1]; BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MaxSliderForce; sc.TargetLinearMotorVelocity = output * MaxSliderSpeed; } else if (joint.cDriver.GetDriveType().IsPneumatic() && node.HasDriverMeta <PneumaticDriverMeta>()) { BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); float output = motors[joint.cDriver.port1 - 1]; float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f; float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f; float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f; float force = psi * ((float)Math.PI) * width * width / 4f; float speed = stroke / 60f; sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = force; sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed; } } }
/// <summary> /// Updates all emulation sensor values. /// </summary> /// <param name="emuList"></param> private static void UpdateEmulationSensors(List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList) { int iter = 0; foreach (Synthesis.Robot.RobotBase.EmuNetworkInfo a in emuList) { RigidNode rigidNode = null; try { rigidNode = (RigidNode)(a.wheel); } catch (Exception e) { Debug.Log(e.StackTrace); } BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>(); if (a.RobotSensor.type == RobotSensorType.ENCODER) { //BRaycastRobot robot = rigidNode.MainObject.GetComponent<BRaycastRobot>(); double wheelRadius = 3 / 39.3701;// robot.RaycastRobot.GetWheelInfo(0).WheelsRadius; Vector3 currentPos = bRaycastWheel.transform.position; //double displacement = (Math.Sqrt(currentPos.x * currentPos.x + currentPos.z * currentPos.z) - (Math.Sqrt(a.previousPosition.x*a.previousPosition.x + a.previousPosition.z * a.previousPosition.z))); double displacement = ((currentPos - a.previousPosition).magnitude) * Math.Sign(bRaycastWheel.GetWheelSpeed()); double angleDisplacement = (displacement) / (2 * 3.1415 * wheelRadius); a.encoderTickCount += angleDisplacement * a.RobotSensor.conversionFactor; a.previousPosition = currentPos; var portAType = a.RobotSensor.conTypePortA.ToString() == "DIO" ? "DI" : a.RobotSensor.conTypePortA.ToString(); var portBType = a.RobotSensor.conTypePortB.ToString() == "DIO" ? "DI" : a.RobotSensor.conTypePortB.ToString(); if (InputManager.Instance.Roborio.Encoders[iter] == null) { InputManager.Instance.Roborio.Encoders[iter] = new EncoderData(); } if (Synthesis.GUI.EmulationDriverStation.Instance.isRunCode) { InputManager.Instance.Roborio.Encoders[iter].updateEncoder((int)a.RobotSensor.portA, portAType, (int)a.RobotSensor.portB, portBType, (int)a.encoderTickCount); } iter++; } } }
/// <summary> /// Updates all emulation sensor values. /// </summary> /// <param name="emuList"></param> private static void UpdateEmulationSensors(List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList) { int iter = 0; foreach (Synthesis.Robot.RobotBase.EmuNetworkInfo a in emuList) { if (a.RobotSensor.type == RobotSensorType.ENCODER) // TODO revisit this { RigidNode rigidNode = null; try { rigidNode = (RigidNode)(a.wheel); } catch (Exception e) { Debug.Log(e.StackTrace); } BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>(); //BRaycastRobot robot = rigidNode.MainObject.GetComponent<BRaycastRobot>(); double wheelRadius = 3 / 39.3701;// robot.RaycastRobot.GetWheelInfo(0).WheelsRadius Vector3 currentPos = bRaycastWheel.transform.position; //double displacement = (Math.Sqrt(currentPos.x * currentPos.x + currentPos.z * currentPos.z) - (Math.Sqrt(a.previousPosition.x*a.previousPosition.x + a.previousPosition.z * a.previousPosition.z))); double displacement = ((currentPos - a.previousPosition).magnitude) * Math.Sign(bRaycastWheel.GetWheelSpeed()); double angleDisplacement = (displacement) / (2 * 3.1415 * wheelRadius); a.encoderTickCount += angleDisplacement * a.RobotSensor.conversionFactor; a.previousPosition = currentPos; if (Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter] == null) { Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter] = new EmulationService.RobotInputs.Types.EncoderManager(); } EmulationService.RobotInputs.Types.EncoderManager.Types.PortType ConvertPortType(SensorConnectionType type) { if (type == SensorConnectionType.DIO) { return(EmulationService.RobotInputs.Types.EncoderManager.Types.PortType.Di); } if (type == SensorConnectionType.ANALOG) { return(EmulationService.RobotInputs.Types.EncoderManager.Types.PortType.Ai); } throw new Exception(); } Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].AChannel = (uint)a.RobotSensor.portA; Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].AType = ConvertPortType(a.RobotSensor.conTypePortA);; Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].BChannel = (uint)a.RobotSensor.portB; Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].BType = ConvertPortType(a.RobotSensor.conTypePortB);; Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].Ticks = (int)a.encoderTickCount; iter++; } } }