Changes from late night Wednesday before GSD. Mostly related to targeting and initial curve-fits for shooter range determination, wheel speed, and hood angle.

coach-streeter · coach-streeter · commit 1e3550db6a9f · 2020-02-27T13:50:53.000-05:00
diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java
@@ -177,8 +177,8 @@ private void configureDriverPadButtons() {
         // about -30 degrees
         // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new
         // ShooterSetTurretVBus(+0.2));// about +30 degrees
-        DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION));
-        DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
+        DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION));
+        DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));
 
         // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new
         // ShooterSetTurretRel(-200.0));
@@ -195,16 +195,17 @@ private void configureDriverPadButtons() {
         // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0));
 
         // Debug shooter pid velocity
-        DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(100.0));
-        DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-100.0));
+        DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0));
+        DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0));
         DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0));
         DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000));
 
         DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1));
         // TODO: above hard-coded constant (3000) should be a named constant from
         // Shooter.java
 
-        DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0));
+        DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterPermissionToFire());
+        DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0));
 
     }
 
@@ -221,7 +222,7 @@ private void configureOperatorPadButtons() {
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0));
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollersWhileHeld(-1.0));
 
-        OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget());
+        OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whenPressed(new ShooterAimToTarget());
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollersWhileHeld(1.0));
 
         OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true));
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java
@@ -29,7 +29,7 @@ public StartBWOppTrench() {
                 addCommands(new IntakeSetRollers(-1.0));
 
                 // make sure the hood is down
-                addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
+                addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
 
                 // drive to get balls from opponent's trench
                 addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 108, 180));
@@ -46,7 +46,7 @@ public StartBWOppTrench() {
                 // in shooting position, prepare everything for shooting
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
-                                new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION + 3000.0),
+                                new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0),
                                 new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 // turn on the intake gently while shooting to help balls settle
@@ -58,6 +58,6 @@ public StartBWOppTrench() {
                 addCommands(new IntakeSetRollers(0.0));
 
                 // turn the wheel off now that the shooting is all done
-                addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
+                addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
         }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java
@@ -28,7 +28,7 @@ public StartBWShoot3() {
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
                                 new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
-                                new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION),
+                                new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 addCommands(new ShooterReadyAimFire(1.5));
@@ -37,6 +37,6 @@ public StartBWShoot3() {
                 addCommands(new ParallelCommandGroup( // below commands in parallel
                                 new ShooterWheelSet(0.0), //
                                 new IntakeSetRollers(0.0), // turn off the rollers
-                                new HoodSetAbs(Hood.HOOD_STARTING_POSITION)));
+                                new HoodSetAbs(Hood.STARTING_POSITION)));
         }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java
@@ -32,7 +32,7 @@ public StartBWTrench() {
                                 // intake while driving down the trench
                                 new IntakeSetRollers(-1.0),
                                 // ensure the hood is down
-                                new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION),
+                                new HoodSetAbsWhileHeld(Hood.STARTING_POSITION),
                                 // drive the path under the control panel to the end
                                 new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180)));
 
@@ -50,7 +50,7 @@ public StartBWTrench() {
                 // wheels, raising the hood, and re-aiming the turret
                 addCommands(new ParallelCommandGroup(
                                 new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
-                                new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION),
+                                new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)),
                                 new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160)));
 
@@ -68,6 +68,6 @@ public StartBWTrench() {
                 addCommands(new IntakeSetRollers(0.0));
 
                 // turn the wheel off now that the shooting is all done
-                addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
+                addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
         }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java
@@ -31,7 +31,7 @@ public StartFWRendezvous() {
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
                                 new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
-                                new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION),
+                                new HoodSetAbs(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 addCommands(new ShooterReadyAimFire(1.0));
@@ -44,8 +44,7 @@ public StartFWRendezvous() {
                 addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN));
 
                 // rais the hood a little to shoot from this increased distance
-                addCommands(new HoodSetAbsWhileHeld(
-                                (Hood.HOOD_INITIATION_LINE_POSITION + Hood.HOOD_TRENCH_MID_POSITION) / 2.0));
+                addCommands(new HoodSetAbsWhileHeld((Hood.INITIATION_LINE_POSITION + Hood.TRENCH_MID_POSITION) / 2.0));
                 addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 144, 50));
 
                 // turn on the intake to pick up balls
@@ -70,6 +69,6 @@ public StartFWRendezvous() {
                 addCommands(new IntakeSetRollers(0.0));
 
                 // turn the wheel off now that the shooting is all done
-                addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION));
+                addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION));
         }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java
@@ -28,7 +28,7 @@ public StartFWShoot3() {
                 addCommands(new ParallelCommandGroup( // run the following commands in parallel:
                                 new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN),
                                 new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED),
-                                new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION),
+                                new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION),
                                 new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE)));
 
                 addCommands(new ShooterReadyAimFire(1.0));
