From: Harel Dor <hareldor@gmail.com>
Date: Sat, 12 Mar 2016 00:54:21 +0000 (-0800)
Subject: Add rudimentary auton strategies based on timing
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=f53566b348b3492a834d8fa6ecbb76c011e2a9ff

Add rudimentary auton strategies based on timing
---

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 2c7971dc..e5c4c4e7 100644
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -110,6 +110,27 @@ public class Constants {
     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;
   }
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java
new file mode 100644
index 00000000..12fb4b7f
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java
@@ -0,0 +1,13 @@
+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
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/ChooseStrategy.java b/src/org/usfirst/frc/team3501/robot/commands/auton/ChooseStrategy.java
new file mode 100644
index 00000000..50337c73
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/ChooseStrategy.java
@@ -0,0 +1,47 @@
+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());
+
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassChivalDeFrise.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassChivalDeFrise.java
new file mode 100644
index 00000000..0a77adda
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassChivalDeFrise.java
@@ -0,0 +1,28 @@
+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.
+    }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawbridge.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawbridge.java
new file mode 100644
index 00000000..a95be168
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassDrawbridge.java
@@ -0,0 +1,28 @@
+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.
+    }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java
new file mode 100644
index 00000000..272085af
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java
@@ -0,0 +1,16 @@
+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));
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java
new file mode 100644
index 00000000..8a43fbd0
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java
@@ -0,0 +1,16 @@
+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));
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java
new file mode 100644
index 00000000..5e6e393e
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassPortcullis.java
@@ -0,0 +1,28 @@
+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.
+    }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java
new file mode 100644
index 00000000..315f23d4
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java
@@ -0,0 +1,16 @@
+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));
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java
new file mode 100644
index 00000000..eca46c35
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java
@@ -0,0 +1,16 @@
+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));
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java
new file mode 100644
index 00000000..e72fce62
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java
@@ -0,0 +1,17 @@
+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));
+  }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java
new file mode 100644
index 00000000..2d956dcc
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassSallyPort.java
@@ -0,0 +1,28 @@
+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.
+    }
+}
diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java
new file mode 100644
index 00000000..e9a8f9f3
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TimeDrive.java
@@ -0,0 +1,58 @@
+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();
+  }
+}