void DisabledPeriodic() {
dashboardLogger->UpdateData();
//robot->UpdateCurrent();
//auton->Stop();
//Reads controls and updates controllers accordingly
//lights->Update(false);
visionController->Update();
humanControl->ReadControls();
gearController->Update();
RefreshAllIni();
}
void TeleopPeriodic() {
dashboardLogger->UpdateEssentialData();
//Updates timer
lastTimeSec = currTimeSec;
currTimeSec = robot->GetTime();
deltaTimeSec = currTimeSec - lastTimeSec;
//Reads controls and updates controllers accordingly
//RefreshAllIni();
humanControl->ReadControls();
driveController->Update(currTimeSec, deltaTimeSec);
climberController->Update();
//visionController->Update();
gearController->Update();
if (humanControl->GetJoystickValue(RemoteControl::kOperatorJoy, RemoteControl::kRY) > 0.2) {
lights->Climbing();
} else if (humanControl->GetShoutRoutineDesired()) {
lights->SetShoutRoutine();
} else if (humanControl->GetGearTitlerIntakeDesired()) {
lights->GearIntake();
} else if (humanControl->GetGearTitlerOuttakeDesired()) {
lights->GearOuttake();
} else if (humanControl->GetSlowDriveTier1Desired()
&& humanControl->GetSlowDriveTier2Desired()) {
lights->Brake2();
} else if (humanControl->GetSlowDriveTier1Desired()) {
lights->Brake1();
} else {
lights->SetEnabledRoutine();
}
}