public override void OnInspectorGUI()
{
BSliderConstraint hc = (BSliderConstraint)target;
EditorGUILayout.HelpBox(BSliderConstraint.HelpMessage, MessageType.Info);
DrawDefaultInspector();
}
public override void OnInspectorGUI()
{
BSliderConstraint hc = (BSliderConstraint)target;
EditorGUILayout.HelpBox(BSliderConstraint.HelpMessage, MessageType.Info);
BTypedConstraintEditor.DrawTypedConstraint(hc);
EditorGUILayout.Separator();
EditorGUILayout.LabelField("Limits", EditorStyles.boldLabel);
hc.lowerLinearLimit = EditorGUILayout.FloatField("Lower Linear Limit", hc.lowerLinearLimit);
hc.upperLinearLimit = EditorGUILayout.FloatField("Upper Linear Limit", hc.upperLinearLimit);
hc.lowerAngularLimitRadians = EditorGUILayout.FloatField("Lower Angular Limit (Deg.)", hc.lowerAngularLimitRadians * Mathf.Rad2Deg) * Mathf.Deg2Rad;
hc.upperAngularLimitRadians = EditorGUILayout.FloatField("Upper Angular Limit (Deg.)", hc.upperAngularLimitRadians * Mathf.Rad2Deg) * Mathf.Deg2Rad;
if (GUI.changed)
{
serializedObject.ApplyModifiedProperties();
EditorUtility.SetDirty(hc);
EditorSceneManager.MarkSceneDirty(EditorSceneManager.GetActiveScene());
Repaint();
}
}
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;
}
}
}
}
}
}
public void CreateJoint(RobotBase robotBase, float wheelFriction = 1f, float lateralFriction = 1f)
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL)
{
RigidNode parent = (RigidNode)GetParent();
if (parent.MainObject.GetComponent <BRaycastRobot>() == null)
{
parent.MainObject.AddComponent <BRaycastRobot>().RootNode = RootNode;
}
WheelType wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
BRaycastWheel wheel = MainObject.AddComponent <BRaycastWheel>();
wheel.CreateWheel(this);
if (robotBase is MaMRobot)
{
wheel.Friction = wheelFriction;
wheel.SlidingFriction = lateralFriction;
}
MainObject.transform.parent = parent.MainObject.transform;
}
else
{
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
}
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
Vector3 cAxis = cNode.axis.AsV3().normalized;
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cAxis, AxisType.X));
bc.localConstraintAxisX = cAxis;
bc.localConstraintAxisY = new Vector3(cAxis.y, cAxis.z, cAxis.x);
bc.linearLimitLower = new Vector3((cNode.linearLimitStart - cNode.currentLinearPosition) * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3((cNode.linearLimitEnd - cNode.currentLinearPosition) * 0.01f, 0f, 0f);
// TODO: Implement angular cylinder limits
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
Vector3 lAxis = lNode.axis.AsV3().normalized;
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lAxis, AxisType.X));
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = (lNode.linearLimitLow - lNode.currentLinearPosition) * 0.01f;
sc.upperLinearLimit = (lNode.linearLimitHigh - lNode.currentLinearPosition) * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
public void CreateJoint()
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
WheelType wheelType = WheelType.NOT_A_WHEEL;
if (this.HasDriverMeta <WheelDriverMeta>())
{
OrientWheelNormals();
wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
}
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cNode.axis.AsV3(), AxisType.X));
bc.linearLimitLower = new Vector3(cNode.linearLimitStart * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3(cNode.linearLimitEnd * 0.01f, 0f, 0f);
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
Vector3 lAxis = lNode.axis.AsV3().normalized;
// TODO: Figure out how to make a vertical slider?
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lNode.axis.AsV3(), AxisType.X));
if (lAxis.x < 0)
{
lAxis.x *= -1f;
}
if (lAxis.y < 0)
{
lAxis.y *= -1f;
}
if (lAxis.z < 0)
{
lAxis.z *= -1f;
}
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = lNode.linearLimitLow * 0.01f;
sc.upperLinearLimit = lNode.linearLimitHigh * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
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 void CreateJoint(int numWheels, bool mixAndMatch, float wheelFriction = 1f, float lateralFriction = 1f)
{
if (joint != null || GetSkeletalJoint() == null)
{
return;
}
switch (GetSkeletalJoint().GetJointType())
{
case SkeletalJointType.ROTATIONAL:
if (this.HasDriverMeta <WheelDriverMeta>() && this.GetDriverMeta <WheelDriverMeta>().type != WheelType.NOT_A_WHEEL)
{
RigidNode parent = (RigidNode)GetParent();
if (parent.MainObject.GetComponent <BRaycastRobot>() == null)
{
BRaycastRobot robot = parent.MainObject.AddComponent <BRaycastRobot>();
robot.NumWheels = numWheels;
}
WheelType wheelType = this.GetDriverMeta <WheelDriverMeta>().type;
BRaycastWheel wheel = MainObject.AddComponent <BRaycastWheel>();
wheel.CreateWheel(this);
if (mixAndMatch)
{
wheel.Friction = wheelFriction;
wheel.SlidingFriction = lateralFriction;
}
MainObject.transform.parent = parent.MainObject.transform;
}
else
{
RotationalJoint_Base rNode = (RotationalJoint_Base)GetSkeletalJoint();
rNode.basePoint.x *= -1;
BHingedConstraintEx hc = (BHingedConstraintEx)(joint = ConfigJoint <BHingedConstraintEx>(rNode.basePoint.AsV3() - ComOffset, rNode.axis.AsV3(), AxisType.X));
Vector3 rAxis = rNode.axis.AsV3().normalized;
rAxis.x *= -1f;
hc.axisInA = rAxis;
hc.axisInB = rAxis;
if (hc.setLimit = rNode.hasAngularLimit)
{
hc.lowLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitHigh;
hc.highLimitAngleRadians = rNode.currentAngularPosition - rNode.angularLimitLow;
}
hc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
}
break;
case SkeletalJointType.CYLINDRICAL:
CylindricalJoint_Base cNode = (CylindricalJoint_Base)GetSkeletalJoint();
B6DOFConstraint bc = (B6DOFConstraint)(joint = ConfigJoint <B6DOFConstraint>(cNode.basePoint.AsV3() - ComOffset, cNode.axis.AsV3(), AxisType.X));
bc.linearLimitLower = new Vector3(cNode.linearLimitStart * 0.01f, 0f, 0f);
bc.linearLimitUpper = new Vector3(cNode.linearLimitEnd * 0.01f, 0f, 0f);
bc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
break;
case SkeletalJointType.LINEAR:
LinearJoint_Base lNode = (LinearJoint_Base)GetSkeletalJoint();
lNode.basePoint.x *= -1;
Vector3 lAxis = lNode.axis.AsV3().normalized;
// TODO: Figure out how to make a vertical slider?
BSliderConstraint sc = (BSliderConstraint)(joint = ConfigJoint <BSliderConstraint>(lNode.basePoint.AsV3() - ComOffset, lNode.axis.AsV3(), AxisType.X));
if (lAxis.x < 0)
{
lAxis.x *= -1f;
}
if (lAxis.y < 0)
{
lAxis.y *= -1f;
}
if (lAxis.z < 0)
{
lAxis.z *= -1f;
}
sc.localConstraintAxisX = lAxis;
sc.localConstraintAxisY = new Vector3(lAxis.y, lAxis.z, lAxis.x);
sc.lowerLinearLimit = lNode.linearLimitLow * 0.01f;
sc.upperLinearLimit = lNode.linearLimitHigh * 0.01f;
sc.lowerAngularLimitRadians = 0f;
sc.upperAngularLimitRadians = 0f;
sc.constraintType = BTypedConstraint.ConstraintType.constrainToAnotherBody;
bool b = this.HasDriverMeta <ElevatorDriverMeta>();
if (GetSkeletalJoint().cDriver != null)
{
if (GetSkeletalJoint().cDriver.GetDriveType().IsElevator())
{
MainObject.GetComponent <BRigidBody>().mass *= 2f;
}
}
break;
}
}
/// <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 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;
}
}
}
}
}
}