void ShootCommand::Initialize() {
m_done = false;
m_ticks = 0;
double currentYaw = navXSubsystem->GetYaw();
double yawError = fabs(currentYaw - Robot::end_of_centering_yaw);
if(yawError > 180){ // get the shorter angle
yawError = yawError - 180;
}
if (/*!wedgeSubsystem->isWedgeDown()
&& */!intakeSubsystem->isIntakeArmUp()) {
m_done = true;
printf("ShooterCommand: not safe to fire\n");
} else if(GetGroup() != NULL && yawError > 5.0){
// if this isn't the "plain" shoot command but part of ShootSequence
// and we were hit, don't shoot
// bumps should be well over 5 degrees; we don't want a potential
// sudden 1 degree drift that we saw on 2nd robot to screw us up
m_done = true;
printf("ShooterCommand: robot is hit; not shooting\n");
} else {
// turns on the shooting animation on the same tick as the actual shooter fire
// opponents should have no time to react
Robot::instance->SetLeds(Robot::SHOOT);
}
SetInterruptible(false);
}
// Called just before this Command runs the first time
void RightOnly::Initialize() {
SetInterruptible(false);
m_startPosition = RobotMap::driveBackRight->GetPosition();
printf("Right Only initialized for %1.2f inches.\n", m_inches);
// Disable the voltage ramp rate
RobotMap::driveBackLeft->SetVoltageRampRate(0.0);
RobotMap::driveBackRight->SetVoltageRampRate(0.0);
}
UnloadShooter::UnloadShooter(): Command() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
Requires(Robot::mainSubsystem.get());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
SetInterruptible(true);
finish = false;
}
ShootCommand::ShootCommand() :
CommandBase("ShootCommand_") {
m_done = false;
m_ticks = 0;
Requires(shooterSubsystem.get());
Requires(wedgeSubsystem.get());
Requires(intakeSubsystem.get());
SetInterruptible(false);
}
LauncherTriggerCommand::LauncherTriggerCommand() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
Requires(Robot::launcher);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
SetTimeout(0.6);
SetInterruptible(false);
}
Turn::Turn(Drive_t *drive, double velocity):
drive_(drive), velocity_(velocity), length_(0.0), type_(FOREVER)
{
Requires(drive_);
SetInterruptible(true);
if(velocity > 1.0) velocity = 1.0;
if(velocity < -1.0) velocity = -1.0;
}
// Called just before this Command runs the first time
void DrivePID::Initialize() {
SetTimeout(6000); // set 8 second timeout. Good enough?
_timeticks = 0;
SetInterruptible(false);
Robot::parameters->UpdateDrivePIDParams();
// Vestigial:
// double distance = Parameters::drivePIDDistance;
printf("drivePID initialized for leftTicks=%1.2f, rightTicks=%1.2f.\n", _leftTicks, _rightTicks);
Robot::driveSubsystem->SetPIDDistance(_leftTicks, _rightTicks);
}
AngelChange::AngelChange(float target, float timeout) :
CommandBase(CommandBase::createNameFromFloat("AngleChange", target)) {
Requires(pterodactyl);
this->target = target;
this->stability = 0;
this->tmpTarget = 0;
this->angleThreshold = PTERODACTYL_ANGLE_THRESHOLD;
SetTimeout(timeout);
SetInterruptible(true);
}
Turn::Turn(Drive_t *drive, double velocity, double length, bool in_seconds):
drive_(drive), velocity_(velocity), length_(fabs(length)), type_(in_seconds? SECONDS:REVOLUTIONS)
{
Requires(drive_);
SetInterruptible(true);
if(velocity > 1.0) velocity = 1.0;
if(velocity < -1.0) velocity = -1.0;
if(type_ == SECONDS) SetTimeout(length_);
}
SelectCamera::SelectCamera(Cameras::CameraDirection direction)
{
// Use Requires(&*) here to declare subsystem dependencies
// eg. Requires(&*chassis);
Requires(&*cameras);
SetInterruptible(true);
SetRunWhenDisabled(true);
camera_direction = direction;
is_finished = false;
}
void TurnCommand::Initialize() {
printf("TurnCommand::Initialize\n");
// make sure it doesn't get stuck turning forever if something fails
SetTimeout(2);
SetInterruptible(false);
double currentYaw = Subsystems::pYawSensor->getYaw();
if (currentYaw == YawSensor::YAW_INVALID) {
printf("Warning: No way to find yaw; using dead reckoning\n");
// todo: dead reckoning
SetTimeout(fabs(targetDeltaAngle) / M_PI);
} else {
currentAngle = Subsystems::pYawSensor->getYaw() * M_PI / 180;
startAngle = currentAngle;
}
}
Shoot::Shoot() {
Requires(pusher);
SetInterruptible(false);
SetTimeout(0.3);
}
Wait2Command::Wait2Command(double duration): CommandBase("Wait2Command"){
// Use Requires() here to declare subsystem dependencies
this->duration = duration;
SetInterruptible(false);
}
void DriveJoystickCommand::Initialize() {
printf("DriveJoystickCommand: initialize\n");
// DriveSubsystem should have set everything correctly
SetInterruptible(true);
}
