public static void Main()
{
leftDrive = new CTRE.TalonSrx(1);
rightDrive = new CTRE.TalonSrx(2);
leftDrive.SetInverted(true);
leftDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
rightDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
leftDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
rightDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
leftDrive.ConfigEncoderCodesPerRev(560);
rightDrive.ConfigEncoderCodesPerRev(560);
leftDrive.SetSensorDirection(true);
leftDrive.SetEncPosition(0);
rightDrive.SetEncPosition(0);
CTRE.Gamepad gamePad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
/* loop forever */
while (true)
{
if (gamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CheesyDrive(gamePad.GetAxis(1), gamePad.GetAxis(2));
CTRE.Watchdog.Feed();
Debug.Print("" + leftDrive.GetPosition() + ":" + rightDrive.GetPosition());
}
else
{
Debug.Print("No Driver Pad");
}
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public override Int32 Begin()
{
#if DEBUG
Debug.Print(ToString() + " [BEGIN]");
#endif
/* Setup Left and Right followers */
RightSlave.SetControlMode(CTRE.TalonSrx.ControlMode.kFollower);
RightSlave.Set(RIGHT_TALONSRX_ID);
LeftSlave.SetControlMode(CTRE.TalonSrx.ControlMode.kFollower);
LeftSlave.Set(LEFT_TALONSRX_ID);
/* Configure the Left TalonSRX */
Left.SetInverted(LEFT_INVT);
Left.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
Left.ConfigFwdLimitSwitchNormallyOpen(true);
Left.ConfigRevLimitSwitchNormallyOpen(true);
LeftSlave.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
LeftSlave.ConfigFwdLimitSwitchNormallyOpen(true);
LeftSlave.ConfigRevLimitSwitchNormallyOpen(true);
/* Configure the Right TalonSRX */
Right.SetInverted(!LEFT_INVT);
Right.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
Right.ConfigFwdLimitSwitchNormallyOpen(true);
Right.ConfigRevLimitSwitchNormallyOpen(true);
RightSlave.ConfigLimitMode(CTRE.TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
RightSlave.ConfigFwdLimitSwitchNormallyOpen(true);
RightSlave.ConfigRevLimitSwitchNormallyOpen(true);
if (USE_SPEED_MODE)
{
/* Setup Left and Right leaders with PID */
Right.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
Right.SetSensorDirection(false);
Right.ConfigEncoderCodesPerRev(RIGHT_EncTPR);
Right.SetControlMode(CTRE.TalonSrx.ControlMode.kSpeed);
Right.SetPID(0, RIGHT_P, RIGHT_I, RIGHT_D);
Right.SetF(0, RIGHT_F);
Left.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder);
Left.SetSensorDirection(false);
Left.ConfigEncoderCodesPerRev(LEFT_EncTPR);
Left.SetControlMode(CTRE.TalonSrx.ControlMode.kSpeed);
Left.SetPID(0, LEFT_P, LEFT_I, LEFT_D);
Left.SetF(0, LEFT_F);
}
/* Enable the TalonSRXs */
Right.Enable();
RightSlave.Enable();
Left.Enable();
LeftSlave.Enable();
return(0);
}
const float JOYSTICK_DEADBAND_VALUE = .1f; // TBD
//////////////////////// Function to Invert the Motors ////////////////////////
static void InitializeMotors()
{
// Inverting Each Motors If Necessary
// True -> Invert
// False -> Don't Invert
frontMotor.SetInverted(FRONT_MOTOR_INVERTED);
backRightMotor.SetInverted(BACK_RIGHT_MOTOR_INVERTED);
backLeftMotor.SetInverted(BACK_LEFT_MOTOR_INVERTED);
}
