Skip to content

Commit afdeefe

Browse files
coach-streetercoach-streeter
committed
adding new autonomous routines parts.
Adding more commands.
1 parent 76c801f commit afdeefe

20 files changed

+836
-0
lines changed

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.RobotContainer;
12+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
13+
import edu.wpi.first.wpilibj2.command.*;
14+
15+
public class StartBWDriveOnlyToRP extends SequentialCommandGroup {
16+
/**
17+
* Add your docs here.
18+
*/
19+
public StartBWDriveOnlyToRP() {
20+
21+
// start backwards
22+
addCommands(new DriveZeroGyro(180.0));
23+
24+
// lower the intake
25+
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
27+
// then, drive to the rendezvous point
28+
addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180));
29+
}
30+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.RobotContainer;
12+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
13+
import edu.wpi.first.wpilibj2.command.*;
14+
15+
public class StartBWDriveOnlyToWall extends SequentialCommandGroup {
16+
/**
17+
* Add your docs here.
18+
*/
19+
public StartBWDriveOnlyToWall() {
20+
21+
// start backwards
22+
addCommands(new DriveZeroGyro(180.0));
23+
24+
// lower the intake
25+
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
27+
// then, drive towards the wall
28+
addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 40, 180));
29+
}
30+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.RobotContainer;
11+
import org.mayheminc.robot2020.commands.*;
12+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
13+
import org.mayheminc.robot2020.subsystems.Hood;
14+
import org.mayheminc.robot2020.subsystems.ShooterWheel;
15+
import org.mayheminc.robot2020.subsystems.Turret;
16+
17+
import edu.wpi.first.wpilibj2.command.*;
18+
19+
public class StartBWOppTrench extends SequentialCommandGroup {
20+
/**
21+
* Add your docs here.
22+
*/
23+
public StartBWOppTrench() {
24+
25+
addCommands(new DriveZeroGyro(180.0));
26+
27+
// lower the intake and turn it on before driving forwards
28+
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
29+
addCommands(new IntakeSetRollers(-1.0));
30+
31+
// make sure the hood is down
32+
addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
33+
34+
// drive to get balls from opponent's trench
35+
addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 108, 180));
36+
37+
// now driven to get the balls from opposing trench
38+
addCommands(new Wait(0.8), // wait for last two balls to get into robot
39+
new IntakeSetRollers(0), // turn off the intake
40+
new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 36, 180)); // start backing up
41+
// slowly
42+
43+
// backup about halfway across the field
44+
addCommands(new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 180, 270));
45+
46+
// in shooting position, prepare everything for shooting
47+
addCommands(new ParallelCommandGroup( // run the following commands in parallel:
48+
new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
49+
new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION + 3000.0),
50+
new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
51+
52+
// turn on the intake gently while shooting to help balls settle
53+
addCommands(new IntakeSetRollers(-0.2));
54+
addCommands(new ShooterReadyAimFire(5.0));
55+
56+
// turn the shooter wheel and intake off now that the shooting is all done
57+
addCommands(new ShooterWheelSet(0.0));
58+
addCommands(new IntakeSetRollers(0.0));
59+
60+
// turn the wheel off now that the shooting is all done
61+
addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
62+
}
63+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.RobotContainer;
11+
import org.mayheminc.robot2020.commands.*;
12+
import org.mayheminc.robot2020.subsystems.Hood;
13+
import org.mayheminc.robot2020.subsystems.ShooterWheel;
14+
import org.mayheminc.robot2020.subsystems.Turret;
15+
16+
import edu.wpi.first.wpilibj2.command.*;
17+
18+
public class StartBWShoot3 extends SequentialCommandGroup {
19+
/**
20+
* Add your docs here.
21+
*/
22+
public StartBWShoot3() {
23+
24+
addCommands(new DriveZeroGyro(180.0));
25+
26+
// first, lower the intake, start the shooter wheel, and wait until the turret
27+
// is turned towards the target
28+
addCommands(new ParallelCommandGroup( // run the following commands in parallel:
29+
new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
30+
new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
31+
new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION),
32+
new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
33+
34+
addCommands(new ShooterReadyAimFire(1.5));
35+
36+
// turn the shooter wheel and intake off now that the shooting is all done
37+
addCommands(new ParallelCommandGroup( // below commands in parallel
38+
new ShooterWheelSet(0.0), //
39+
new IntakeSetRollers(0.0), // turn off the rollers
40+
new HoodSetAbs(Hood.HOOD_STARTING_POSITION)));
41+
}
42+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
import edu.wpi.first.wpilibj2.command.*;
13+
14+
public class StartBWShoot3ThenToRP extends SequentialCommandGroup {
15+
/**
16+
* Add your docs here.
17+
*/
18+
public StartBWShoot3ThenToRP() {
19+
20+
// start backwards and shoot the first three balls
21+
addCommands(new StartBWShoot3());
22+
23+
// then, drive to the rendezvous point
24+
addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180));
25+
}
26+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
13+
import edu.wpi.first.wpilibj2.command.*;
14+
15+
public class StartBWShoot3ThenToWall extends SequentialCommandGroup {
16+
/**
17+
* Add your docs here.
18+
*/
19+
public StartBWShoot3ThenToWall() {
20+
21+
// start backwards and shoot the first three balls
22+
addCommands(new StartBWShoot3());
23+
24+
// then, drive to the rendezvous point
25+
addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, -40, 180));
26+
}
27+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
12+
import org.mayheminc.robot2020.subsystems.Hood;
13+
import org.mayheminc.robot2020.subsystems.ShooterWheel;
14+
import org.mayheminc.robot2020.subsystems.Turret;
15+
16+
import edu.wpi.first.wpilibj2.command.*;
17+
18+
public class StartBWTrench extends SequentialCommandGroup {
19+
/**
20+
* Add your docs here.
21+
*/
22+
public StartBWTrench() {
23+
24+
// start backwards and shoot the first three balls
25+
addCommands(new StartBWShoot3());
26+
27+
// then, drive to the trench entrance (jog left a little to get there)
28+
addCommands(new DriveStraightOnHeading(0.5, DistanceUnits.INCHES, 40, 150));
29+
30+
// pick up balls while heading down the trench.
31+
addCommands(new ParallelCommandGroup(
32+
// intake while driving down the trench
33+
new IntakeSetRollers(-1.0),
34+
// ensure the hood is down
35+
new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION),
36+
// drive the path under the control panel to the end
37+
new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180)));
38+
39+
// now driven to the balls at far end of trench
40+
addCommands(new Wait(0.8), // wait for last two balls to get into robot
41+
new IntakeSetRollers(0), // turn off the intake
42+
new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 12, 180)); // start backing up
43+
// slowly
44+
45+
// after getting all three balls, go back to shooting position
46+
// first, make sure we drive straight out from under the control panel
47+
addCommands(new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 48, 180));
48+
49+
// drive diagonally over towards the shooting position, while turning on shooter
50+
// wheels, raising the hood, and re-aiming the turret
51+
addCommands(new ParallelCommandGroup(
52+
new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
53+
new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION),
54+
new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)),
55+
new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160)));
56+
57+
// straighten out again to enable turret to aim to the target
58+
addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 24, 180));
59+
60+
// turn on the intake gently while shooting to help balls settle
61+
addCommands(new IntakeSetRollers(-0.2));
62+
63+
// start the shooting sequence
64+
addCommands(new ShooterReadyAimFire(6.0));
65+
66+
// turn the shooter wheel and intake off now that the shooting is all done
67+
addCommands(new ShooterWheelSet(0.0));
68+
addCommands(new IntakeSetRollers(0.0));
69+
70+
// turn the wheel off now that the shooting is all done
71+
addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
72+
}
73+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.RobotContainer;
12+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
13+
import edu.wpi.first.wpilibj2.command.*;
14+
15+
public class StartFWDriveOnlyToRP extends SequentialCommandGroup {
16+
/**
17+
* Add your docs here.
18+
*/
19+
public StartFWDriveOnlyToRP() {
20+
21+
// start forwards
22+
addCommands(new DriveZeroGyro(0.0));
23+
24+
// lower the intake
25+
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
27+
// then, drive to the RP
28+
addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0));
29+
}
30+
}

‎src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package org.mayheminc.robot2020.autonomousroutines;
9+
10+
import org.mayheminc.robot2020.commands.*;
11+
import org.mayheminc.robot2020.RobotContainer;
12+
import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
13+
import edu.wpi.first.wpilibj2.command.*;
14+
15+
public class StartFWDriveOnlyToWall extends SequentialCommandGroup {
16+
/**
17+
* Add your docs here.
18+
*/
19+
public StartFWDriveOnlyToWall() {
20+
21+
// start forwards
22+
addCommands(new DriveZeroGyro(0.0));
23+
24+
// lower the intake
25+
addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
26+
27+
// then, drive to the RP
28+
addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0));
29+
}
30+
}

0 commit comments

Comments
 (0)