public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules) { float[] pwm = dioModules[0].pwmValues; float[] can = dioModules[0].canValues; if (Input.anyKey) { pwm[0] += (Input.GetKey(KeyCode.UpArrow) ? SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.DownArrow) ? -SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.LeftArrow) ? SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.RightArrow) ? -SPEED_ARROW_PWM : 0.0f); pwm[1] += (Input.GetKey(KeyCode.UpArrow) ? -SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.DownArrow) ? SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.LeftArrow) ? SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.RightArrow) ? -SPEED_ARROW_PWM : 0.0f); pwm[2] += Input.GetKey(KeyCode.Alpha1) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha2) ? -SPEED_ARROW_PWM : 0f; pwm[3] += Input.GetKey(KeyCode.Alpha3) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha4) ? -SPEED_ARROW_PWM : 0f; pwm[4] += Input.GetKey(KeyCode.Alpha5) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha6) ? -SPEED_ARROW_PWM : 0f; pwm[5] += Input.GetKey(KeyCode.Alpha7) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha8) ? -SPEED_ARROW_PWM : 0f; pwm[6] += Input.GetKey(KeyCode.Alpha9) ? SPEED_ARROW_PWM : Input.GetKey(KeyCode.Alpha0) ? -SPEED_ARROW_PWM : 0f; } List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>(); skeleton.ListAllNodes(listOfSubNodes); for (int i = 0; i < pwm.Length; i++) { foreach (RigidNode_Base node in listOfSubNodes) { RigidNode rigidNode = (RigidNode)node; if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null) { if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor()) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; if (rigidNode.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WHEEL_MAX_SPEED; impulse = WHEEL_MOTOR_IMPULSE; friction = WHEEL_COAST_FRICTION; } else { maxSpeed = HINGE_MAX_SPEED; impulse = HINGE_MOTOR_IMPULSE; friction = HINGE_COAST_FRICTION; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse); } } else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator()) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>()) { BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MAX_SLIDER_FORCE; sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED; } } } } } }
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex, bool mecanum) { bool IsMecanum = mecanum; int reverse = -1; float[] pwm = new float[10]; float[] can = new float[10]; if (dioModules[0] != null) { pwm = dioModules[0].pwmValues; can = dioModules[0].canValues; } if (IsMecanum) { pwm[(int)MecanumPorts.FRONT_RIGHT] += (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate (Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate pwm[(int)MecanumPorts.BACK_LEFT] += (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate (Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate pwm[(int)MecanumPorts.FRONT_LEFT] += (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].left) ? SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].right) ? -SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.O) ? -SPEED_ARROW_PWM : 0.0f) + //Left Rotate (Input.GetKey(KeyCode.P) ? SPEED_ARROW_PWM : 0.0f); //Right Rotate pwm[(int)MecanumPorts.BACK_RIGHT] += (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? reverse * SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? reverse * -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].left) ? reverse * SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].right) ? reverse * -SPEED_ARROW_PWM : 0.0f) + (Input.GetKey(KeyCode.O) ? reverse * SPEED_ARROW_PWM : 0.0f) + //Left Rotate (Input.GetKey(KeyCode.P) ? reverse * -SPEED_ARROW_PWM : 0.0f); //Right Rotate } else { pwm[0] += (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f); pwm[1] += (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) + (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f); pwm[2] += (InputControl.GetButton(Controls.buttons[controlIndex].pwm2Plus)) ? SPEED_ARROW_PWM : (InputControl.GetButton(Controls.buttons[controlIndex].pwm2Neg)) ? -SPEED_ARROW_PWM : 0f; pwm[3] += (InputControl.GetButton(Controls.buttons[controlIndex].pwm3Plus)) ? SPEED_ARROW_PWM : (InputControl.GetButton(Controls.buttons[controlIndex].pwm3Neg)) ? -SPEED_ARROW_PWM : 0f; pwm[4] += (InputControl.GetButton(Controls.buttons[controlIndex].pwm4Plus)) ? SPEED_ARROW_PWM : (InputControl.GetButton(Controls.buttons[controlIndex].pwm4Neg)) ? -SPEED_ARROW_PWM : 0f; pwm[5] += (InputControl.GetButton(Controls.buttons[controlIndex].pwm5Plus)) ? SPEED_ARROW_PWM : (InputControl.GetButton(Controls.buttons[controlIndex].pwm5Neg)) ? -SPEED_ARROW_PWM : 0f; pwm[6] += (InputControl.GetButton(Controls.buttons[controlIndex].pwm6Plus)) ? SPEED_ARROW_PWM : (InputControl.GetButton(Controls.buttons[controlIndex].pwm6Neg)) ? -SPEED_ARROW_PWM : 0f; } List <RigidNode_Base> listOfSubNodes = new List <RigidNode_Base>(); skeleton.ListAllNodes(listOfSubNodes); for (int i = 0; i < pwm.Length; i++) { foreach (RigidNode_Base node in listOfSubNodes) { RigidNode rigidNode = (RigidNode)node; if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null) { if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor()) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; if (rigidNode.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WHEEL_MAX_SPEED; impulse = WHEEL_MOTOR_IMPULSE; friction = WHEEL_COAST_FRICTION; } else { maxSpeed = HINGE_MAX_SPEED; impulse = HINGE_MOTOR_IMPULSE; friction = HINGE_COAST_FRICTION; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse); } } else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator()) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>()) { BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MAX_SLIDER_FORCE; sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_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 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 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>(); if (raycastWheel != null) { if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1) { float force = pwm[i]; if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0) { force *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear); } raycastWheel.ApplyForce(force); } } if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null) { if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null) { if (rigidNode.GetSkeletalJoint().cDriver.port1 == i + 1) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0) { impulse *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear); } if (rigidNode.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WheelMaxSpeed; impulse = WheelMotorImpulse; friction = WheelCoastFriction; } else { maxSpeed = HingeMaxSpeed; impulse = HingeMotorImpulse; friction = HingeCostFriction; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = rigidNode.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse); } } else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator()) { if (rigidNode.GetSkeletalJoint().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; } } } } } }
/// <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); 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]; if (node.GetSkeletalJoint().cDriver.InputGear != 0 && node.GetSkeletalJoint().cDriver.OutputGear != 0) { output *= Convert.ToSingle(node.GetSkeletalJoint().cDriver.InputGear / node.GetSkeletalJoint().cDriver.OutputGear); } raycastWheel.ApplyForce(output); } else if (joint.cDriver.GetDriveType().IsMotor() && node.MainObject.GetComponent <BHingedConstraint>() != null) { if (!joint.cDriver.isCan) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; float output = motors[joint.cDriver.port1 - 1]; if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0) { impulse *= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear); } if (node.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WheelMaxSpeed; impulse = WheelMotorImpulse; friction = WheelCoastFriction; } else { maxSpeed = HingeMaxSpeed; impulse = HingeMotorImpulse; friction = HingeCostFriction; } BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : output == 0f ? friction : Mathf.Abs(output * impulse); } else { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; float output = motors[joint.cDriver.port1 - 10]; if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0) { impulse *= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear); } if (node.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WheelMaxSpeed; impulse = WheelMotorImpulse; friction = WheelCoastFriction; } else { maxSpeed = HingeMaxSpeed; impulse = HingeMotorImpulse; friction = HingeCostFriction; } BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : 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; } } }
/// <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, int controlIndex, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList) { UpdateAllOutputs(controlIndex, emuList); listOfSubNodes.Clear(); skeleton.ListAllNodes(listOfSubNodes); foreach (RigidNode_Base node in listOfSubNodes) { RigidNode rigidNode = (RigidNode)node; BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>(); if (raycastWheel != null) { float force = GetOutput(rigidNode.GetSkeletalJoint().cDriver); if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0) { force *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear); } raycastWheel.ApplyForce(force); } if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null) { if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null) { float output = GetOutput(rigidNode.GetSkeletalJoint().cDriver); float maxSpeed = 0f; float impulse = 0f; float friction = 0f; if (rigidNode.GetSkeletalJoint().cDriver.InputGear != 0 && rigidNode.GetSkeletalJoint().cDriver.OutputGear != 0) { impulse *= Convert.ToSingle(rigidNode.GetSkeletalJoint().cDriver.InputGear / rigidNode.GetSkeletalJoint().cDriver.OutputGear); } if (rigidNode.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WheelMaxSpeed; impulse = WheelMotorImpulse; friction = WheelCoastFriction; } else { maxSpeed = HingeMaxSpeed; impulse = HingeMotorImpulse; friction = HingeCostFriction; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = rigidNode.GetSkeletalJoint().cDriver.hasBrake ? HingeMotorImpulse : output == 0f ? friction : Mathf.Abs(output * impulse); } else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator()) { if (rigidNode.HasDriverMeta <ElevatorDriverMeta>()) { float output = GetOutput(rigidNode.GetSkeletalJoint().cDriver); BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MaxSliderForce; sc.TargetLinearMotorVelocity = output * MaxSliderSpeed; } } } } }
/// <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) * SPEED_ARROW_PWM); pwm[5] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM); pwm[6] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM); 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>(); if (raycastWheel != null) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1) { raycastWheel.ApplyForce(pwm[i]); } } if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null) { if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; if (rigidNode.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WHEEL_MAX_SPEED; impulse = WHEEL_MOTOR_IMPULSE; friction = WHEEL_COAST_FRICTION; } else { maxSpeed = HINGE_MAX_SPEED; impulse = HINGE_MOTOR_IMPULSE; friction = HINGE_COAST_FRICTION; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse); } } else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator()) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>()) { BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MAX_SLIDER_FORCE; sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED; } } } } } }
public static void UpdateAllMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex, bool mecanum) { bool IsMecanum = mecanum; int reverse = -1; 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]; } if (IsMecanum) { #region Mecanum Drive pwm[(int)MecanumPorts.FRONT_RIGHT] += (InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * -SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * -SPEED_ARROW_PWM); pwm[(int)MecanumPorts.FRONT_LEFT] += (InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * -SPEED_ARROW_PWM); //For some reason, giving the back wheels 0.25 power instead of 0.5 works for strafing pwm[(int)MecanumPorts.BACK_RIGHT] += (InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * -SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * 0.25f); pwm[(int)MecanumPorts.BACK_LEFT] += (InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * 0.25f); pwm[4] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM); pwm[5] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM); pwm[6] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM); #endregion } if (Controls.TankDriveEnabled) { #region Tank Drive //pwm[0] += // (InputControl.GetButton(Controls.buttons[controlIndex].tankFrontLeft) ? SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].tankBackLeft) ? -SPEED_ARROW_PWM : 0.0f); //pwm[1] += // (InputControl.GetButton(Controls.buttons[controlIndex].tankFrontRight) ? -SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].tankBackRight) ? SPEED_ARROW_PWM : 0.0f); pwm[0] += (InputControl.GetAxis(Controls.axes[controlIndex].tankRightAxes) * SPEED_ARROW_PWM); pwm[1] += (InputControl.GetAxis(Controls.axes[controlIndex].tankLeftAxes) * SPEED_ARROW_PWM); pwm[2] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * SPEED_ARROW_PWM); pwm[3] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm3Axes) * SPEED_ARROW_PWM); pwm[4] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM); pwm[5] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM); pwm[6] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM); #endregion } else { #region Arcade Drive //pwm[0] += // (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? -SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f); //pwm[1] += // (InputControl.GetButton(Controls.buttons[controlIndex].forward) ? -SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].backward) ? SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].left) ? -SPEED_ARROW_PWM : 0.0f) + // (InputControl.GetButton(Controls.buttons[controlIndex].right) ? SPEED_ARROW_PWM : 0.0f); pwm[0] += (InputControl.GetAxis(Controls.axes[controlIndex].vertical) * -SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM); pwm[1] += (InputControl.GetAxis(Controls.axes[controlIndex].vertical) * SPEED_ARROW_PWM) + (InputControl.GetAxis(Controls.axes[controlIndex].horizontal) * SPEED_ARROW_PWM); pwm[2] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm2Axes) * SPEED_ARROW_PWM); pwm[3] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm3Axes) * SPEED_ARROW_PWM); pwm[4] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SPEED_ARROW_PWM); pwm[5] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SPEED_ARROW_PWM); pwm[6] += (InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SPEED_ARROW_PWM); #endregion } listOfSubNodes.Clear(); skeleton.ListAllNodes(listOfSubNodes); for (int i = 0; i < pwm.Length; i++) { foreach (RigidNode_Base node in listOfSubNodes) { RigidNode rigidNode = (RigidNode)node; if (pwm[i] != 0f) { BRigidBody rigidBody = rigidNode.MainObject.GetComponent <BRigidBody>(); if (rigidBody != null && !rigidBody.GetCollisionObject().IsActive) { rigidBody.GetCollisionObject().Activate(); } } BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>(); if (raycastWheel != null) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1) { raycastWheel.ApplyForce(pwm[i]); } } if (rigidNode.GetSkeletalJoint() != null && rigidNode.GetSkeletalJoint().cDriver != null) { if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1) { float maxSpeed = 0f; float impulse = 0f; float friction = 0f; if (rigidNode.HasDriverMeta <WheelDriverMeta>()) { maxSpeed = WHEEL_MAX_SPEED; impulse = WHEEL_MOTOR_IMPULSE; friction = WHEEL_COAST_FRICTION; } else { maxSpeed = HINGE_MAX_SPEED; impulse = HINGE_MOTOR_IMPULSE; friction = HINGE_COAST_FRICTION; } BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>(); hingedConstraint.enableMotor = true; hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f; hingedConstraint.maxMotorImpulse = pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse); } } else if (rigidNode.GetSkeletalJoint().cDriver.GetDriveType().IsElevator()) { if (rigidNode.GetSkeletalJoint().cDriver.portA == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>()) { BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>(); SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint(); sc.PoweredLinearMotor = true; sc.MaxLinearMotorForce = MAX_SLIDER_FORCE; sc.TargetLinearMotorVelocity = pwm[i] * MAX_SLIDER_SPEED; } } } } } }