public void DriveDistance(double distance)
{
//double position = (distance * 12) / (4 * Math.PI);
//RightPrimary.SetPosition(position);
//RightSecondary.SetPosition(position);
//LeftPrimary.SetPosition(position);
//LeftSecondary.SetPosition(position);
WPILib.Timer timer = new WPILib.Timer();
timer.Reset();
timer.Start();
while (timer.Get() < distance)
{
RightPrimary.Set(.5);
RightSecondary.Set(.5);
LeftPrimary.Set(-.5);
LeftSecondary.Set(-.5);
}
timer.Stop();
RightPrimary.Set(0);
RightSecondary.Set(0);
LeftPrimary.Set(0);
LeftSecondary.Set(0);
}
/// <summary>
/// Basic Constructor for the Shooting Mechanism
/// </summary>
/// <param XboxController="nxbox"></param>
/// <param name="nxbox"></param>
public TimShooter(XboxController nxbox)
{
_t = null;
xbox = nxbox;
mode = ControlMode.Disabled;
one = new Jaguar(5);
two = new Jaguar(6);
elevator = new Victor(7);
trigger = new DoubleSolenoid(0, 1);
Zero();
}
/// <summary>
/// Creates a new PID object with the given contants for P, I, D and F.
/// </summary>
/// <param name="kp">The proportional coefficient.</param>
/// <param name="ki">The integral coefficient</param>
/// <param name="kd">The derivative coefficient</param>
/// <param name="kf">The feed forward term.</param>
/// <param name="source">The PIDSource object that is used to get values.</param>
/// <param name="output">The PIDOutput object that is set to the output percentage.</param>
/// <param name="period">The loop time for doing calculations.</param>
public PIDController(double kp, double ki, double kd, double kf,
IPIDSource source, IPIDOutput output,
double period)
{
if (source == null)
throw new ArgumentNullException(nameof(source), "Null PIDSource was given");
if (output == null)
throw new ArgumentNullException(nameof(output), "Null PIDOutput was given");
CalculateCallback = Calculate;
m_controlLoop = new Notifier(CalculateCallback);
m_setpointTimer = new Timer();
m_setpointTimer.Start();
m_P = kp;
m_I = ki;
m_D = kd;
m_F = kf;
PIDInput = source;
PIDOutput = output;
m_period = period;
m_controlLoop.StartPeriodic(m_period);
s_instances++;
HLUsageReporting.ReportPIDController(s_instances);
m_toleranceType = ToleranceType.NoTolerance;
m_buf = new Queue<double>();
}
/// <summary>
/// Reads controller input we can make different methods based on different times and places and to save previous attemps
/// </summary>
private void ReadController()
{
//Controls the main spining wheels
if(Math.Abs(_wheelSet) < .0005)
{
if (xbox.GetRightBumper()) _wheelSet += .001;
}
else if( Math.Abs(_wheelSet - 1) < .005)
{
if (xbox.GetLeftBumper()) _wheelSet -= 0.001;
}
else
{
if (xbox.GetLeftBumper()) _wheelSet -= 0.001;
if (xbox.GetRightBumper()) _wheelSet += .001;
}
switch (_fstate)
{
case FireingState.ReadyToFire:
SmartWriter.WriteString("Frisbee Fireing State", "Ready To Fire", DebugMode.Competition);
if (xbox.GetRightTrigger() > .5)
{
_t = new Timer();
_t.Start();
_triggerSet = DoubleSolenoid.Value.Forward;
_fstate = FireingState.Fireing;
}
break;
case FireingState.Fireing:
SmartWriter.WriteString("Frisbee Fireing State", "Fireing", DebugMode.Competition);
if (_t.Get() > 1)
{
_t.Reset();
_fstate = FireingState.Reloading;
}
break;
case FireingState.Reloading:
SmartWriter.WriteString("Frisbee Fireing State", "Reloading", DebugMode.Competition);
if (_t.Get() > 2)
{
_t = null;
_fstate = FireingState.ReadyToFire;
}
break;
}
}
