Merge branch 'fix/week1' into vision/#161-cameraoverlay

frc5687 · Mar 6, 2016 · bca3cfc · bca3cfc
2 parents 043333b + 67d9d34
commit bca3cfc
Show file tree

Hide file tree

Showing 21 changed files with 156 additions and 75 deletions.
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/Constants.java b/src/main/java/org/usfirst/frc/team5687/robot/Constants.java
@@ -20,7 +20,7 @@ public class Deadbands {
          */
         public static final double DRIVE_STICK = 0.1;
         public static final double SHOOTER_WHEELS = 0.1;
-        public static final double INTAKE_STICK = 0.2;
+        public static final double INTAKE_STICK = 0.8;
         public static final double ARMS = 0.1;
     }
 
@@ -106,7 +106,10 @@ public class Shooter {
         /**
          * Speed to run the shooter wheels to shoot high goal
          */
-        public static final double SHOOTER_SPEED = .94;
+        public static final double SHOOTER_SPEED_LOW = 0.92;
+        public static final double SHOOTER_SPEED = 0.94;
+        public static final double SHOOTER_SPEED_HIGH = 0.96;
+        public static final double SHOOTER_SPEED_EXTREME = 0.98;
     }
 
     public class Arms {
@@ -119,7 +122,7 @@ public class Arms {
 
     public class Climber {
         public static final double RAISE_SPEED = 0.6;
-        public static final double LOWER_SPEED = -0.6;
+        public static final double LOWER_SPEED = -1;
     }
 
     public class Encoders {

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/OI.java b/src/main/java/org/usfirst/frc/team5687/robot/OI.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.usfirst.frc.team5687.robot.commands.*;
 import org.usfirst.frc.team5687.robot.utils.Gamepad;
 import org.usfirst.frc.team5687.robot.utils.Helpers;
@@ -28,6 +29,11 @@ public class OI {
     public static final int UNPRIME = 6;
     public static final int FIRE = 1;
     public static final int RECOVER = 4;
+    // Prime Speed Buttons
+    public static final int LOW_PRIME_SPEED = 8; // 0.92
+    public static final int DEFAULT_PRIME_SPEED = 7; // 0.94
+    public static final int HIGH_PRIME_SPEED = 10; // 0.96
+    public static final int EXTREME_PRIME_SPEED = 9; // 0.98
     // Lights Buttons
     public static final int SWITCH_RING_LIGHT = 12;
     public static final int SWITCH_FLASHLIGHT = 11;
@@ -54,6 +60,11 @@ public OI() {
         JoystickButton visionLightSwitch = new JoystickButton(joystick, SWITCH_RING_LIGHT);
         JoystickButton flashlightSwitch = new JoystickButton(joystick, SWITCH_FLASHLIGHT);
 
+        JoystickButton lowSpeedButton = new JoystickButton(joystick, LOW_PRIME_SPEED);
+        JoystickButton normalSpeedButton = new JoystickButton(joystick, DEFAULT_PRIME_SPEED);
+        JoystickButton highSpeedButton = new JoystickButton(joystick, HIGH_PRIME_SPEED);
+        JoystickButton extremeSpeedButton = new JoystickButton(joystick, EXTREME_PRIME_SPEED);
+
         // Drive Train Commands
         reverseButton.whenPressed(new ReverseDrive());
         // Boulder Commands
@@ -68,6 +79,11 @@ public OI() {
         flashlightSwitch.whenPressed(new ToggleFlashlight());
         // Reset Camera Command
         resetCameraButton.whenPressed(new ResetCamera());
+
+        lowSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED_LOW));
+        normalSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED));
+        highSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED_HIGH));
+        extremeSpeedButton.whenPressed(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED_EXTREME));
     }
 
     /**
@@ -117,6 +133,7 @@ public double getIntakeSpeed() {
         return Helpers.applyDeadband(joystick.getRawAxis(1), Constants.Deadbands.INTAKE_STICK);
     }
 
+
     /**
      * Gets the desired speed for the arms
      * @return the control value for the arms' motor

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/Robot.java b/src/main/java/org/usfirst/frc/team5687/robot/Robot.java
@@ -145,16 +145,19 @@ public void robotInit() {
         SmartDashboard.putData("Start Position", positionChooser);
 
         autoChooser.addDefault("Do Nothing At All", new AutonomousDoNothing());
-        autoChooser.addObject("Calibrate CVT", new AutonomousTestCVT());
         autoChooser.addObject("Traverse Defense", new AutoTraverseBuilder());
         autoChooser.addObject("Traverse And Shoot", new AutoTraverseAndShootBuilder());
+        autoChooser.addDefault("---Below are for Testing---", new AutonomousDoNothing());
+        autoChooser.addObject("Target and Shoot", new AutoShootOnly());
         autoChooser.addObject("Chase Target", new AutoChaseTarget());
+        autoChooser.addObject("Calibrate CVT", new AutonomousTestCVT());
         autoChooser.addObject("Left 90", new AutoAlign(-90));
         autoChooser.addObject("Right 90", new AutoAlign(90));
         autoChooser.addObject("Drive 12", new AutoDrive(0.4, 12f));
         autoChooser.addObject("Drive 24", new AutoDrive(0.4, 24f));
         autoChooser.addObject("Drive 48", new AutoDrive(0.4, 48f));
         autoChooser.addObject("Drive 96", new AutoDrive(0.4, 96f));
+        autoChooser.addObject("Calibrate CVT", new AutonomousTestCVT());
         SmartDashboard.putData("Autonomous mode", autoChooser);
 
     }
@@ -198,7 +201,7 @@ public void autonomousPeriodic() {
         driveTrain.sendAmpDraw();
         intake.updateDashboard();
         arms.updateDashboard();
-
+        lights.updateDashboard();
     }
 
     public void teleopInit() {
@@ -219,6 +222,7 @@ public void teleopPeriodic() {
         Scheduler.getInstance().run();
         intake.updateDashboard();
         arms.updateDashboard();
+        lights.updateDashboard();
     }
 
     /**

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/RobotMap.java b/src/main/java/org/usfirst/frc/team5687/robot/RobotMap.java
@@ -22,10 +22,11 @@ public static class Drive {
         public static final int PDP_RIGHT_MOTOR_FRONT = 12;
         public static final int PDP_RIGHT_MOTOR_REAR = 13;
 
-        public static final int LEFT_ENCODER_CHANNEL_A = 8;
-        public static final int LEFT_ENCODER_CHANNEL_B = 9;
-        public static final int RIGHT_ENCODER_CHANNEL_A = 6;
-        public static final int RIGHT_ENCODER_CHANNEL_B = 7;
+        // Encoder channel ports as of 03/02, left reversed with right
+        public static final int LEFT_ENCODER_CHANNEL_A = 6;
+        public static final int LEFT_ENCODER_CHANNEL_B = 7;
+        public static final int RIGHT_ENCODER_CHANNEL_A = 8;
+        public static final int RIGHT_ENCODER_CHANNEL_B = 9;
     }
 
     /**

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoChaseTarget.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoChaseTarget.java
@@ -19,8 +19,8 @@ public class AutoChaseTarget extends Command {
     private static final double deadbandWidth = 50;
     private static final double deadbandX = 20;
 
-    private static final double baseSpeed = 0.6;
-    private static final double twist = .2;
+    private static final double baseSpeed = 0.4;
+    private static final double twist = .1;
 
     private boolean centered = false;
     private boolean inRange = false;

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoShootOnly.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoShootOnly.java
@@ -0,0 +1,17 @@
+package org.usfirst.frc.team5687.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Created by Ben Bernard on 3/5/2016.
+ */
+public class AutoShootOnly extends CommandGroup {
+
+    public AutoShootOnly() {
+
+        addParallel(new Prime());
+        addSequential(new AutoChaseTarget());
+        addSequential(new Fire());
+    }
+
+}
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverse.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverse.java
@@ -11,7 +11,7 @@ public abstract class AutoTraverse extends Command{
      *
      */
     static final long inchesToTraverseDefense = 90;
-    static final long inchesToClear = 3;
+    static final long inchesToClear = 9; //TODO! back to 3 with Rotation
     //static final long inchesToCross = 20;
     //static final double rollThreshold = 8.0;
     static final double rollErrorMargin = 1.0;

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverseAndShoot.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverseAndShoot.java
@@ -10,8 +10,8 @@ public class AutoTraverseAndShoot extends CommandGroup {
     public AutoTraverseAndShoot(String defense, int position) {
 
         addSequential(new AutoTraverseOnly(defense, position));
-        // addSequential(new AutoChaseTarget());
-        addSequential(new Prime());
+        addParallel(new Prime());
+        addSequential(new AutoChaseTarget());
         addSequential(new Fire());
 
 

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverseBuilder.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverseBuilder.java
@@ -22,7 +22,7 @@ protected void initialize() {
         DriverStation.reportError("Building autotraverse for defense " + defense + " in position " + position,false);
 
         // Traverse the selected defense
-        Scheduler.getInstance().add(new AutoTraverseOnly(defense, position));
+        Scheduler.getInstance().add(new AutoTraverseOnly(defense, 0));
 
     }
 

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverseOnly.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/AutoTraverseOnly.java
@@ -47,12 +47,11 @@ public  AutoTraverseOnly(String defense, int position) {
                 break;
         }
 
-
         switch (position){
             case 1:
 
-                // Run forward 112 inches
-                addSequential(new AutoDrive(.7, 124.0f));
+                // Run forward 100 inches
+                addSequential(new AutoDrive(.7, 100.0f));
 
                 // Turn towards the tower
                 addSequential(new AutoAlign(50));
@@ -97,9 +96,6 @@ public  AutoTraverseOnly(String defense, int position) {
                 break;
         }
 
-
-
-
     }
 
 }
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/CancelPrime.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/CancelPrime.java
@@ -10,7 +10,7 @@
 public class CancelPrime extends CommandGroup{
 
     public CancelPrime() {
-        addSequential(new SetShooterSpeed(0, Constants.Shooter.UNPRIME_TIME)); // Stops the shooter
+        addSequential(new ToggleShooter(false)); // Stops the shooter
         addSequential(new UnprimeBoulder()); // Unprimes the shooter to captured position
     }
 }
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/Fire.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/Fire.java
@@ -11,7 +11,7 @@ public class Fire extends CommandGroup{
 
     public Fire() {
         addSequential(new FireBoulder());
-        addSequential(new SetShooterSpeed(0, Constants.Shooter.UNPRIME_TIME));
+        addSequential(new ToggleShooter(false));
         addSequential(new DisableFlashlight());
     }
 }
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/FireBoulder.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/FireBoulder.java
@@ -11,13 +11,16 @@
  * @author wil
  */
 public class FireBoulder extends Command {
+    private long endTime;
+
 
     public FireBoulder() {
         requires(intake);
     }
 
     @Override
     protected void initialize() {
+        endTime = System.currentTimeMillis() + Constants.Shooter.UNPRIME_TIME;
     }
 
     @Override
@@ -27,7 +30,7 @@ protected void execute() {
 
     @Override
     protected boolean isFinished() {
-        return !intake.isPrimed();
+        return !intake.isPrimed() && System.currentTimeMillis() > endTime;
     }
 
     @Override

diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/Prime.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/Prime.java
@@ -3,6 +3,8 @@
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import org.usfirst.frc.team5687.robot.Constants;
 
+import static org.usfirst.frc.team5687.robot.Robot.oi;
+
 /**
  * Command group to prime the shooter subsystem
  * @author wil
@@ -12,6 +14,6 @@ public class Prime extends CommandGroup{
     public Prime() {
         addSequential(new EnableFlashlight());
         addSequential(new PrimeBoulder());
-        addSequential(new SetShooterSpeed(Constants.Shooter.SHOOTER_SPEED, Constants.Shooter.PRIME_TIME));
+        addSequential(new ToggleShooter(true));
     }
 }
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/RecoverBoulder.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/RecoverBoulder.java
@@ -2,18 +2,17 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 
-import static org.usfirst.frc.team5687.robot.Robot.intake;
 import static org.usfirst.frc.team5687.robot.Robot.shooter;
 
 /**
  * Command to try to suckset speed of shooter motor
  * @author wil
  */
 public class RecoverBoulder extends Command {
-    private static final double speed = -0.8f;
-    private static final long time = 5000;
+    private static final double speed = -1f;
+    private static final long time = 40;
     private long endTime;
-
+    private double previous;
     /**
      * Constructor for RecoverBoulder
      */
@@ -23,25 +22,29 @@ public RecoverBoulder() {
 
     @Override
     protected void initialize() {
+        previous = shooter.getSpeed();
         endTime = System.currentTimeMillis() + time;
+        shooter.setSpeed(speed);
     }
 
     @Override
     protected void execute() {
-        shooter.setSpeed(speed);
+        shooter.toggle(true);
     }
 
     @Override
     protected boolean isFinished() {
-        return /*intake.isCaptured() ||*/ System.currentTimeMillis() > endTime;
+        return System.currentTimeMillis() > endTime;
     }
 
     @Override
     protected void end() {
-        shooter.stop();
+        shooter.toggle(false);
+        shooter.setSpeed(previous);
     }
 
     @Override
     protected void interrupted() {
+        end();
     }
 }
diff --git a/src/main/java/org/usfirst/frc/team5687/robot/commands/SetShooterSpeed.java b/src/main/java/org/usfirst/frc/team5687/robot/commands/SetShooterSpeed.java
@@ -1,7 +1,6 @@
 package org.usfirst.frc.team5687.robot.commands;
 
 import edu.wpi.first.wpilibj.command.Command;
-import org.usfirst.frc.team5687.robot.Constants;
 
 import static org.usfirst.frc.team5687.robot.Robot.shooter;
 
@@ -10,34 +9,26 @@
  * @author wil
  */
 public class SetShooterSpeed extends Command {
-    private double speed;
-    private long time;
-    private long endTime;
-
-    /**
-     * Constructor for SetShooterSpeed
-     * @param speed motor speed to run the shooter
-     * @param time duration of time to run the shooter at speed
-     */
-    public SetShooterSpeed(double speed, long time) {
+    private double _speed;
+
+    public SetShooterSpeed(double speed) {
         requires(shooter);
-        this.speed = speed;
-        this.time = time;
+        _speed = speed;
     }
 
+
     @Override
     protected void initialize() {
-        endTime = System.currentTimeMillis() + time;
+        shooter.setSpeed(_speed);
     }
 
     @Override
     protected void execute() {
-        shooter.setSpeed(speed);
     }
 
     @Override
     protected boolean isFinished() {
-        return System.currentTimeMillis() > endTime;
+        return true;
     }
 
     @Override