void TestPeriodic()
{
bool nowButton4 = stick->GetRawButton(4);
bool nowButton1 = stick->GetRawButton(1);
if (nowButton4 && !lastButton4) {
testingLeft = !testingLeft;
}
if (nowButton1 && !lastButton1) {
testingRight = !testingRight;
}
lastButton4 = nowButton4;
lastButton1 = nowButton1;
if (testingRight) {
rShooter->Set(kRightHighRPM);
std::cout << rShooter->GetSpeed() << std::endl;
} else {
rShooter->Set(0.0);
}
if (testingLeft) {
lShooter->Set(kLeftHighRPM);
std::cout << lShooter->GetSpeed() << std::endl;
} else {
lShooter->Set(0.0);
}
}
void stateLaunching() {
stateTimer++;
if (stateTimer == 1) {
if (shootingHigh) {
rShooter->Set(kRightHighRPM);
lShooter->Set(kLeftHighRPM);
} else {
rShooter->Set(kRightLowRPM);
lShooter->Set(kLeftLowRPM);
}
} else if (stateTimer > 50 && stateTimer <= 65) {
shooterInOut->Set(-1.0);
LOGGER(DEBUG) << "[stateLaunching] Timer: " << stateTimer << " Angle: " << launchPIDSource.PIDGet()
<< " Right RPM: " << rShooter->GetSpeed() << " Left RPM: " << lShooter->GetSpeed();
} else if (stateTimer > 65 && stateTimer <= 70) {
shooterInOut->Set(0.0);
} else if (stateTimer > 70 && stateTimer <= 75) {
shooterInOut->Set(0.6);
} else if (stateTimer > 75) {
shootingHigh = false;
stateTimer = 0;
rShooter->Set(0.0);
lShooter->Set(0.0);
shooterInOut->Set(0.0);
robotState = kOperatorControl;
}
}