コード例 #1
0
ファイル: Init.cs プロジェクト: solomondg/synthesis
    void FixedUpdate()
    {
        if (skeleton != null)
        {
            List <RigidNode_Base> nodes = skeleton.ListAllNodes();

            unityPacket.OutputStatePacket packet = udp.GetLastPacket();

            //stops robot while menu is open
            if (gui.guiVisible)
            {
                mainNode.rigidbody.isKinematic = true;
            }
            else
            {
                mainNode.rigidbody.isKinematic = false;
            }

            DriveJoints.UpdateAllMotors(skeleton, packet.dio);
            //TODO put this code in drivejoints, figure out nullreference problem with cDriver
            foreach (RigidNode_Base node in nodes)
            {
                UnityRigidNode uNode = (UnityRigidNode)node;
                if (uNode.GetSkeletalJoint() != null)
                {
                    if (uNode.GetSkeletalJoint().GetJointType() == SkeletalJointType.LINEAR && uNode.GetSkeletalJoint().cDriver != null && uNode.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.ELEVATOR)
                    {
                        ElevatorScript es = uNode.unityObject.GetComponent <ElevatorScript>();

                        float[] pwm = packet.dio[0].pwmValues;
                        if (Input.anyKey)
                        {
                            pwm[3] += (Input.GetKey(KeyCode.Alpha1) ? 1f : 0f);
                            pwm[3] += (Input.GetKey(KeyCode.Alpha2) ? -1f : 0f);
                        }
                        if (es != null)
                        {
                            for (int i = 0; i < 8; i++)
                            {
                                if (uNode.GetSkeletalJoint().cDriver.portA == i)
                                {
                                    es.currentTorque = pwm[i] * 2;
                                }
                            }
                        }
                    }
                }
            }
            DriveJoints.UpdateSolenoids(skeleton, packet.solenoid);

            #region HANDLE_SENSORS
            InputStatePacket sensorPacket = new InputStatePacket();
            foreach (RigidNode_Base node in nodes)
            {
                UnityRigidNode uNode = (UnityRigidNode)node;

                if (node.GetSkeletalJoint() == null)
                {
                    continue;
                }

                foreach (RobotSensor sensor in node.GetSkeletalJoint().attachedSensors)
                {
                    int aiValue;                     //int between 0 and 1024, typically
                    InputStatePacket.DigitalState dioValue;
                    switch (sensor.type)
                    {
                    case RobotSensorType.POTENTIOMETER:
                        if (node.GetSkeletalJoint() != null && node.GetSkeletalJoint() is RotationalJoint_Base)
                        {
                            float angle = DriveJoints.GetAngleBetweenChildAndParent(uNode) + ((RotationalJoint_Base)uNode.GetSkeletalJoint()).currentAngularPosition;
                            sensorPacket.ai [sensor.module - 1].analogValues [sensor.port - 1] = (int)sensor.equation.Evaluate(angle);
                        }
                        break;

                    case RobotSensorType.ENCODER:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.LIMIT:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.GYRO:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.MAGNETOMETER:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.ACCELEROMETER:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.LIMIT_HALL:
                        throw new NotImplementedException();
                        break;
                    }
                }
            }
            udp.WritePacket(sensorPacket);
            #endregion

            //calculates stats of robot
            if (mainNode != null)
            {
                speed        = (float)Math.Abs(mainNode.rigidbody.velocity.magnitude);
                weight       = (float)Math.Round(mainNode.rigidbody.mass * 2.20462 * 860, 1);
                angvelo      = (float)Math.Abs(mainNode.rigidbody.angularVelocity.magnitude);
                acceleration = (float)(mainNode.rigidbody.velocity.magnitude - oldSpeed) / Time.deltaTime;
                oldSpeed     = speed;
                if (!time_stop)
                {
                    time += Time.deltaTime;
                }
            }
        }
    }
コード例 #2
0
    void FixedUpdate()
    {
        if (skeleton != null)
        {
            List <RigidNode_Base> nodes = skeleton.ListAllNodes();

            unityPacket.OutputStatePacket packet = udp.GetLastPacket();


            DriveJoints.UpdateAllMotors(skeleton, packet.dio);
            //TODO put this code in drivejoints, figure out nullreference problem with cDriver
            foreach (RigidNode_Base node in nodes)
            {
                UnityRigidNode uNode = (UnityRigidNode)node;
                if (uNode.GetSkeletalJoint() != null)
                {
                    if (uNode.GetSkeletalJoint().GetJointType() == SkeletalJointType.LINEAR && uNode.GetSkeletalJoint().cDriver != null && uNode.GetSkeletalJoint().cDriver.GetDriveType() == JointDriverType.ELEVATOR)
                    {
                        ElevatorScript es = uNode.unityObject.GetComponent <ElevatorScript>();

                        float[] pwm = packet.dio[0].pwmValues;
                        if (Input.anyKey)
                        {
                            pwm[3] += (Input.GetKey(KeyCode.Alpha1) ? 1f : 0f);
                            pwm[3] += (Input.GetKey(KeyCode.Alpha2) ? -1f : 0f);
                        }
                        if (es != null)
                        {
                            for (int i = 0; i < 8; i++)
                            {
                                if (uNode.GetSkeletalJoint().cDriver.portA == i)
                                {
                                    es.currentTorque = pwm[i] * 2;
                                }
                            }
                        }
                    }
                }
            }
            DriveJoints.UpdateSolenoids(skeleton, packet.solenoid);

            #region HANDLE_SENSORS
            InputStatePacket sensorPacket = new InputStatePacket();
            foreach (RigidNode_Base node in nodes)
            {
                UnityRigidNode uNode = (UnityRigidNode)node;

                if (node.GetSkeletalJoint() == null)
                {
                    continue;
                }

                foreach (RobotSensor sensor in node.GetSkeletalJoint().attachedSensors)
                {
                    int aiValue; //int between 0 and 1024, typically
                    InputStatePacket.DigitalState dioValue;
                    switch (sensor.type)
                    {
                    case RobotSensorType.POTENTIOMETER:
                        if (node.GetSkeletalJoint() != null && node.GetSkeletalJoint() is RotationalJoint_Base)
                        {
                            float angle = DriveJoints.GetAngleBetweenChildAndParent(uNode) + ((RotationalJoint_Base)uNode.GetSkeletalJoint()).currentAngularPosition;
                            sensorPacket.ai[sensor.module - 1].analogValues[sensor.port - 1] = (int)sensor.equation.Evaluate(angle);
                        }
                        break;

                    case RobotSensorType.ENCODER:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.LIMIT:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.GYRO:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.MAGNETOMETER:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.ACCELEROMETER:
                        throw new NotImplementedException();
                        break;

                    case RobotSensorType.LIMIT_HALL:
                        throw new NotImplementedException();
                        break;
                    }
                }
            }
            udp.WritePacket(sensorPacket);
            #endregion
        }
    }