void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* scale the x-axis to go from 0 to sensorRange, left to right */
float leftAxisX = (((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2));
float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2);
Deadband(ref rightAxisX);
if (rightAxisX != 0)
{
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
_talon.Set(rightAxisX);
}
else if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kPercentVbus)
{
_targetPosition = _talon.GetPosition();
/* user has let go of the stick, lets closed-loop whereever we happen to be */
EnableClosedLoop();
}
/* When you press the 'A' button on a Logitech Gamepad
* and the enable button is pressed */
if (_btns[2] && !_btnsLast[2] && _gamepad.GetButton(kEnableButton))
{
_targetPosition = servo(leftAxisX, _talon.GetPosition(), _sensorRange);
EnableClosedLoop();
}
}
public static void Main()
{
CTRE.UsbHostDevice.GetInstance().SetSelectableXInputFilter(CTRE.UsbHostDevice.SelectableXInputFilter.BothDInputAndXInput);
/* loop forever */
while (true)
{
/* get buttons */
for (uint i = 1; i < 20; ++i)
{
Buttons[i].last = Buttons[i].now;
Buttons[i].now = _gamepad.GetButton(i);
}
/* if button one was pressed, toggles field oriented*/
FieldOriented1 = Buttons[0].WasPressed() ? !FieldOriented1 : FieldOriented1;
/* kills drivetrain if button 2 is pressed or held */
if (Buttons[1].now)
{
motorA.Set(0);
motorB.Set(0);
motorC.Set(0);
motorA.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);
motorB.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);
motorC.ConfigNeutralMode(CTRE.TalonSrx.NeutralMode.Brake);
EStop = true;
}
/* turns off estop mode if button 10 was pressed*/
if (Buttons[9].WasPressed())
{
EStop = false;
motorA.ConfigNeutralMode(motorAInitialNeutralMode);
motorB.ConfigNeutralMode(motorBInitialNeutralMode);
motorC.ConfigNeutralMode(motorCInitialNeutralMode);
}
/* resets yaw if button six was pressed*/
if (Buttons[5].WasPressed())
{
try
{
Pidgy.SetYaw(0);
}
catch (Exception) { }
}
/* adjust orientation by 120 degrees if button three was pressed*/
Orientation += Buttons[2].WasPressed() ? (float)System.Math.PI * 2 / 3 : 0;
/*adjust orientation back by 120 degrees if button four was pressed*/
Orientation -= Buttons[3].WasPressed() ? (float)System.Math.PI * 2 / 3 : 0;
/* adjust orientation by 180 degrees if button five was pressed*/
Orientation += Buttons[4].WasPressed() ? (float)System.Math.PI : 0;
/* drive robot using gamepad */
Drive();
/* print whatever is in our string builder */
Debug.Print(stringBuilder.ToString());
stringBuilder.Clear();
/* feed watchdog to keep Talon's enabled */
CTRE.Watchdog.Feed();
/* run this task every 20ms */
Thread.Sleep(20);
}
}
public static void Main()
{
CTRE.TalonSrx talon = new CTRE.TalonSrx(0);
talon.SetControlMode(TalonSrx.ControlMode.kVoltage);
talon.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative);
talon.SetSensorDirection(false);
talon.SetVoltageRampRate(0.0f);
talon.EnableCurrentLimit(false);
talon.ConfigLimitMode(TalonSrx.LimitMode.kLimitMode_SrxDisableSwitchInputs);
double time_per_voltage = 20;
double[] voltages = new double[] { 0, 7 };
bool time_out = false;
bool continuous = false;
/* simple counter to print and watch using the debugger */
int counter = 0;
/* loop forever */
double time_last = DateTime.Now.Hour * 3600 + DateTime.Now.Minute * 60 + DateTime.Now.Second + DateTime.Now.Millisecond / 1000.0;
while (true)
{
/* print the three analog inputs as three columns */
//Debug.Print("Counter Value: " + counter);
CTRE.Watchdog.Feed();
/* increment counter */
++counter; /* try to land a breakpoint here and hover over 'counter' to see it's current value. Or add it to the Watch Tab */
/* wait a bit */
//using(System.IO.StreamWriter file = new System.IO.StreamWriter(@"C:\Users\Public\TestFolder\data.srv"))
//{
double seconds = DateTime.Now.Hour * 3600 + DateTime.Now.Minute * 60 + DateTime.Now.Second + DateTime.Now.Millisecond / 1000.0;
UInt64 index = (UInt64)System.Math.Floor((seconds - time_last) / time_per_voltage);
double voltage = 0;
if (continuous)
{
if (index < (UInt64)voltages.Length - 1)
{
double value = (seconds - time_last) / time_per_voltage;
voltage = (voltages[index + 1] - voltages[index]) * (value - index) + voltages[index];
}
else if (!time_out)
{
voltage = voltages[voltages.Length - 1];
}
}
else
{
if (index < (UInt64)voltages.Length)
{
voltage = voltages[index];
}
else if (!time_out)
{
voltage = voltages[voltages.Length - 1];
}
}
talon.Set((float)voltage); //low ish voltage
//CTRE.TalonSrx.VelocityMeasurementPeriod.Period_100Ms period;
Debug.Print(seconds.ToString() + "," + talon.GetPosition().ToString() + "," + talon.GetSpeed().ToString() + "," + talon.GetOutputVoltage().ToString()
+ "," + talon.GetOutputCurrent().ToString() + "," + talon.GetBusVoltage().ToString() + "," + talon.GetTemperature().ToString());
//talon.SetVelocityMeasurementPeriod(CTRE.TalonSrx.VelocityMeasurementPeriod.Period_10Ms);
// file.WriteLine(seconds.ToString() + "," + talon.GetPosition().ToString() + "," + talon.GetSpeed().ToString());
//}
System.Threading.Thread.Sleep(10);
}
}