@@ -37,6 +37,6 @@ public StartFWShoot3() {
                 addCommands(new ParallelCommandGroup( // below commands in parallel
                                 new ShooterWheelSet(0.0), //
                                 new IntakeSetRollers(0.0), // turn off the rollers
-                                new HoodSetAbs(Hood.HOOD_STARTING_POSITION)));
+                                new HoodSetAbs(Hood.STARTING_POSITION)));
         }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.mayheminc.robot2020.commands;
+
+import org.mayheminc.robot2020.RobotContainer;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class ShooterAimToTarget extends CommandBase {
+  /**
+   * Creates a new TurretAimToTarget.
+   */
+  public ShooterAimToTarget() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.hood);
+    addRequirements(RobotContainer.shooterWheel);
+    addRequirements(RobotContainer.turret);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    // aim the hood and turret
+    RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood());
+    RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth());
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // aim the hood and turret
+    RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood());
+    RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    if (!interrupted) {
+      RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed());
+
+      // want to also schedule a command here that does the shooting!
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (RobotContainer.hood.isAtPosition() && RobotContainer.turret.isAtDesiredPosition());
+  }
+}
diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java
@@ -30,17 +30,30 @@ public ShooterPermissionToFire() {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    // do the following: command the shooterWheel to turn at speed, aimTurret,
-    // aimHood
+    // command the shooterWheel to turn at speed, aimHood, aimTurret
+    // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed());
+    RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood());
+    RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth());
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    // command the shooterWheel to turn at speed
+    // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getWheelSpeedFromY());
+
+    // aim the hood
+    RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood());
+
+    // aim the turret
+    RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth());
+
+    // if speed is good, turret is aimed, and hood is aimed, start shooting!
     // if shooterWheels at speed, turret aimed, and hood aimed, then fire!
     // firing actions as follows:
     // feed roller after 0.0 seconds since start of firing
     // chimney after 0.1 seconds since start of firing
+
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java
@@ -46,6 +46,6 @@ public ShooterReadyAimFire(double waitDuration) {
         new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), new Wait(0.1)));
 
     // Lower the hood now that we're done shooting
-    addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION));
+    addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));
   }
 }
diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java
@@ -14,13 +14,15 @@
 public class Hood extends SubsystemBase implements PidTunerObject {
     private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD);
 
-    public final static double HOOD_STARTING_POSITION = 0;
-    public final static double HOOD_TARGET_ZONE_POSITION = 5000;
-    public final static double HOOD_INITIATION_LINE_POSITION = 65000;
-    public final static double HOOD_TRENCH_MID_POSITION = 80000;
-
+    private final static int MIN_POSITION = 0;
+    private final static int MAX_POSITION = 90000;
     private final static double POSITION_TOLERANCE = 1000.0;
 
+    public final static double STARTING_POSITION = 0;
+    public final static double TARGET_ZONE_POSITION = 5000;
+    public final static double INITIATION_LINE_POSITION = 65000;
+    public final static double TRENCH_MID_POSITION = 80000;
+
     private double m_desiredPosition = 0.0;
 
     /**
@@ -49,9 +51,9 @@ private void configureTalon() {
         hoodTalon.setInverted(true);
         hoodTalon.setSensorPhase(true);
 
-        hoodTalon.configForwardSoftLimitThreshold(100000);
+        hoodTalon.configForwardSoftLimitThreshold(MAX_POSITION);
         hoodTalon.configForwardSoftLimitEnable(true);
-        hoodTalon.configReverseSoftLimitThreshold(0);
+        hoodTalon.configReverseSoftLimitThreshold(MIN_POSITION);
         hoodTalon.configReverseSoftLimitEnable(true);
     }
 
@@ -63,6 +65,7 @@ public void periodic() {
 
     private void UpdateDashboard() {
         SmartDashboard.putNumber("Shooter Hood Pos", hoodTalon.getPosition());
+        SmartDashboard.putNumber("Shooter Hood Setpoint", m_desiredPosition);
     }
 
     public void zero() {
diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java
@@ -124,7 +124,14 @@ private void updatePivotPower() {
       // if the pivot is close enough, turn the motor on gently downards
       if (Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) {
         isMoving = false;
-        setPivotVBus(-0.05);
+
+        // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little
+        // positive power.
+        if (pivotTalon.getPosition() > PIVOT_UP / 2) {
+          setPivotVBus(+0.05);
+        } else { // we are close to the PIVOT DOWN, so apply a little negative power.
+          setPivotVBus(-0.05);
+        }
       } else {
         pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward);
       }
diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java

Original file line number	Diff line number	Diff line change
`@@ -46,6 +46,6 @@ public ShooterReadyAimFire(double waitDuration) {`
`46`	`46`	`new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), new Wait(0.1)));`
`47`	`47`
`48`	`48`	`// Lower the hood now that we're done shooting`
`49`		`- addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION));`
	`49`	`+ addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION));`
`50`	`50`	`}`
`51`	`51`	`}`