public void Run()
{
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);
_talon.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
_talon.SetSensorDirection(false);
_talon.SetVoltageRampRate(0.0f);
_talon.SetP(0, 0.80f);
_talon.SetI(0, 0f);
_talon.SetD(0, 0f);
_talon.SetF(0, 0.09724488664269079041176191004297f);
_talon.SelectProfileSlot(0);
_talon.ConfigNominalOutputVoltage(0f, 0f);
_talon.ConfigPeakOutputVoltage(+12.0f, -12.0f);
_talon.ChangeMotionControlFramePeriod(5);
/* loop forever */
while (true)
{
_talon.GetMotionProfileStatus(out _motionProfileStatus);
Drive();
CTRE.Watchdog.Feed();
Instrument();
Thread.Sleep(5);
}
}
/**
* Zero the sensor and zero the throttle.
*/
void ZeroSensorAndThrottle()
{
_talon.SetPosition(0); /* start our position at zero, this example uses relative positions */
_targetPosition = 0;
/* zero throttle */
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
_talon.Set(0);
Thread.Sleep(100); /* wait a bit to make sure the Setposition() above takes effect before sampling */
}
/** spin in this routine forever */
public void RunForever()
{
/* config our talon, don't continue until it's successful */
int initStatus = SetupConfig(); /* configuration */
while (initStatus != 0)
{
Instrument.PrintConfigError();
initStatus = SetupConfig(); /* (re)config*/
}
/* robot loop */
while (true)
{
/* get joystick params */
float leftY = -1f * _gamepad.GetAxis(1);
bool btnTopLeftShoulder = _gamepad.GetButton(5);
bool btnBtmLeftShoulder = _gamepad.GetButton(7);
Deadband(ref leftY);
/* keep robot enabled if gamepad is connected and in 'D' mode */
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* set the control mode based on button pressed */
if (btnTopLeftShoulder)
{
_mode = CTRE.TalonSrx.ControlMode.kPercentVbus;
}
if (btnBtmLeftShoulder)
{
_mode = CTRE.TalonSrx.ControlMode.kMotionMagic;
}
/* calc the Talon output based on mode */
if (_mode == CTRE.TalonSrx.ControlMode.kPercentVbus)
{
float output = leftY; // [-1, +1] percent duty cycle
_talon.SetControlMode(_mode);
_talon.Set(output);
}
else if (_mode == CTRE.TalonSrx.ControlMode.kMotionMagic)
{
float servoToRotation = leftY * 10;// [-10, +10] rotations
_talon.SetControlMode(_mode);
_talon.Set(servoToRotation);
}
/* instrumentation */
Instrument.Process(_talon);
/* wait a bit */
System.Threading.Thread.Sleep(5);
}
}
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);
}
}
//I think this is where the magic happens and the motors move
private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
{
//For each setpointData, for the talon that matches it set each mode and setpoint based on the list information
for (int i = 0; i < setpointDataList.Count; i++)
{
try
{
SetpointData setpointData = (SetpointData)setpointDataList[i];
float setpointVal = (float)(setpointData.getSetpoint());
CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
if (talon.GetControlMode() != setpointData.getMode())
{
talon.SetControlMode(setpointData.getMode());
//Debug.Print(setpointData.getMode().ToString());
}
if (talon.GetSetpoint() != setpointVal)
{
talon.Set(setpointVal);
Debug.Print("Setting it to value = " + setpointVal.ToString());
//Debug.Print(setpointVal.ToString());
}
}
catch (ArgumentOutOfRangeException ex)
{
Debug.Print(ex.ToString());
}
}
}
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);
}
//Password to module wifi is "password1"
/** entry point of the application */
public static void Main()
{
ds = new CTRE.FRC.DriverStation(wifiport); //Create new Driver station object at port 4
_gamepad = new CTRE.Controller.GameController(ds, 0); //Set controller to look at DS with ID 0
//Create two TalonSrx objects and set their control mode
left1 = new CTRE.TalonSrx(1);
right1 = new CTRE.TalonSrx(2);
left1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
right1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
//Set IP module looks for and is configured with, Computer must set to Static IP
ds.SendIP(new byte[] { 10, 0, 33, 2 }, new byte[] { 10, 0, 33, 5 });
while (true)
{
//Must be called for DS to function
ds.update();
//Send Battery Voltage information to Driver Station
ds.SendBattery(left1.GetBusVoltage());
if (ds.IsAuton() && ds.IsEnabled())
{
//Auton Code while enabled
}
else if (ds.IsAuton() && !ds.IsEnabled())
{
//Auton Code while disabled
}
else if (!ds.IsAuton() && ds.IsEnabled())
{
//Teleop Code while enabled
Drive();
Debug.Print(stringBuilder.ToString());
ds.SendUDP(2550, Encoding.UTF8.GetBytes(stringBuilder.ToString()));
stringBuilder.Clear();
}
else if (!ds.IsAuton() && !ds.IsEnabled())
{
//Teleop Code while disabled
}
}
}
public static void Main()
{
CTRE.Gamepad _gamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
//0 - left x
//1 - left y
//2 - right x
//
//5 - LB
//6 - RB
ledSpike.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);
ledSpike.SetCurrentLimit(1);
while (true)
{
float y = -_gamepad.GetAxis(1);
float turn = _gamepad.GetAxis(2);
Deadband(ref y);
Deadband(ref turn);
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
if (_gamepad.GetButton(5))
{
ArcadeDrive(y, turn);
/* feed watchdog to keep Talon's enabled */
CTRE.Watchdog.Feed();
}
if (_gamepad.GetButton(6))
{
ledSpike.Set(3.5f);
CTRE.Watchdog.Feed();
}
else
{
ledSpike.Set(0);
}
}
/* run this task every 10ms */
Thread.Sleep(10);
}
}
private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
{
for (int i = 0; i < setpointDataList.Count; i++)
{
SetpointData setpointData = (SetpointData)setpointDataList[i];
float setpointVal = (float)(setpointData.getSetpoint());
CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
if (talon.GetControlMode() != setpointData.getMode())
{
talon.SetControlMode(setpointData.getMode());
Debug.Print(setpointData.getMode().ToString());
}
if (talon.GetSetpoint() != setpointVal)
{
talon.Set(setpointVal);
Debug.Print(setpointVal.ToString());
}
}
}
