uint [] _debLeftY = { 0, 0 }; // _debLeftY[0] is how many times leftY is zero, _debLeftY[1] is how many times leftY is not zeero. public void Run() { /* Factory Default all hardware to prevent unexpected behaviour */ _talon.ConfigFactoryDefault(); /* first choose the sensor */ _talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs); _talon.SetSensorPhase(false); /* set closed loop gains in slot0 */ _talon.Config_kP(0, 0.2f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */ _talon.Config_kI(0, 0f, kTimeoutMs); _talon.Config_kD(0, 0f, kTimeoutMs); _talon.Config_kF(0, 0f, kTimeoutMs); /* For position servo kF is rarely used. Leave zero */ /* use slot0 for closed-looping */ _talon.SelectProfileSlot(0, 0); /* set the peak and nominal outputs, 1.0 means full */ _talon.ConfigNominalOutputForward(0.0f, kTimeoutMs); _talon.ConfigNominalOutputReverse(0.0f, kTimeoutMs); _talon.ConfigPeakOutputForward(+1.0f, kTimeoutMs); _talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs); /* how much error is allowed? This defaults to 0. */ _talon.ConfigAllowableClosedloopError(0, 0, kTimeoutMs); /* put in a ramp to prevent the user from flipping their mechanism in open loop mode */ _talon.ConfigClosedloopRamp(0, kTimeoutMs); _talon.ConfigOpenloopRamp(1, kTimeoutMs); /* zero the sensor and throttle */ ZeroSensorAndThrottle(); /* loop forever */ while (true) { Loop10Ms(); //if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) // check if gamepad is plugged in OR.... if (_gamepad.GetButton(kEnableButton)) // check if bottom left shoulder buttom is held down. { /* then enable motor outputs*/ Watchdog.Feed(); } /* print signals to Output window */ Instrument(); /* 10ms loop */ Thread.Sleep(10); } }
private void SetupPositionServo() { TalonSRX armTalon = (TalonSRX)_gearBox.MasterMotorController; armTalon.ConfigNominalOutputForward(0); armTalon.ConfigNominalOutputReverse(0); armTalon.ConfigPeakOutputForward(1); armTalon.ConfigPeakOutputReverse(-1); armTalon.ConfigAllowableClosedloopError(0, Constants.TOLERANCE); armTalon.Config_kP(Constants.KPARM); armTalon.Config_kI(Constants.KIARM); armTalon.Config_kD(Constants.KDARM); _controlMode = ControlMode.MotionMagic; armTalon.SelectProfileSlot(0); armTalon.ConfigMotionAcceleration(60); armTalon.ConfigMotionCruiseVelocity(22); }
static void Initialize() { // Serial port _uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200); _uart.Open(); // Victor SPX Slaves // Left Slave victor1.Set(ControlMode.Follower, 0); // Right Slave victor3.Set(ControlMode.Follower, 2); // Talon SRX Slaves // Feeder Slave feederL.Set(ControlMode.Follower, 0); feederL.SetInverted(true); // Intake Slave intakeLft.Set(ControlMode.Follower, 2); intakeLft.SetInverted(true); // Hood hood.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog, 0, kTimeoutMs); hood.SetSensorPhase(false); hood.Config_kP(0, 30f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */ hood.Config_kI(0, 0.0005f, kTimeoutMs); hood.Config_kD(0, 0f, kTimeoutMs); hood.Config_kF(0, 0f, kTimeoutMs); /* use slot0 for closed-looping */ hood.SelectProfileSlot(0, 0); /* set the peak and nominal outputs, 1.0 means full */ hood.ConfigNominalOutputForward(0.0f, kTimeoutMs); hood.ConfigNominalOutputReverse(0.0f, kTimeoutMs); hood.ConfigPeakOutputForward(+1.0f, kTimeoutMs); hood.ConfigPeakOutputReverse(-1.0f, kTimeoutMs); hood.ConfigForwardSoftLimitThreshold(HOOD_LOWER_BOUND_ANALOG, kTimeoutMs); hood.ConfigReverseSoftLimitThreshold(HOOD_UPPER_BOUND_ANALOG, kTimeoutMs); hood.ConfigForwardSoftLimitEnable(true, kTimeoutMs); hood.ConfigReverseSoftLimitEnable(true, kTimeoutMs); /* how much error is allowed? This defaults to 0. */ hood.ConfigAllowableClosedloopError(0, 5, kTimeoutMs); //*********************** // MAY NEED TUNING //*********************** // Turret turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 1, kTimeoutMs); int absPos = turret.GetSelectedSensorPosition(1); turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs); turret.SetSelectedSensorPosition(0, 0, kTimeoutMs); turret.SetSensorPhase(true); turret.Config_IntegralZone(20); turret.Config_kP(0, 2f, kTimeoutMs); // tweak this first, a little bit of overshoot is okay turret.Config_kI(0, 0f, kTimeoutMs); turret.Config_kD(0, 0f, kTimeoutMs); turret.Config_kF(0, 0f, kTimeoutMs); // use slot0 for closed-looping turret.SelectProfileSlot(0, 0); // set the peak and nominal outputs, 1.0 means full turret.ConfigNominalOutputForward(0.0f, kTimeoutMs); turret.ConfigNominalOutputReverse(0.0f, kTimeoutMs); turret.ConfigPeakOutputForward(+0.5f, kTimeoutMs); turret.ConfigPeakOutputReverse(-0.5f, kTimeoutMs); turret.ConfigReverseSoftLimitThreshold(TURRET_UPPER_BOUND_ANALOG, kTimeoutMs); turret.ConfigForwardSoftLimitThreshold(TURRET_LOWER_BOUND_ANALOG, kTimeoutMs); turret.ConfigReverseSoftLimitEnable(true, kTimeoutMs); turret.ConfigForwardSoftLimitEnable(true, kTimeoutMs); // how much error is allowed? This defaults to 0. turret.ConfigAllowableClosedloopError(0, 5, kTimeoutMs); // Shooter shooterSensorTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs); shooterSensorTalon.SetSensorPhase(false); shooterSensorTalon.ConfigPeakOutputReverse(0.0f, kTimeoutMs); shooterSensorTalon.ConfigPeakOutputForward(1.0f, kTimeoutMs); shooterVESC = new PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7); shooterVESC.Set(0); pcm.SetSolenoidOutput(shooterFeedbackLEDPort, false); }