public static final double OUTPUT_SPEED = -0.7;
}
+ public static class Auton {
+ // Defense crossing speeds from -1 to 1
+ public static final double DEFAULT_SPEED = 0.6;
+ public static final double MOAT_SPEED = 0.6;
+ public static final double ROCK_WALL_SPEED = 0.8;
+ public static final double ROUGH_TERRAIN_SPEED = 0.6;
+ public static final double RAMPART_SPEED = 0.6;
+ public static final double LOW_BAR_SPEED = 0.6;
+
+ // Defense crossing times in seconds
+ public static final double DEFAULT_TIME = 5.0;
+ public static final double MOAT_TIME = 5.0;
+ public static final double ROCK_WALL_TIME = 5.0;
+ public static final double ROUGH_TERRAIN_TIME = 5.0;
+ public static final double RAMPART_TIME = 5.0;
+ public static final double LOW_BAR_TIME = 5.0;
+
+ // Time to wait before shooting in seconds
+ public static final double WAIT_TIME = 1.0;
+ }
+
public enum Defense {
PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHIVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART;
}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AlignToScore extends CommandGroup {
+
+ public AlignToScore(int position) {
+ // TODO write aligning code using encoders and gyro
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.commands.shooter.Shoot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class ChooseStrategy extends CommandGroup {
+
+ public ChooseStrategy(int position, Defense defense) {
+
+ if (defense == Defense.PORTCULLIS)
+ addSequential(new PassPortcullis());
+
+ else if (defense == Defense.CHIVAL_DE_FRISE)
+ addSequential(new PassChivalDeFrise());
+
+ else if (defense == Defense.MOAT)
+ addSequential(new PassMoat());
+
+ else if (defense == Defense.ROCK_WALL)
+ addSequential(new PassRockWall());
+
+ else if (defense == Defense.DRAWBRIDGE)
+ addSequential(new PassDrawbridge());
+
+ else if (defense == Defense.SALLY_PORT)
+ addSequential(new PassSallyPort());
+
+ else if (defense == Defense.RAMPART)
+ addSequential(new PassRampart());
+
+ else if (defense == Defense.ROUGH_TERRAIN)
+ addSequential(new PassRoughTerrain());
+
+ else if (defense == Defense.LOW_BAR)
+ addSequential(new PassLowBar());
+
+ addSequential(new AlignToScore(position));
+ // TODO: test for how long robot should wait
+ addSequential(new WaitCommand(Auton.WAIT_TIME));
+ addSequential(new Shoot());
+
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassChivalDeFrise extends CommandGroup {
+
+ public PassChivalDeFrise() {
+ // 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.
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassDrawbridge extends CommandGroup {
+
+ public PassDrawbridge() {
+ // 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.
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassLowBar extends CommandGroup {
+
+ public PassLowBar() {
+ addSequential(new TimeDrive(Auton.LOW_BAR_SPEED, Auton.LOW_BAR_TIME));
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassMoat extends CommandGroup {
+
+ public PassMoat() {
+ addSequential(new TimeDrive(Auton.MOAT_SPEED, Auton.MOAT_TIME));
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassPortcullis extends CommandGroup {
+
+ public PassPortcullis() {
+ // 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.
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassRampart extends CommandGroup {
+
+ public PassRampart() {
+ addSequential(new TimeDrive(Auton.RAMPART_SPEED, Auton.RAMPART_TIME));
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassRockWall extends CommandGroup {
+
+ public PassRockWall() {
+ addSequential(new TimeDrive(Auton.ROCK_WALL_SPEED, Auton.ROCK_WALL_TIME));
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.commands.driving.TimeDrive;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassRoughTerrain extends CommandGroup {
+
+ public PassRoughTerrain() {
+ addSequential(new TimeDrive(Auton.ROUGH_TERRAIN_SPEED,
+ Auton.ROUGH_TERRAIN_TIME));
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.auton;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class PassSallyPort extends CommandGroup {
+
+ public PassSallyPort() {
+ // 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.
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands.driving;
+
+import org.usfirst.frc.team3501.robot.Constants.Auton;
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+
+public class TimeDrive extends Command {
+ Timer timer;
+ double currentTime, targetTime, speed;
+
+ public TimeDrive() {
+ this(Auton.DEFAULT_TIME, Auton.DEFAULT_SPEED);
+ }
+
+ public TimeDrive(double time) {
+ this(time, Auton.DEFAULT_SPEED);
+ }
+
+ public TimeDrive(double time, double speed) {
+ requires(Robot.driveTrain);
+
+ timer = new Timer();
+ this.currentTime = 0;
+ this.targetTime = time;
+ this.speed = speed;
+ }
+
+ @Override
+ protected void initialize() {
+ timer.start();
+ }
+
+ @Override
+ protected void execute() {
+ currentTime = timer.get();
+
+ double output = speed * ((targetTime - currentTime) / (targetTime));
+
+ Robot.driveTrain.setMotorSpeeds(output, output);
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return currentTime >= targetTime;
+ }
+
+ @Override
+ protected void end() {
+ Robot.driveTrain.stop();
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}
projects / 3501 / stronghold-2016 / commitdiff