Exemplo n.º 1
0
    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();
        }
    }
Exemplo n.º 3
0
    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;
                        }
                    }
                }
            }
        }
    }
Exemplo n.º 4
0
        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;
            }
        }
Exemplo n.º 5
0
    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;
        }
    }
Exemplo n.º 6
0
    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;
                        }
                    }
                }
            }
        }
    }
Exemplo n.º 7
0
    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;
        }
    }
Exemplo n.º 8
0
    /// <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;
                    }
                }
            }
        }
    }
Exemplo n.º 9
0
    /// <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;
            }
        }
    }
Exemplo n.º 10
0
    /// <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;
                        }
                    }
                }
            }
        }
    }
Exemplo n.º 11
0
    /// <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;
            }
        }
    }
Exemplo n.º 12
0
    /// <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;
                    }
                }
            }
        }
    }
Exemplo n.º 13
0
    /// <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;
                        }
                    }
                }
            }
        }
    }
Exemplo n.º 14
0
    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;
                        }
                    }
                }
            }
        }
    }