private void Start() //Sets motor speed
{
//Lower sensor says when a ball passes through a the tube and incruments the ball
//upper sensor tells us tha twe have balls in the tube.
Robotmap map = Robotmap.GetInstance();
transferMotor.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, transferMotorSpeed);
bool ballPresent = IsLowerSensorTripped();
if (ballPresent)
{
numberOfBalls += 1;
}
bool ballPresentTop = IsUpperSensorTripped();
if (!ballPresentTop && numberOfBalls > 0)
{
numberOfBalls = 0;
}
if (numberOfBalls == 0)
{
glowing.set_color(LED.MECHANISM_LED.GREEN);
eject.setState(Intake.INTAKESTATE.Sweep);
}
else if (numberOfBalls == 1)
{
glowing.set_color(LED.MECHANISM_LED.YELLOW);
eject.setState(Intake.INTAKESTATE.Sweep);
}
else
{
glowing.set_color(LED.MECHANISM_LED.RED);
eject.setState(Intake.INTAKESTATE.Expel);
}
}
public void BallHandler()
{
bool deliver = !m_gamepad.GetButton(Y_BUTTON);
bool runintake = m_gamepad.GetButton(LEFT_BUMPER); //whether the Intake button is pressed. bool is true or false
bool runexpel = m_gamepad.GetButton(RIGHT_BUMPER); // Whether if the Expel button is pressed is true or false
if (runintake) // If/else if/else statement
{
m_intake.setState(Intake.INTAKESTATE.Sweep); // If the Sweep button is pressed then it will sweep
m_transfer.SetState(Transfer.TRANSFER_STATE.TRANSFER_ON);
}
else if (runexpel)
{
m_intake.setState(Intake.INTAKESTATE.Expel); // Else if the Expel button is pressed then it will expel
m_transfer.SetState(Transfer.TRANSFER_STATE.TRANSFER_EXPEL);
}
else
{
m_intake.setState(Intake.INTAKESTATE.Off); // else no button is pressed then the sweep will be off.
m_transfer.SetState(Transfer.TRANSFER_STATE.TRANSFER_OFF);
}
if (deliver)
{
m_deliver.setState(Deliver.DELIVERSTATE.Deliver);
}
else
{
m_deliver.setState(Deliver.DELIVERSTATE.HoldBalls);
}
m_intake.Run();
m_transfer.Run();
if (prevDeliver != deliver)
{
m_deliver.Run();
}
prevDeliver = deliver;
}