Changes from practice field on Friday, 21 Feb 2020. Tuned azimuth control (thresholds, FOV sizing, PID tuning, motion magic, camera lag), tweaked "drive straight" PID loop, initial autonomous shooting and "trench run" programs. Added various "TODO" comments in code; still lots of others!

Author: coach-streeter

src/main/java/frc/robot/Robot.java

@@ -7,9 +7,12 @@
 
 package frc.robot;
 
+import java.util.Date;
+
 import org.mayheminc.robot2020.RobotContainer;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -35,6 +38,9 @@ public void robotInit() {
     // and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    // TODO:  Improve below to display something like "Code Last Loaded:  7 minutes ago"
+    SmartDashboard.putString("Robot Loaded on:", new Date().toLocaleString());
   }
 
   /**

src/main/java/org/mayheminc/robot2020/RobotContainer.java

@@ -80,9 +80,12 @@ private void configureDefaultCommands() {
 
     private void configureAutonomousPrograms() {
         LinkedList<Command> autonomousPrograms = new LinkedList<Command>();
-
-        autonomousPrograms.push(new StayStill());
-        autonomousPrograms.push(new DriveStraight());
+        // TODO:  fix "wierdness" with auto program selection - sometimes doesn't seem to work
+        // TODO:  fix so that auto program is shown not just when changed (as shows old setting sometimes)
+        
+        // autonomousPrograms.push(/* 01 */ new StayStill());
+        autonomousPrograms.push(/* 00 */ new TrenchAuto());
+        // autonomousPrograms.push( new ShooterReadyAimFire());
         // autonomousPrograms.push(new TestTurret());
 
         autonomous.setAutonomousPrograms(autonomousPrograms);
@@ -139,11 +142,21 @@ private void configureDriverPadButtons() {
         // ShooterSetTurretAbs(-5500));// about -30 degrees
         // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new
         // ShooterSetTurretAbs(+5500));// about +30 degrees
-        DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// about -30 degrees
-        DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));// about +30 degrees
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));//
+        // 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 ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
         DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION));
 
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new
+        // ShooterSetTurretRel(-200.0));
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new
+        // ShooterSetTurretRel(+200.0));
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetTurretAbs(+0.0));
+        // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new
+        // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION));
+
         // Debug Hood
         // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodRel(+1000));
         // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodRel(-1000));
@@ -154,7 +167,8 @@ private void configureDriverPadButtons() {
         DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0));
         DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0));
         DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0));
-        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(2700));
+        DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(3000));
+        // TODO:  above hard-coded constant (3000) should be a named constant from Shooter.java
 
         DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(1.0));
 

src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java

@@ -22,10 +22,8 @@ public DriveStraight() {
     addCommands(new DriveZeroGyro());
     // addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0));
     addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 50, 0));
-    addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION));
+    addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
     // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0));
 
-    // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new
-    // MagazineSetTurntable(0.0)));
   }
 }

src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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.autonomousroutines;
+
+import org.mayheminc.robot2020.RobotContainer;
+import org.mayheminc.robot2020.commands.*;
+import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits;
+import org.mayheminc.robot2020.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj2.command.*;
+
+public class TrenchAuto extends SequentialCommandGroup {
+    /**
+     * Add your docs here.
+     */
+    public TrenchAuto() {
+
+        addCommands(new DriveZeroGyro());
+        // first, shoot the balls that were pre-loaded
+
+        addCommands(new IntakeSetPositionWithoutWaiting(RobotContainer.intake.PIVOT_DOWN));
+        addCommands(new ShooterReadyAimFire());
+
+        // then, perform a 3-point turn
+        addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 160));
+
+        // pick up balls while heading down the trench.
+        addCommands(new ParallelRaceGroup(
+                // intake while driving down the trench
+                new IntakeSetRollers(-1.0),
+                new SequentialCommandGroup(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 132, 180),
+                        new Wait(0.5), new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 12, 180))));
+
+        // after getting all three balls, go back to shooting position
+        addCommands(new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 24, 30));
+        addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0));
+        addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0));
+
+        addCommands(new ShooterReadyAimFire());
+
+        // turn the wheel off now that the shooting is all done
+        addCommands( new ShooterSetWheel(0.0));
+
+        addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION));
+    }
+}

