/* Initialize Motors */ static void InitMotors() { /* Factory Default all hardware to prevent unexpected behaviour */ _Talon.ConfigFactoryDefault(); _victor.ConfigFactoryDefault(); /* Select Sensor */ _Talon.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.FeedbackDevice.QuadEncoder, 0, kTimeoutMs); _victor.ConfigRemoteFeedbackFilter(_Talon.GetDeviceID(), CTRE.Phoenix.MotorControl.RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, 0, kTimeoutMs); _victor.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.RemoteFeedbackDevice.RemoteFeedbackDevice_RemoteSensor0, 0, kTimeoutMs); _Talon.SetInverted(true); _victor.SetInverted(true); _Talon.SetSensorPhase(true); _victor.SetSensorPhase(true); /* Select Neutral Mode */ _Talon.SetNeutralMode(CTRE.Phoenix.MotorControl.NeutralMode.Brake); _victor.SetNeutralMode(CTRE.Phoenix.MotorControl.NeutralMode.Brake); /* Closed loop / Motion Magic Parameters */ _Talon.Config_kF(kSlotIdx, kF, kTimeoutMs); _Talon.Config_kP(kSlotIdx, kP, kTimeoutMs); _Talon.Config_kI(kSlotIdx, kI, kTimeoutMs); _Talon.Config_kD(kSlotIdx, kD, kTimeoutMs); _Talon.Config_IntegralZone(kSlotIdx, kIZone, kTimeoutMs); _Talon.SelectProfileSlot(kSlotIdx, 0); _Talon.ConfigNominalOutputForward(kNominalOuput, kTimeoutMs); _Talon.ConfigNominalOutputReverse(-(kNominalOuput), kTimeoutMs); _Talon.ConfigPeakOutputForward(kPeakOutput, kTimeoutMs); _Talon.ConfigPeakOutputReverse(-(kPeakOutput), kTimeoutMs); _Talon.ConfigMotionCruiseVelocity(kMotionVelocity, kTimeoutMs); _Talon.ConfigMotionAcceleration(kMotionAcceleration, kTimeoutMs); _Talon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_10_Targets, 10, kTimeoutMs); _Talon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_2_Feedback0, 10, kTimeoutMs); /* Closed loop / Motion Magic Parameters */ _victor.Config_kF(kSlotIdx, kF, kTimeoutMs); _victor.Config_kP(kSlotIdx, kP, kTimeoutMs); _victor.Config_kI(kSlotIdx, kI, kTimeoutMs); _victor.Config_kD(kSlotIdx, kD / 2, kTimeoutMs); _victor.Config_IntegralZone(kSlotIdx, kIZone, kTimeoutMs); _victor.SelectProfileSlot(kSlotIdx, 0); _victor.ConfigNominalOutputForward(kNominalOuput, kTimeoutMs); _victor.ConfigNominalOutputReverse(-(kNominalOuput), kTimeoutMs); _victor.ConfigPeakOutputForward(kPeakOutput, kTimeoutMs); _victor.ConfigPeakOutputReverse(-(kPeakOutput), kTimeoutMs); _victor.ConfigMotionCruiseVelocity(kMotionVelocity, kTimeoutMs); _victor.ConfigMotionAcceleration(kMotionAcceleration, kTimeoutMs); _victor.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_10_Targets, 10, kTimeoutMs); /* Set current position to 0, can be changed with button 4 */ _victor.SetSelectedSensorPosition(0, 0, kTimeoutMs); _Talon.SetSelectedSensorPosition(0, 0, kTimeoutMs); }
public static void Main() { /* Current setup is a CTRE MAG Encoder and a 128 CPR Opitcal Encoder, where printf's can be used to compare accuracy */ magTalon.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.FeedbackDevice.PulseWidthEncodedPosition, 0, kTimeout); anaTalon.ConfigSelectedFeedbackSensor(CTRE.Phoenix.MotorControl.FeedbackDevice.QuadEncoder, 0, kTimeout); magTalon.ConfigSelectedFeedbackCoefficient(0.125f, 0, kTimeout); /* Mag Encoder 4096 CPR * 0.125 = 128 == 128 CPR Opitcal Encoder, could scale up */ anaTalon.ConfigSelectedFeedbackCoefficient(1, 0, kTimeout); anaTalon.SetSensorPhase(false); magTalon.SetInverted(true); magTalon.SetSensorPhase(true); /* Reduce the velocity averaging to generate useful plots in Vehicle spy */ magTalon.ConfigVelocityMeasurementWindow(1, kTimeout); magTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_1Ms, kTimeout); anaTalon.ConfigVelocityMeasurementWindow(1, kTimeout); anaTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_1Ms, kTimeout); /* Increase the rate of the CAN frame */ magTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_2_Feedback0, 4, kTimeout); anaTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_2_Feedback0, 4, kTimeout); /* For PWM test, scale down motor so optical has a chance to keep up? */ float peakOutput = 1f; float nominalOutput = 0; magTalon.ConfigPeakOutputForward(peakOutput, kTimeout); magTalon.ConfigPeakOutputReverse(-peakOutput, kTimeout); magTalon.ConfigNominalOutputForward(nominalOutput, kTimeout); magTalon.ConfigNominalOutputReverse(-nominalOutput, kTimeout); differenceDisplay = displayModule.AddLabelSprite(gFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Orange, 0, 0, 100, 15); statusDisplay = displayModule.AddLabelSprite(gFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Green, 0, 20, 100, 15); errorDisplay = displayModule.AddLabelSprite(gFont, CTRE.Gadgeteer.Module.DisplayModule.Color.Red, 0, 60, 100, 15); statusDisplay.SetText("Gamepad Mode"); errorDisplay.SetText("None"); /* Clear Position */ SetPositon(0); /* Variables to test various features */ bool lastBtn = false; bool lastBtn3 = false; bool state = false; float output = 0; while (true) { /* CTRE Output enable */ if (gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected) { CTRE.Phoenix.Watchdog.Feed(); } /* Encoder Postion taring */ bool btn = gamepad.GetButton(2); bool btn3 = gamepad.GetButton(3); if (btn && !lastBtn) { SetPositon(0); errorDisplay.SetText("Error: " + 0); } if (btn3 && !lastBtn3) { state = !state; if (state) { stopwatch.Start(); statusDisplay.SetText("Sine Mode"); } else { statusDisplay.SetText("Gamepad Mode"); } } lastBtn = btn; lastBtn3 = btn3; if (!state) { /* Joypad value */ output = gamepad.GetAxis(1); /* Reduce speed if right trigger is held down */ if (gamepad.GetButton(6)) { output *= 0.50f; } } else { float amplitude = 1; float frequencyHZ = 0.2f; float time = stopwatch.Duration; output = amplitude * (float)System.Math.Sin((2 * System.Math.PI) * frequencyHZ * time); } byte[] Frame = new byte[8]; Frame[0] = (byte)((int)(output * 1000) >> 8); Frame[1] = (byte)((int)(output * 1000) & 0xFF); ulong data = (ulong)BitConverter.ToUInt64(Frame, 0); CTRE.Native.CAN.Send(0x09, data, 8, 0); /* (scale for maximum rpm @3000 - 6900ish) */ output *= (1 / 5f); /* Talon with CTRE Mag Encoder drives the Talon */ magTalon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, output); /* Telemetry */ int magTemp = magTalon.GetSelectedSensorPosition(0); int anaTemp = anaTalon.GetSelectedSensorPosition(0); int difference = magTemp - anaTemp; Debug.Print("mag: " + magTemp + " ana: " + anaTemp + " dif: " + difference); differenceDisplay.SetText("Dif: " + difference); if (output > -0.05 && output < 0.05) { /* We are slow enough to check */ if (System.Math.Abs(difference) > 50) { /* Disruption in position */ state = false; statusDisplay.SetText("Bad Postion"); errorDisplay.SetText("Error: " + difference); } } else if (System.Math.Abs(difference) > 500) { /* Disruption in position */ state = false; statusDisplay.SetText("Bad Postion"); errorDisplay.SetText("Error: " + difference); } /* Allow some breathing room for the CAN Frames */ Thread.Sleep(5); } }