AutonomousModeFastOneBall::AutonomousModeFastOneBall()
{
AddSequential(new AutonomousLowGearCommand());
AddParallel(new OperatorHighCommand());
AddSequential(new AutonomousDriveCommand(1.0f, TURN_CORRECTION, 2.2));
AddSequential(new DriveLaunchReleaseCommand());
}
AutonCheval::AutonCheval() : CommandGroup("AutonCheval")
{
// 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 DriveForwardReach());
AddSequential(new ArmLowerLowGoal());
AddSequential(new ArmLowerLowGoal());
AddSequential(new DriveWait());
AddSequential(new DriveForwardReach());
//AddSequential(new ArmRaise());
}
LowNoShoot::LowNoShoot()
{
// 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 ResetGyro());
AddSequential(new FoldIntakeOut(), 1);
AddSequential(new TurnForAngle(.3, 0), .5);
AddSequential(new DriveForTime(.45,0),3.75);
// AddSequential(new AutoRollerIn(), 1);
//AddSequential(new IntakeCommand(), .1);
AddSequential(new DriveUntilClose(.4,94));
}
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");
}
AlignVerticalAndHorizontal::AlignVerticalAndHorizontal(float angle)
{
AddSequential(new GoalAlignVertical(101));
AddSequential(new GoalAlign(angle));
//AddSequential(new DriveCommandAuto(0,0,0,.5,0));
}
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.
}
DriveUnderPort::DriveUnderPort()
{
AddSequential(new DriveSetDistance(100));
AddSequential(new lowerEverything());
AddSequential(new DriveSetDistance(25));
AddSequential(new LiftLoader());
AddSequential(new DriveSetDistance(100));
// 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.
}
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
}
CollectMode::CollectMode() : CommandGroup("CollectMode")
{
AddParallel(new StopShooter());
AddSequential(new LowerInjectorAndOpenFingers());
AddSequential(new StartCollector());
AddSequential(new LowerBridge());
}
AutoPosANYMove::AutoPosANYMove() {
// 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.
frc2135::RobotConfig* config = frc2135::RobotConfig::GetInstance();
double cmdDistance;
config->GetValueAsDouble("AutoPosANYMove", cmdDistance, 97.25);
std::printf("2135: Auto Pos ANY Move - Init %5.3f inches\n", cmdDistance);
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
AddSequential(new AutoDriveDist(cmdDistance));
AddSequential(new AutoStop());
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
}
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.
}
AutonomousDriveLowBarShootLow::AutonomousDriveLowBarShootLow() {
// 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
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
AddSequential (new ShooterInitialize);
AddSequential (new AutonomousMotionProfile(&AutonomousDrivePastLowBar_60ips));
AddSequential (new AutonomousMotionProfile(&Rotate60Clockwise,&Rotate60Clockwise));
AddSequential (new AutonomousFireBoulderLow());
}
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());
}
Launch::Launch(double 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.
// AddParallel(new FoldIntakeIn(), 1.5);
AddSequential(new SpinUp(speed), 1.5);
// AddSequential(new PrepLaunch(speed), 1.5);
//AddSequential(new ReverseIntakeCommand(),1);
AddSequential(new ServoExtend(), 0.5);
// AddSequential(new WaitCommand(1));
AddSequential(new StopSpeed());
AddSequential(new ServoRetract(), 0.5);
}
MoveToDefense::MoveToDefense()
{
log = Log::getInstance();
log->write(Log::TRACE_LEVEL, "Moving to Defense");
AddSequential(new AngleIntake(90.0));
AddSequential(new DriveStraight(1.0, DriveStraight::SensorType::GYRO, 3.1)); //temporary time
}
MoveAround::MoveAround()
{
AddSequential(new ChangeHeading(360));
AddSequential(new ChangeDistance(36));
AddSequential(new ChangeHeading(180));
AddSequential(new ChangeDistance(36));
AddSequential(new ChangeHeading(180));
}
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));
}
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());
}
CenterGoalAuto::CenterGoalAuto()
{
AddSequential(new AlignParallel(0, 110));
AddSequential(new AlignCenter());
AddSequential(new DriveCommandAuto(0, 0, 0, 1, 0));
AddSequential(new ShooterWheels(6000, 3, 0, 200));
AddSequential(new ShooterSolenoid());
}
WarlockAuton::WarlockAuton() {
// // Ball 1
// AddSequential(new ReverseBelt(true), 2.0);
//
// Ball 2
AddSequential(new WaitCommand(2.0));
AddParallel(new LoaderLoad());
AddSequential(new ReverseBelt(true));
}
AutoPos1Switch::AutoPos1Switch() {
// 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.
frc2135::RobotConfig* config = frc2135::RobotConfig::GetInstance();
double cmdDistLeg1 = 0.0;
double cmdDistTurn1 = 0.0;
double cmdDistLeg2 = 0.0;
config->GetValueAsDouble("AutoPos1SwitchLeg1", cmdDistLeg1, 145.25);
config->GetValueAsDouble("AutoPos1SwitchTurn1", cmdDistTurn1, 90.0);
config->GetValueAsDouble("AutoPos1SwitchLeg2", cmdDistLeg2, 18.0);
std::printf("2135: Auto Pos 1 Switch - Init Leg1 %4.2f in, Turn1 %4.1f deg, Leg2 %4.2f in\n",
cmdDistLeg1, cmdDistTurn1, cmdDistLeg2);
AddParallel(new GripperRun(Robot::gripper->GRIPPER_HOLD)); // Gripper should hold cube (instantaneous)
AddParallel(new ElevatorRun(Robot::elevator->SWITCH_HEIGHT)); // Elevator to switch height - takes about 1 sec
AddParallel(new WristRun(Robot::wrist->WRIST_DELIVER)); // Wrist ready to deliver angle < 1 sec
AddSequential(new AutoDriveDist(cmdDistLeg1)); // Drive to the turning point - about 1 sec
// Gripper still holding
// Elevator still at switch height
// Wrist still at deliver angle
AddSequential(new AutoDriveTurn(cmdDistTurn1)); // Drive turn to aim at switch - < 1 sec
// Gripper still holding
// Elevator still at switch height
AddParallel(new WristRun(Robot::wrist->WRIST_FLAT)); // Wrist to flat level < 1 sec
AddSequential(new AutoDriveDist(cmdDistLeg2)); // Drive to switch < 1 sec
AddParallel(new GripperRun(Robot::gripper->GRIPPER_REVERSE)); // Expel cube (instantaneous)
// Elevator still at switch height
// Wrist still flat level
AddSequential(new AutoStop(), 1.0); // Tell drive motors to explicitly stop for 1 second
AddParallel(new GripperRun(Robot::gripper->GRIPPER_STOP)); // Stop gripper
AddSequential(new AutoStop()); // Tell drive motors to explicitly stop
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=COMMAND_DECLARATIONS
}
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));
}
LoadAndFireCommand::LoadAndFireCommand(
BaseFrisbeeLoader *loader,
BaseFrisbeeAimer *aimer,
BaseFrisbeeTurret *turret,
BaseFrisbeeShooter *shooter) :
CommandGroup("LoadAndFireCommand") {
AddSequential(new LoadFrisbeeCommand(loader));
AddSequential(new AimTurretCommand(aimer, turret, Tracking::ClosestOffset, 5));
AddSequential(new FireFrisbeeCommand(shooter));
}
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());
}
Autonomous::Autonomous(int selection) : CommandGroup("Autonomous") {
// [AJN] I believe this is necessary to let the WPI code know that
// each Subsystem you are going to use is needed by the command so
// that it can ensure an instance is constructed.
// Requires(Robot::drive.get());
AddSequential(new ShootMoveFetcher());
AddSequential(new MoveFetcher(false, 1, false));
switch(selection) {
case 1: // Drive forward
AddSequential(new Move(4, .4));
break;
case 2: // Drive and shoot (lowbar)
AddSequential(new Move(3.75, .4));
AddSequential(new Rotate(48));
AddSequential(new Move(1.5, .4));
AddSequential(new Move(0, 0));
AddSequential(new ShootMoveFetcher());
break;
case 3: // Drive forward short
AddSequential(new Move(1.4, .4));
break;
default:
break;
}
}
void AutonomousSequence::DoSequence(){
switch(startPosition){
case leftBack: puts("got left"); break;
case rightBack: puts("got right"); break;
case middleBack:
puts("got middle");
//In Autonomous Middle back we moved forward 0units, we should update this.
AddSequential(new AutonomousForward(0.0));
puts("Added AutonomousForward(0.0)");
break;
default: puts("hell broke loose"); break; //to my understanding, left and right don't do anything prior to this sequence...
}
//magic number for autonomous angle is 21 degrees...
//AddSequential(new SetRawShooterStuffDontUse(shooterSpeed, elevationAngle)); //don't need
//AddSequential(new MoveShooterToSetElevationAngle(true)); //check for encoder failure
Robot::elevator->SetRawAngle(Robot::elevator->GetGoodShootingAngle());
AddSequential(new ShooterToggleOnOff()); // turns shooter on at default speed
AddSequential(new PrintCommand("turning shooter on\n"));
AddSequential(new MoveShooterToSetElevationAngle(true));
AddSequential(new PrintCommand("moved shooter to set angle"));
AddSequential(new WaitUntilCommand(GetAutonomousWaitTime()));
AddSequential(new PrintCommand("Waited 7 Seconds\n"));
AddSequential(new SpinFeeder()); //shoots all discs
AddSequential(new PrintCommand("Spun the feeder...\n"));
}
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());
}
GrabNGoAuto::GrabNGoAuto()
{
AddSequential(new SetOffSet(180));
//FALSE IS CLOSE, TRUE IS OPEN
printf("grabandgoauto 0\n");
//side, fow, rot, yaw, time
AddSequential(new PneumaticsAuto(true, true, false));
//printf("grabandgoauto 1\n");
AddSequential(new DriveWhileWinching(0.0, 0.5, 0.0, 0.0, 1.95, 1, 1));
//printf("grabandgoauto 2\n");
//printf("grabandgoauto 3\n");
}
