AutonomousCommand::AutonomousCommand(Drive *drive,
MotorSubsystem *arm_wheels,
MotorSubsystem *front_arm)
: CommandGroup("AutonomousCommand") {
log_debug("AutonomousCommand()");
AddSequential(new AutonomousDrive(drive, 4.0, 0.50));
AddParallel(
new MotorMove("WheelSpinAutonomous", arm_wheels, Direction::DOWN),
3.0); //timeout
m_arm_n_wheels = new CommandGroup("Arm And Wheels");
m_arm_n_wheels->AddSequential(
new MotorMove("FrontArmUp", front_arm, Direction::UP),
0.1); //timeout
m_arm_n_wheels->AddSequential(new WaitCommand(0.75));
m_arm_n_wheels->AddSequential(
new MotorMove("FrontArmDown", front_arm, Direction::DOWN),
0.2); //timeout
AddParallel(m_arm_n_wheels);
}
AutoPos1Level2Ship::AutoPos1Level2Ship() {
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
AddParallel(new EBBump(false));
AddParallel(new ELRun(0));
AddParallel(new WRRun(0));
AddSequential(new AutoDriveDist(0));
AddSequential(new AutoDriveTurn(0));
AddParallel(new INDelivery(false));
AddSequential(new AutoStop());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
}
ContainersOnStepAuto::ContainersOnStepAuto() {
/*
AddParallel(new ResetToteNew());
AddParallel(new ResetClaw());
//AddParallel(new ExtendWings());
AddSequential(new JoystickAutoDrive(-.7,0,0,5000,0,0), 2);
//AddSequential(new JoystickAutoDrive(0,0,.7,0,0,-90), 1.125); //Important
AddSequential(new JoystickAutoDrive(0,0,0,0,0,0),2.5);
//AddSequential(new RetractWings());
*/
//Official
AddParallel(new ResetToteNew());
AddParallel(new ResetClaw());
AddSequential(new JoystickAutoDrive(0,0,.7,0,0,-90), 1.125);
AddParallel(new JoystickAutoDrive(0,0,0,0,0,0), 1);
AddSequential(new ExtendWings());
AddSequential(new JoystickAutoDrive(-.7,0,0,5000,0,0), 73.7 * INCH_TIME);
AddSequential(new JoystickAutoDrive(0,0,0,0,0,0), .5);
AddSequential(new JoystickAutoDrive(.7,0,0,5000,0,0), 12 * INCH_TIME);
AddSequential(new JoystickAutoDrive(0,0,0,0,0,0), 1.5);
AddSequential(new JoystickAutoDrive(.7,0,0,5000,0,0), 107 * INCH_TIME);
AddParallel(new JoystickAutoDrive(0,0,0,0,0,0), 1);
AddSequential(new JoystickAutoDrive(-.7,0,0,5000,0,0), 24 * INCH_TIME);
AddParallel(new JoystickAutoDrive(0,0,0,0,0,0), 1);
AddSequential(new RetractWings());
}
AutonomousRPMShootfromRight::AutonomousRPMShootfromRight()
{
//Ramp up shooter speed
AddParallel(new SetShooterSpeed(2500, true));
//Set shooter angle
AddParallel(new SetShooterAngle(SHOOTER_ANGLE));
//Drive forward
AddSequential(new ChassisDriveDistance(AUTONOMOUS_DRIVE_FORWARD_DISTANCE, DRIVING_POWER));
//Turn left
AddSequential(new ChassisTurnAngle(AUTONOMOUS_TURN_ANGLE));
//Shoot first Frisbee
AddSequential(new FrisbeeShoot());
//wait
AddSequential(new WaitCommand(AUTONOMOUS_SHOOT_WAIT1));
//Shoot second Frisbee
AddSequential(new FrisbeeShoot());
//wait
AddSequential(new WaitCommand(AUTONOMOUS_SHOOT_WAIT2));
//Shoot third Frisbee
AddSequential(new FrisbeeShoot());
}
IntakePositionSequence::IntakePositionSequence(TurretState state, ReleaseState ball) {
AddSequential(new SetArmPosition(c_armIntakePosition));
AddParallel(new AutoIntake());
AddParallel(new SetTurretCylinder(state));
AddSequential(new BallRelease(ball));
//trigger stuff for low bar. sensors are not on robot. cannot yet happen.
}
LiftContainerAuto::LiftContainerAuto() {
AddParallel(new CloseClaw());
AddParallel(new ResetToteNew());
AddSequential(new ResetClaw());
AddSequential(new CloseClaw());
AddSequential(new ContainerPositionChange(5));
}
LowShot::LowShot() {
AddParallel(new PositionDrive(40));
AddSequential(new SetWinchPosition(1, true, 2));
AddParallel(new LowGoalShoot(2));
AddParallel(new Feed(2));
AddSequential(new SetWheelsTwist(2));
AddParallel(new SetWinchPosition(2, false, 4));
AddSequential(new StopShoot());
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
AutonomousPlayLeft::AutonomousPlayLeft() {
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
AddParallel(new CameraUp(true));
AddSequential(new ShootNow(true, 1.5,false));
AddSequential(new DriveDistance(false,-16,125,false));
//AddSequential(new WaitTime(1.0));
AddSequential(new DriveDistance(true,85,125,false));
//AddSequential(new WaitTime(1.0));
AddParallel(new ShootNow(true, 10.0,true));
AddSequential(new DriveDistance(false,18,125,false));
AddSequential(new CameraUp(false));
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
void AutonomousCommand::Moat() {
AddParallel(new Intake_Pos(-125865));//set intake to horizontal position while intaking pre-load ball
AddSequential(new Roller_In(0.75, 0.5));//Intake preload ball
AddParallel(new Roller_Stop());
AddSequential(new AutonomousDrive(-1,0,2.5));//Drive Forward, full speed over defense
AddSequential(new AutonomousDrive(0,0,0.5));//stop after 2.5 seconds
AddSequential(new Intake_Pos(-24661));
}
ShootHangingonTower::ShootHangingonTower()
{
// disable the shooter
AddParallel(new SetShooterSpeed(HANGING_ON_TOWER_VELOCITY, HANGING_ON_TOWER_IS_RPM), 3.0);
// set the shooter angle
AddParallel(new SetShooterAngle(HANGING_ON_TOWER_ANGLE), 3.0);
}
SpyBoxAuto::SpyBoxAuto() : CommandGroup("SpyBoxAuto")
{
AddSequential(new ArmGoTo(angle));
AddParallel(new ShooterSet(1));
AddSequential(new WaitForArm());
AddSequential(new ShooterPunchSet(DoubleSolenoid::kForward));
AddSequential(new WaitCommand(1));
AddParallel(new ShooterPunchSet(DoubleSolenoid::kReverse));
AddParallel(new ShooterSet(0));
}
Load::Load()
{
AddSequential(new CSetLoad()); // Monte de peu le shooter
AddParallel(new CKeep()); //Le maintien toujours
AddParallel(new BSetHaut(true)); // monte avec TRUE pour spin lent
AddSequential(new CLoad()); //Attends que la switch soit vraie
AddParallel(new BSetBas()); //Redescends, stop spin
AddSequential(new CSetBas()); // Redescends le shooter
}
SimpleAuton::SimpleAuton() {
AddSequential(new ArmLower());
AddParallel(new RollerSpin(false, true));
// AddSequential(new JoystickAutoDrive(1, -1), TIME_TO_DRIVE);
AddSequential(new DriveToDistance(DIST_TO_WALL, true));
AddParallel(new JoystickAutoDrive(0, 0));
AddParallel(new BallToShooter(), .7);
AddSequential(new WaitCommand(1));
AddSequential(new Fire(1, true));
AddParallel(new RollerStop());
AddParallel(new ArmRaise());
}
MoveRocketLevel2::MoveRocketLevel2() {
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
AddParallel(new ELRun(3));
AddSequential(new WRRun(3));
AddSequential(new EBRun(3));
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
std::printf("2135: Moved to Rocket Level 2 Position\n");
}
AutoPosition5ShootGrp::AutoPosition5ShootGrp()
{
AddParallel(new PrepareShotCmd());
AddSequential(new DriveStraightCmd(POSITION_FIVE_APPROACH_DISTANCE, CROSS_BASIC_DEFENSE_SPEED));
AddSequential(new DriveTurnCmd(POSITION_FIVE_TURN_TO_SHOOT));
AddSequential(new WaitCommand(AIM_TIME_AUTO));
AddSequential(new BallToShooterGrp());
AddSequential(new DriveTurnCmd(POSITION_FIVE_TURN_TO_DEFENCE));
AddSequential(new DriveStraightCmd(-POSITION_FIVE_APPROACH_DISTANCE, CROSS_BASIC_DEFENSE_SPEED));
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
}
CollectMode::CollectMode() : CommandGroup("CollectMode")
{
AddParallel(new StopShooter());
AddSequential(new LowerInjectorAndOpenFingers());
AddSequential(new StartCollector());
AddSequential(new LowerBridge());
}
AutonomousCommand3::AutonomousCommand3() {
//Working 1 bin autonomous; don't delete
//This commented out code makes the robot move a recycling bin to the auto zone, uncomment if useful.
/*
AddParallel(new DriveForward250());
AddSequential(new RollersIn());
AddParallel(new DriveForward250());
AddSequential(new ElevatorUp());
AddSequential(new Turn90CounterClockwise());
AddSequential(new DriveForward750());
*/
//This is REALLY tentative.
//TODO Lot of testing
/*
AddParallel(new RollersIn());
AddSequential(new DriveForward250());
AddSequential(new Turn90CounterClockwise());
AddSequential(new DriveForward250());
*/
// This code has the robot start facing the driver's station and picks up a can and drives backward into the auto zone.
AddParallel(new DriveForward250());
AddSequential(new RollersIn());
AddSequential(new DriveForward100());
AddSequential(new ElevatorUp());
AddSequential(new DriveBackward1000());
//AddSequential(new DriveForward250());
}
ShooterCommand::LoadAndFire::LoadAndFire(
FrisbeeLoader::Base *loader,
FrisbeeShooter::Base *shooter) :
CommandGroup("LoadAndFireCommand") {
AddParallel(new ShooterCommand::FireFrisbee(shooter), 5.0);
AddSequential(new ShooterCommand::LoadFrisbee(loader));
}
Rockwall::Rockwall()
{
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
AddParallel(new HoldBall(), 14);
AddSequential(new MoveArm(-50.), 1);
AddSequential(new TankDrive(0), 3);
AddSequential(new TankDrive(.5), 2.2);
AddSequential(new MoveArm(-90.), 2);
AddSequential(new MoveArm(-90.), 2);
AddSequential(new TankDrive(1.), .5);
AddSequential(new DriveandMoveArm(.75, .5, -55.));
AddSequential(new TankDrive(-.25), 1);
}
AutonReleaseBallsDropBridgeTakeAndReleaseBalls::AutonReleaseBallsDropBridgeTakeAndReleaseBalls() {
// Add Commands here:
// e.g. AddSequential(new Command1());
// AddSequential(new Command2());
// these will run in order.
// To run multiple commands at the same time,
// use AddParallel()
// e.g. AddParallel(new Command1());
// AddSequential(new Command2());
// Command1 and Command2 will run in parallel.
// A command group will require all of the subsystems that each member
// would require.
// e.g. if Command1 requires chassis, and Command2 requires arm,
// a CommandGroup containing them would require both the chassis and the
// arm.
AddSequential(new WaitCommand(8.0));
AddSequential(new BridgeManipulatorForward());
AddSequential(new DriveForward());
AddSequential(new BridgeManipulatorForward());
AddParallel(new BridgeManipulatorRearward());
AddSequential(new DriveBackwards());
AddSequential(new BridgeManipulatorForward());
AddSequential(new WaitCommand(1.0));
AddSequential(new BridgeManipulatorRearward());
}
AutonomousModeFastOneBall::AutonomousModeFastOneBall()
{
AddSequential(new AutonomousLowGearCommand());
AddParallel(new OperatorHighCommand());
AddSequential(new AutonomousDriveCommand(1.0f, TURN_CORRECTION, 2.2));
AddSequential(new DriveLaunchReleaseCommand());
}
FeederHomePos::FeederHomePos() {
// these command can be run in parallel
AddParallel(new SetElevationPosition(SETPT_EL_WHEN_FEEDING));
//AddParallel(new SetAzimuthPosition());
}
PyramidMode::PyramidMode() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
AddParallel(new RunShooterSlow());
//AddSequential(new ShooterOff());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
}
Auto::Auto()
{
AddParallel(new SetShooterMode<ShooterOn>());
AddSequential(new AutonomousDrive<DriveStraight>(6, 0.8, 4500, 0)); // was 5200, before that 4100
AddSequential(new AutonomousDrive<DriveTurn>(2, 0.8, 0, 33.0));
AddSequential(new AutonomousDrive<DriveStraight>(3, 0.8, 450, 0));
AddSequential(new ShootBall());
}
PorticullisAuton::PorticullisAuton(){
AddSequential(new ShiftLow());
AddSequential(new AutoDriveForward(-.25, 42));
AddSequential(new ArmDriveToPos(Arm::Position::POS_DOWN), 1.8);
AddSequential(new AutoDriveForward(-.25, 10));
AddParallel(new ArmDriveToPos(Arm::Position::POS_PORT), 1.8);
AddSequential(new AutoDriveForward(-.75, 98));
AddSequential(new ShiftHigh());
}
MoveArmsFancy::MoveArmsFancy(State state) {
switch (state) {
case State::up:
AddParallel(new ActuateCanStabilizer(ActuateCanStabilizer::open));
AddSequential(new MoveArms(CAN_POT_UP_POSITION));
// AddSequential(new Induct(Induct::forward, CAN_INDUCT_UP_TIMEOUT)); Probably not be needed with fancy craaaw
break;
case State::down:
AddSequential(new MoveWrist(MoveWrist::open));
AddSequential(new MoveArms(CAN_POT_DOWN_POSITION));
break;
case State::toggle:
AddParallel(new ActuateCanStabilizer(ActuateCanStabilizer::open));
AddSequential(new MoveWrist(MoveWrist::close));
AddSequential(new ToggleArms());
break;
}
}
WarlockAuton::WarlockAuton() {
// // Ball 1
// AddSequential(new ReverseBelt(true), 2.0);
//
// Ball 2
AddSequential(new WaitCommand(2.0));
AddParallel(new LoaderLoad());
AddSequential(new ReverseBelt(true));
}
ChevalAuton::ChevalAuton(){
AddSequential(new ShiftLow());
AddSequential(new AutoDriveForward(-.5,48));
AddSequential(new ArmDriveToPos(Arm::Position::POS_DOWN), 2);
//AddSequential(new AutoDriveForward(.25, 2));
AddSequential(new AutoDriveForward(-.5, 6));
AddParallel(new ArmDriveToPos(Arm::Position::POS_START));
AddSequential(new AutoDriveForward(-1, 64));
AddSequential(new ShiftHigh());
}
SimpleDriveAuton::SimpleDriveAuton(){
AddSequential(new ResetDisplacement());
AddParallel(new ArmDriveToPos(Arm::Position::POS_SALLY));
AddSequential(new ShiftLow());
AddSequential(new AutoDriveForward(1, 24));
AddSequential(new AutoDriveForward(.75, 108));
AddSequential(new AutoDriveTurn(0));
AddSequential(new AutoDriveForward(1, 18));
AddSequential(new ShiftHigh());
}
Autonomous::Autonomous() : CommandGroup("Autonomous") {
AddSequential(new PrepareToPickup());
AddSequential(new Pickup());
AddSequential(new SetDistanceToBox(0.10));
// AddSequential(new DriveStraight(4)); // Use Encoders if ultrasonic is broken
AddSequential(new Place());
AddSequential(new SetDistanceToBox(0.60));
// addSequential(new DriveStraight(-2)); // Use Encoders if ultrasonic is broken
AddParallel(new SetWristSetpoint(-45));
AddSequential(new CloseClaw());
}