src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java

@@ -24,7 +24,7 @@ public AirCompressorDefault() {
   @Override
   public void initialize() {
     // robot.is
-    RobotContainer.compressor.setCompresor(true);
+    RobotContainer.compressor.setCompressor(true);
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* 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 IntakeSetPositionWithoutWaiting extends CommandBase {
+  double m_position;
+
+  /**
+   * Creates a new IntakeSetPositionWithoutWaiting.
+   */
+  public IntakeSetPositionWithoutWaiting(Double position) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.intake);
+    m_position = position;
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.intake.setPivot(m_position);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java

@@ -7,6 +7,9 @@
 
 package org.mayheminc.robot2020.commands;
 
+import org.mayheminc.robot2020.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
@@ -21,15 +24,31 @@ public ShooterReadyAimFire() {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     super();
-    addCommands(new TargetingIsOnTarget());
-    addCommands(new ShooterFireWhenReady());
-    addCommands(new ShooterSetFeeder(1.0));
-    addCommands(new ChimneySetChimney(0.5));
-    addCommands(new MagazineSetTurntable(0.3));
 
-    addCommands(new Wait(4.0));
+    addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION));
+    // addCommands(new ShooterSetWheel(3000.0, true));
+    
+    // addCommands(new Wait(3.0));
+
+    // TODO:  Add compressor control so that compressor is turned off while actively shooting.
+    
+    // aim to the target until we are on target.
+    addCommands(
+      new ParallelRaceGroup( 
+          new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)), 
+          new TurretAimToTarget()));
+
+    // turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the magazine, shoot for about 4 seconds
+    addCommands(new ParallelRaceGroup(
+                    new ShooterSetFeeder(1.0),
+                    new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)),
+                    new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)),
+                    new Wait(6.0)));
 
-    addCommands(new MagazineSetTurntable(0.0));
-    addCommands(new ChimneySetChimney(0.0));
+    addCommands(new ParallelRaceGroup(
+      new MagazineSetTurntable(0.0),
+      new ChimneySetChimney(0.0),
+      new ShooterSetFeeder(0.0),
+      new Wait(0.1)));
   }
 }

src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java

@@ -32,7 +32,9 @@ public void initialize() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    RobotContainer.shooter.setFeederSpeed(0.0);
+
+      RobotContainer.shooter.setFeederSpeed(0.0);
+
   }
 
 }

src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java

@@ -13,15 +13,21 @@
 
 public class ShooterSetWheel extends CommandBase {
   double m_rpm;
+  boolean m_waitForSpeed;
+
+  public ShooterSetWheel(double rpm) {
+    this(rpm, false);
+  }
 
   /**
    * Creates a new ShooterSetWheel.
    */
-  public ShooterSetWheel(double rpm) {
+  public ShooterSetWheel(double rpm, boolean wait) {
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(RobotContainer.shooter);
+    // addRequirements(RobotContainer.shooter);
 
     m_rpm = rpm;
+    m_waitForSpeed = wait;
   }
 
   // Called when the command is initially scheduled.
@@ -33,6 +39,13 @@ public void initialize() {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return true;
+    if( m_waitForSpeed)
+    {
+      return  (Math.abs( m_rpm - RobotContainer.shooter.getShooterWheelSpeed() ) < 100);
+    }
+    else
+    {
+      return true;
+    }
   }
 }

src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java

@@ -7,6 +7,8 @@
 
 package org.mayheminc.robot2020.commands;
 
+import org.mayheminc.robot2020.RobotContainer;
+
 // import org.mayheminc.robot2020.RobotContainer;
 // import org.mayheminc.robot2020.subsystems.Targeting;
 
@@ -50,6 +52,10 @@ public boolean isFinished() {
     // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget())
     // - RobotContainer.shooter.getShooterWheelSpeed()) < 100;
     // return bearingGood && wheelsGood;
-    return true;
+
+    double targetPos = RobotContainer.targeting.getDesiredAzimuth();
+    double turretPos = RobotContainer.shooter.getTurretPosition();
+
+    return ( Math.abs( targetPos - turretPos) < 50);
   }
 }