// Initialize all variables and start the main loop
public void Init()
{
controller = new Controller();
frontLeftMotor = new TalonSRX(1);
frontRightMotor = new TalonSRX(2);
backLeftMotor = new TalonSRX(3);
backRightMotor = new TalonSRX(4);
frontLeftMotor.SetNeutralMode(NeutralMode.Brake);
frontRightMotor.SetNeutralMode(NeutralMode.Brake);
backLeftMotor.SetNeutralMode(NeutralMode.Brake);
backRightMotor.SetNeutralMode(NeutralMode.Brake);
backLeftMotor.Follow(frontLeftMotor);
backRightMotor.Follow(frontRightMotor);
intakePivot = new TalonSRX(5);
intakeRoller = new TalonSRX(6);
disabled = false;
prevButton = false;
InitPeriodic();
}
private void robotInit()
{
js0 = new GameController(UsbHostDevice.GetInstance());
leftSlave1.Follow(leftMaster);
leftSlave2.Follow(leftMaster);
rightSlave1.Follow(rightMaster);
rightSlave2.Follow(rightMaster);
intake2.Follow(intake1);
rightMaster.SetInverted(true);
rightSlave1.SetInverted(true);
rightSlave2.SetInverted(true);
}
public static void Main()
{
//CSK 10/22/2019 .Follow method tells the follower motor to do whatever the lead motor does
//The
//Simplifies code a little bit
leftdrive2.Follow(leftdrive1, FollowerType.PercentOutput);
rightdrive2.Follow(rightdrive1, FollowerType.PercentOutput);
//brushlessTest.Follow(rightdrive1, FollowerType.PercentOutput);
#if (HASDISPLAY)
_AMLogo = _displayModule.AddResourceImageSprite(Properties.Resources.ResourceManager,
Properties.Resources.BinaryResources.andymark_logo_160x26,
Bitmap.BitmapImageType.Jpeg,
0, 0);
_labelTitle = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, 28, 160, 15);
_labelBtn = _displayModule.AddLabelSprite(_smallFont, DisplayModule.Color.Cyan, 30, 60, 100, 10);
_leftCrossHair = _displayModule.AddResourceImageSprite(Properties.Resources.ResourceManager,
Properties.Resources.BinaryResources.crosshair,
Bitmap.BitmapImageType.Jpeg,
lftCrossHairOrigin, 100);
_rightCrossHair = _displayModule.AddResourceImageSprite(Properties.Resources.ResourceManager,
Properties.Resources.BinaryResources.crosshair,
Bitmap.BitmapImageType.Jpeg,
rtCrossHairOrigin, 100);
_labelThrottle = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 0, 45, 75, 15);
_labelSteering = _displayModule.AddLabelSprite(_bigFont, DisplayModule.Color.White, 80, 45, 75, 15);
#endif
/* loop forever */
while (true)
{
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
/* feed watchdog to keep Talon's enabled */
CTRE.Phoenix.Watchdog.Feed();
}
/* drive robot using gamepad */
Drive();
}
}
public static void Main()
{
/* Disable drivetrain/motors */
_rightTalon.Set(ControlMode.PercentOutput, 0);
_leftTalon.Set(ControlMode.PercentOutput, 0);
#if (fourWheeled)
_rightFollower.Follow(_rightTalon);
_leftFollower.Follow(_leftTalon);
#endif
/* Configure output and sensor direction */
_rightTalon.SetInverted(true);
_leftTalon.SetInverted(false);
#if (fourWheeled)
_rightFollower.SetInverted(true);
_leftFollower.SetInverted(false);
#endif
/* Mode print */
Debug.Print("This is arcade drive using Arbitrary Feedforward");
bool Btn1 = false;
bool Btn2 = false;
bool Btn3 = false;
bool Btn4 = false;
bool Btn10 = false;
bool VoltageComp = false;
bool CurrentLimit = false;
bool NeutralState = false;
bool RampRate = false;
bool FirstCall = true;
while (true)
{
/* Enable motor controllers if gamepad connected */
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Gamepad Stick Control */
float forward = -1 * _gamepad.GetAxis(1);
float turn = 1 * _gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref forward);
CTRE.Phoenix.Util.Deadband(ref turn);
turn *= 0.5f; //Scaled down for safety
forward *= 0.75f;
bool btn1 = _gamepad.GetButton(1);
bool btn2 = _gamepad.GetButton(2);
bool btn3 = _gamepad.GetButton(3);
bool btn4 = _gamepad.GetButton(4);
bool btn10 = _gamepad.GetButton(10);
if (btn1 && !Btn1)
{
VoltageComp = !VoltageComp;
FirstCall = true;
}
else if (btn2 && !Btn2)
{
CurrentLimit = !CurrentLimit;
FirstCall = true;
}
else if (btn3 && !Btn3)
{
NeutralState = !NeutralState;
FirstCall = true;
}
else if (btn4 && !Btn4)
{
RampRate = !RampRate;
FirstCall = true;
}
else if (btn10 && !Btn10)
{
VoltageComp = false;
CurrentLimit = false;
NeutralState = false;
RampRate = false;
FirstCall = true;
}
Btn1 = btn1;
Btn2 = btn2;
Btn3 = btn3;
Btn4 = btn4;
Btn10 = btn10;
if (VoltageComp)
{
_rightTalon.ConfigVoltageCompSaturation(10, 10);
_rightTalon.ConfigVoltageMeasurementFilter(16, 10);
_rightTalon.EnableVoltageCompensation(true);
_leftTalon.ConfigVoltageCompSaturation(10, 10);
_leftTalon.ConfigVoltageMeasurementFilter(16, 10);
_leftTalon.EnableVoltageCompensation(true);
if (FirstCall)
{
Debug.Print("Voltage Compensation: On");
}
}
else
{
_rightTalon.EnableVoltageCompensation(false);
_leftTalon.EnableVoltageCompensation(false);
if (FirstCall)
{
Debug.Print("Voltage Compensation: Off");
}
}
if (CurrentLimit)
{
_rightTalon.ConfigContinuousCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentDuration(0, 10);
_rightTalon.EnableCurrentLimit(true);
_leftTalon.ConfigContinuousCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentDuration(0, 10);
_leftTalon.EnableCurrentLimit(true);
if (FirstCall)
{
Debug.Print("Current Limit: On");
}
}
else
{
_rightTalon.EnableCurrentLimit(false);
_leftTalon.EnableCurrentLimit(false);
if (FirstCall)
{
Debug.Print("Current Limit: Off");
}
}
if (NeutralState)
{
_rightTalon.SetNeutralMode(NeutralMode.Coast);
_leftTalon.SetNeutralMode(NeutralMode.Coast);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Coast);
_leftFollower.SetNeutralMode(NeutralMode.Coast);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Coast");
}
}
else
{
_rightTalon.SetNeutralMode(NeutralMode.Brake);
_leftTalon.SetNeutralMode(NeutralMode.Brake);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Brake);
_leftFollower.SetNeutralMode(NeutralMode.Brake);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Brake");
}
}
if (RampRate)
{
_rightTalon.ConfigOpenloopRamp(3, 0);
_leftTalon.ConfigOpenloopRamp(3, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: On, 3 Seconds");
}
}
else
{
_rightTalon.ConfigOpenloopRamp(0.0f, 0);
_leftTalon.ConfigOpenloopRamp(0.0f, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: Off, 0 Seconds");
}
}
/* Use Arbitrary FeedForward to create an Arcade Drive Control by modifying the forward output */
_rightTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, -turn);
_leftTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, +turn);
if (FirstCall)
{
Debug.Print("");
}
FirstCall = false;
Thread.Sleep(5);
}
}