Esempio n. 1
0
    void FixedUpdate()
    {
        // if we don't have a simMotor assigned to this component, don't
        // do anything
        if (simMotor != null)
        {
            // find the motor we look to for commands
            SimMotor leader = simMotor;
            if (simMotor.Following != null)
            {
                leader = simMotor.Following;
            }

            double radsPerSecond = leader.Config.TargetVelocity;
            double radsPosition  = leader.Config.TargetPosition;
            switch (leader.Config.ControlMode)
            {
            case MotorConfig.Types.ControlMode.PercentOutput:
                float output = (float)(stallTorque * leader.Config.TargetOutput);
                body.AddRelativeTorque(0, output, 0);
                break;

            case MotorConfig.Types.ControlMode.MotionVelocity:
            case MotorConfig.Types.ControlMode.Velocity:
                break;

            case MotorConfig.Types.ControlMode.Position:
                break;
            }
        }
    }
Esempio n. 2
0
    /// <summary>
    /// This will run a motor through a pid loop to determine the output
    /// </summary>
    /// <param name="motor"></param>
    /// <param name="setPoint"></param>
    /// <param name="pv"></param>
    /// <returns></returns>
    double RunPid(SimMotor motor, double setPoint, double pv)
    {
        MotorConfig.Types.FPID fpid = motor.Config.Fpid[motor.Config.CurrentPidProfile];

        // calculate p
        double error = setPoint - pv;
        double pComp = error * fpid.P;

        // calculate the (iState or integralState)
        if (Math.Abs(error) <= fpid.IZone || fpid.IZone == 0.0f)
        {
            motor.IntegralState = motor.IntegralState + (error * fpid.I);
        }
        else
        {
            motor.IntegralState = 0;
        }

        // calculate the d component
        double dComp = (error - motor.LastError);

        motor.LastError = error;
        dComp          *= fpid.D;

        // calculate the f component
        double fComp = setPoint * fpid.F;

        // for a PID controller, the output is the sum of the components
        double output = pComp + motor.IntegralState + dComp + fComp;

        return(output);
    }
Esempio n. 3
0
    // Start is called before the first frame update
    void Start()
    {
        EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
        {
            Id     = 1,
            Handle = 100001,
        });
        EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
        {
            Id          = 2,
            Handle      = 100002,
            FollowingId = 1
        });
        EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
        {
            Id     = 3,
            Handle = 100003,
        });
        EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
        {
            Id          = 4,
            Handle      = 100004,
            FollowingId = 3
        });

        leftFrontMotor  = motorStore.GetSimMotor(100001);
        leftRearMotor   = motorStore.GetSimMotor(100002);
        rightFrontMotor = motorStore.GetSimMotor(100003);
        rightRearMotor  = motorStore.GetSimMotor(100004);
    }
Esempio n. 4
0
    private void MotorConfigUpdated(MotorConfig motorConfig)
    {
        SimMotor simMotor;

        if (!simMotorsByHandle.TryGetValue(motorConfig.Handle, out simMotor))
        {
            simMotor = new SimMotor()
            {
                Id     = motorConfig.Id,
                Handle = motorConfig.Handle
            };
            simMotors.Add(simMotor);
            simMotorsByHandle.Add(simMotor.Handle, simMotor);
            simMotorsById.Add(simMotor.Id, simMotor);
        }

        simMotor.Config = motorConfig;

        if (motorConfig.FollowingId != 0)
        {
            SimMotor following;
            if (simMotorsById.TryGetValue(motorConfig.FollowingId, out following))
            {
                simMotor.Following = following;
            }
        }
        else
        {
            simMotor.Following = null;
        }
    }
Esempio n. 5
0
    /// <summary>
    /// When this component gets a motor update, if it's the motor assigned
    /// to this component, update the SimMotor object
    /// </summary>
    /// <param name="motorConfig"></param>
    private void MotorConfigUpdated(MotorConfig motorConfig)
    {
        if (motorId != motorConfig.Id)
        {
            // this motor isn't assigned to us, ignore the update
            return;
        }

        simMotor = motorStore.CreateOrUpdateMotor(motorConfig);
    }
Esempio n. 6
0
    public SimMotor CreateOrUpdateMotor(MotorConfig motorConfig)
    {
        SimMotor simMotor;

        if (!simMotorsByHandle.TryGetValue(motorConfig.Handle, out simMotor))
        {
            simMotor = new SimMotor()
            {
                Id     = motorConfig.Id,
                Handle = motorConfig.Handle
            };
            // initialize the MotorOutput
            simMotors.Add(simMotor);
            simMotorsByHandle.Add(simMotor.Handle, simMotor);
            simMotorsById.Add(simMotor.Id, simMotor);
            Debug.Log("Created motor: " + simMotor.Id);
        }

        simMotor.Config = motorConfig;

        if (motorConfig.FollowingId != 0 && simMotor.Following == null)
        {
            SimMotor following;
            if (simMotorsById.TryGetValue(motorConfig.FollowingId, out following))
            {
                simMotor.Following = following;
                Debug.Log("motor " + motorConfig + " following " + following.Config);
            }
        }
        else
        {
            simMotor.Following = null;
        }

        return(simMotor);
    }