50 lines
1.4 KiB
C++
50 lines
1.4 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "commands/Auto.h"
|
|
|
|
#include <frc/Timer.h>
|
|
|
|
Auto::Auto(Chassis &chassis, Shooter &shooter, Gatherer &gatherer) :
|
|
m_chassis(chassis), m_shooter(shooter), m_gatherer(gatherer) {
|
|
// Use addRequirements() here to declare subsystem dependencies.
|
|
SetName("Auto");
|
|
AddRequirements({&m_chassis, &m_shooter, &m_gatherer});
|
|
}
|
|
|
|
// Called when the command is initially scheduled.
|
|
void Auto::Initialize() {
|
|
m_start_time = frc::Timer::GetFPGATimestamp();
|
|
}
|
|
|
|
// Called repeatedly when this Command is scheduled to run
|
|
void Auto::Execute() {
|
|
auto cur_time = frc::Timer::GetFPGATimestamp();
|
|
auto time = cur_time - m_start_time;
|
|
m_shooter.set(0.95_rpm);
|
|
if(time > 7_s) {
|
|
m_chassis.drive_field_relative(0_mps, 0_mps, 0_rad_per_s);
|
|
m_shooter.set(0_rpm);
|
|
m_gatherer.stop();
|
|
} else if(time > 4_s) {
|
|
auto speeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
|
|
1.5_mps,
|
|
0_mps,
|
|
0_rad_per_s,
|
|
frc::Rotation2d(units::degree_t(0)));
|
|
m_chassis.drive(speeds);
|
|
} else if(time > 2_s) {
|
|
m_gatherer.shoot(false);
|
|
}
|
|
}
|
|
|
|
// Called once the command ends or is interrupted.
|
|
void Auto::End(bool interrupted) {
|
|
m_gatherer.stop();
|
|
}
|
|
|
|
// Returns true when the command should end.
|
|
bool Auto::IsFinished() {
|
|
return false;
|
|
}
|