New shooter command new

.gitignore

@@ -1,3 +1,6 @@
+trajectory_*.csv
+trajectory_*.png
+
 # Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
 
 ### C++ ###

fieldmap.py

@@ -14,6 +14,7 @@
 import argparse
 import math
 import matplotlib.pyplot as plt
+import csv
 
 # unless otherwise noted, these values are from the Manual
 
@@ -82,35 +83,34 @@ def sc_y_pos(index):
         v = 30
     return v
 
-def draw_marker(x, y, color="orange"):
-    logging.info(f'Marker: ({x:.3f}, {y:.3f})')
 
+def draw_circle(x, y, color="orange", radius=2.5, zorder=100):
     # zorder pulls the balls to the front, so they are on top of the lines
-    c = plt.Circle((x, y), 2.5, fill=True, color=color, zorder=100)
+    c = plt.Circle((x, y), radius, fill=True, color=color, zorder=zorder)
     plt.gcf().gca().add_artist(c)
     return
 
+
+def draw_marker(x, y, color="orange"):
+    logging.info(f'Marker: ({x:.3f}, {y:.3f})')
+    draw_circle(x, y, color=color)
+    return
+
+
 def draw_one_ball(x, y):
     logging.info(f'Ball: ({x:.3f}, {y:.3f})')
-
-    # zorder pulls the balls to the front, so they are on top of the lines
-    c = plt.Circle((x, y), ball_rad, fill=True, color='yellow', zorder=100)
-    plt.gcf().gca().add_artist(c)
+    draw_circle(x, y, radius=ball_rad, color='yellow')
     return
 
 
 # there are always matching balls, reflected across the center
 # so draw a pair
 def draw_ball(x, y):
     logging.info(f'Ball: ({x:.3f}, {y:.3f})')
-
-    # zorder pulls the balls to the front, so they are on top of the lines
-    c = plt.Circle((x, y), ball_rad, fill=True, color='yellow', zorder=100)
-    plt.gcf().gca().add_artist(c)
+    draw_circle(x, y, radius=ball_rad, color='yellow')
 
     logging.info(f'Ball: ({field_length - x:.3f}, {field_width - y:.3f})')
-    c = plt.Circle((field_length - x, field_width - y), ball_rad, fill=True, color='yellow', zorder=100)
-    plt.gcf().gca().add_artist(c)
+    draw_circle(field_length - x, field_width - y, radius=ball_rad, color='yellow')
     return
 
 
@@ -163,6 +163,7 @@ def draw_galactic_search(map_name):
         draw_one_ball(sc_x_pos(10), sc_y_pos("D"))
     return
 
+
 def draw_slalom():
     # set the plot area to be the size of the field
     axis1.set_xlim((0, sc_field_length))
@@ -184,6 +185,7 @@ def draw_slalom():
     draw_marker(sc_x_pos(10), sc_y_pos("D"))
     return
 
+
 def draw_bounce():
     # set the plot area to be the size of the field
     axis1.set_xlim((0, sc_field_length))
@@ -215,6 +217,7 @@ def draw_bounce():
     draw_marker(sc_x_pos(9), sc_y_pos("A"), "green")
     return
 
+
 def draw_lightspeed():
     # set the plot area to be the size of the field
     axis1.set_xlim((0, sc_field_length))
@@ -243,6 +246,7 @@ def draw_lightspeed():
     draw_marker(sc_x_pos(10), sc_y_pos("D"))
     return
 
+
 def draw_barrel():
     # set the plot area to be the size of the field
     axis1.set_xlim((0, sc_field_length))
@@ -261,6 +265,7 @@ def draw_barrel():
     draw_marker(sc_x_pos(10), sc_y_pos("D"))
     return
 
+
 def draw_competition_map():
     '''Create the map for 2021 competition field'''
 
@@ -348,10 +353,26 @@ def draw_competition_map():
     return
 
 
+def plot_trajectory(trajfile):
+    with open(trajfile) as f:
+        incsv = csv.DictReader(f)
+        x = []
+        y = []
+        for row in incsv:
+            if int(row['IsWaypoint']):
+                draw_circle(float(row['X']), float(row['Y']), color='blue', radius=1)
+            else:
+                x.append(float(row['X']))
+                y.append(float(row['Y']))
+    plt.plot(x, y, 'red')
+    return
+
+
 map_choices = ('competition', 'redA', 'redB', 'blueA', 'blueB', "slalom", "bounce", "barrel", "lightspeed")
 
 parser = argparse.ArgumentParser(description='Output a PNG of a simple field map')
 parser.add_argument('--map', '-m', required=True, choices=map_choices, help='Which map to produce')
+parser.add_argument('--trajectory', '-t', help='Trajectory CSV')
 parser.add_argument('--verbose', '-v', default=0, action='count', help='Verbose')
 
 args = parser.parse_args()
@@ -379,6 +400,11 @@ def draw_competition_map():
     logging.error(f"Map '{args.map}' not implemented")
     sys.exit(10)
 
+outname = f"fieldmap_{args.map}.png"
+if args.trajectory:
+    plot_trajectory(args.trajectory)
+    outname = f"trajectory_{args.map}.png"
+
 plt.axis('off')
-plt.savefig(f"fieldmap_{args.map}.png", bbox_inches='tight', pad_inches=0, dpi=200, transparent=True)
+plt.savefig(outname, bbox_inches='tight', pad_inches=0, dpi=200, transparent=True)
 plt.show()

src/main/java/frc/robot/Constants.java

@@ -61,7 +61,7 @@ public final class Constants {
     public static final int SHOOTER_ONE_CAN_ID = 5; // Motor 1 on shooter
     public static final int SHOOTER_TWO_CAN_ID = 6; // Motor 2 on shooter
     public static final int SHOOTER_THREE_CAN_ID = 7; // Motor 3 on shooter
-    public static final int SHOOTER_SERVO_PWM_ID = 0; // Servo to controll hood angle 
+    public static final int SHOOTER_SERVO_PWM_ID = 0; // Servo to control hood angle 
     public static final int SHOOTER_TURRET_SERVO_ID = 1; // TODO: Fix this
 
     public static final double TURRET_ANGLE_ZERO_SETTING = 135.0;
@@ -70,29 +70,28 @@ public final class Constants {
 
     public static final double TURRET_ANGLE_COEFFICIENT = 180; // TODO: Fix this
 
-    public static final double WARM_UP_RPM = 3000; // TODO: find a good valuabl
+    public static final double WARM_UP_RPM = 3000; // TODO: find a good value
 
     public static final double MAX_TURRET_OFFSET = 10; // in degrees (may be more)
 
     // Set Shoulder parameters
     public static final int SHOULDER_MOTOR_CAN_ID = 11;; // shoulder for the climber
-    public static final double SHOULDER_MIN_HEIGHT = 0.44;
+    public static final double SHOULDER_MIN_HEIGHT = 0.4418;
     // If arm gets below 10 degrees above min height, we'll either stop the motor and let if fall in brake mode
     // or we'll just move t down really slowly.
     public static final double SHOULDER_MIN_VELOCITY_BUFFER = 2.0/360.0; // 2 degrees;
     public static final double SHOULDER_MIN_VELOCITY_HEIGHT = SHOULDER_MIN_HEIGHT + SHOULDER_MIN_VELOCITY_BUFFER;
     public static final double SHOULDER_MAX_HEIGHT = 0.604; 
     public static final double SHOULDER_CLIMB_HEIGHT = 0.62;
-    public static final double SHOULDER_RELEASE_HEIGHT = 0.6;
-    public static final double SHOULDER_START_HEIGHT = 0.59;
+    public static final double SHOULDER_RELEASE_HEIGHT = 0.592;
+    public static final double SHOULDER_START_HEIGHT = 0.589;
     public static final double SHOULDER_HEIGHT_FOR_SPRING_TO_LIFT = 0.25;
     public static final double SHOULDER_HEIGHT_FOR_RAISE1 = 0.48;
     public static final double SHOULDER_HEIGHT_FOR_FRAME_PERIMETER = 0.56 - 2.0/360.0; 
     public static final double SHOULDER_HEIGHT_FOR_MAX_CLIMB = 0.62;
 
-    public static final double SHOULDER_SPEED_UP = 4.0; //VOLTAGE~!!!!!
-    public static final double SHOULDER_SPEED_HOLD = 0.3
-    ;
+    public static final double SHOULDER_SPEED_UP = 4.0;   // VOLTAGE!!!!!
+    public static final double SHOULDER_SPEED_HOLD = 0.3;
     public static final double SHOULDER_RATE_DOWN = 40.0/360.0 * 12;
     // NOTE: This has to be negative to keep the robot level during climb
     public static final double SHOULDER_SPEED_LEVEL = -5.5;

src/main/java/frc/robot/FieldMapHome.java

@@ -1,13 +1,13 @@
 package frc.robot;
-
+ 
 import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.util.Units;
 
 //Class for the Infinate Recharge At Home challanges Field Map
 public class FieldMapHome {
 
-    public static double fieldLength = 360.0;
-    public static double fieldWidth = 180.0;
+    public static double fieldLength = Units.inchesToMeters(360.0);
+    public static double fieldWidth = Units.inchesToMeters(180.0);
 
     public FieldMapHome(){
 

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

@@ -17,7 +17,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.*;
-import frc.robot.commands.InterstellarAccuracy.WaitForSmartDashboard;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -33,6 +32,7 @@ public class Robot extends TimedRobot {
   SendableChooser<AutoCommandInterface> chosenAuto = new SendableChooser<>();
 
   private AutoCommandInterface m_prevAutoCommand = null;
+  private TrajectoryPlotter m_plotter;
 
   //returns the time since initialization in seconds as a double
   public static double time() {
@@ -54,6 +54,8 @@ public void robotInit() {
     // Set motors to coast so it's easier to move the robot.
     m_robotContainer.robotDrive.setIdleMode(IdleMode.kCoast);
 
+    m_plotter = new TrajectoryPlotter(m_robotContainer.robotDrive.getField2d());
+
     //m_robotContainer.shooter.calibratePID(0.000085, 0.000000033, 0, 6.776 * 0.00001);
 
     SmartDashboard.getEntry("tableUpdateRate").addListener((EntryNotification e)->NetworkTableInstance.getDefault().setUpdateRate(e.value.getDouble()), EntryListenerFlags.kUpdate|EntryListenerFlags.kNew);
@@ -87,19 +89,18 @@ public void robotInit() {
     */
 
     chosenAuto.setDefaultOption("None", null);
-    chosenAuto.addOption("RedAAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.driveCommand, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.RedA));
-    chosenAuto.addOption("RedBAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.driveCommand, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.RedB));
-    chosenAuto.addOption("BlueAAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.driveCommand, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.BlueA));
-    chosenAuto.addOption("BlueBAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.driveCommand, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.BlueB));
-    chosenAuto.addOption("Barrel", new AutoNavPaths(m_robotContainer.robotDrive, m_robotContainer.driveCommand, AutoNavPaths.Path.Barrel));
-    chosenAuto.addOption("Slalom", new AutoNavPaths(m_robotContainer.robotDrive, m_robotContainer.driveCommand, AutoNavPaths.Path.Slalom));
-    chosenAuto.addOption("Bounce", new BounceAuto(m_robotContainer.robotDrive, m_robotContainer.driveCommand));
-    chosenAuto.addOption("VisionPath", new GalacticSearchAutoSelector(m_robotContainer.robotDrive, m_robotContainer.driveCommand, m_robotContainer.carousel, m_robotContainer.intake, m_robotContainer.vision));
-    chosenAuto.addOption("InterstellarAccuracy", new InterstellarAccuracy(m_robotContainer.robotDrive, m_robotContainer.driveCommand,
-        m_robotContainer.shooter, m_robotContainer.carousel, m_robotContainer.carouselCommand));
-
-        SmartDashboard.putData("Chosen Auto", chosenAuto);
-    WaitForSmartDashboard.initSmartDashboard();
+    chosenAuto.addOption("RedAAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.RedA, m_robotContainer.climber));
+    chosenAuto.addOption("RedBAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.RedB, m_robotContainer.climber));
+    chosenAuto.addOption("BlueAAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.BlueA, m_robotContainer.climber));
+    chosenAuto.addOption("BlueBAuto", new GalacticSearchAuto(m_robotContainer.robotDrive, m_robotContainer.carousel, m_robotContainer.intake, GalacticSearchAuto.Path.BlueB, m_robotContainer.climber));
+    chosenAuto.addOption("Barrel", new AutoNavPaths(m_robotContainer.robotDrive, AutoNavPaths.Path.Barrel, m_robotContainer.climber));
+    chosenAuto.addOption("Slalom", new AutoNavPaths(m_robotContainer.robotDrive, AutoNavPaths.Path.Slalom, m_robotContainer.climber));
+    chosenAuto.addOption("Bounce", new BounceAuto(m_robotContainer.robotDrive, m_robotContainer.climber));
+    chosenAuto.addOption("VisionPath", new GalacticSearchAutoSelector(m_robotContainer.robotDrive, m_robotContainer.carousel, m_robotContainer.intake, m_robotContainer.vision, m_robotContainer.climber));
+    chosenAuto.addOption("InterstellarAccuracy", new InterstellarAccuracy(m_robotContainer.robotDrive, m_robotContainer.shooter,
+         m_robotContainer.carousel, m_robotContainer.carouselCommand, m_robotContainer.climber));
+
+    SmartDashboard.putData("Chosen Auto", chosenAuto);
   }
 
   /**
@@ -149,6 +150,10 @@ public void disabledPeriodic() {
     if (autoCommandInterface != null && autoCommandInterface != m_prevAutoCommand) {
       m_robotContainer.robotDrive.setPose(autoCommandInterface.getInitialPose());
       m_prevAutoCommand = autoCommandInterface;
+
+      if (Robot.isSimulation()) {
+        autoCommandInterface.plotTrajectory(m_plotter);
+      }
     }
   }
 

src/main/java/frc/robot/RobotContainer.java

@@ -97,12 +97,17 @@ private void configureButtonBindings() {
     }
 
     JoystickButton xboxA = new JoystickButton(xbox, Constants.XBOX_A);
-    xboxA.whenPressed(new ShooterCommand(shooter, carousel, robotDrive, carouselCommand, true));
+    xboxA.whenPressed(new ShooterCommand(shooter, carousel, carouselCommand));
+
+    JoystickButton xboxX = new JoystickButton(xbox, Constants.XBOX_X);
+    xboxX.whenPressed(new DeployIntake(climber));
+
     JoystickButton xboxB = new JoystickButton(xbox, Constants.XBOX_B);
     xboxB.whenPressed(new TurnShootTurnBack(robotDrive, shooter, carousel, carouselCommand, driveCommand, 180.0));
-    // JoystickButton xboxX = new JoystickButton(xbox, Constants.XBOX_X);
+
     JoystickButton xboxY = new JoystickButton(xbox, Constants.XBOX_Y);
     xboxY.whenPressed(new TurnAndShoot(robotDrive, shooter, carousel, carouselCommand, driveCommand, true));
+
     // JoystickButton xbox7 = new JoystickButton(xbox, Constants.XBOX_BACK);
     // JoystickButton xboxStart = new JoystickButton(xbox, Constants.XBOX_START);
 

src/main/java/frc/robot/commands/AutoCommandInterface.java

@@ -8,4 +8,7 @@
 public interface AutoCommandInterface extends Command {
     public Pose2d getInitialPose();
 
+    public default void plotTrajectory(TrajectoryPlotter plotter) {
+        plotter.clear();
+    }
 }

src/main/java/frc/robot/commands/AutoNavPaths.java

@@ -15,11 +15,14 @@
 // import edu.wpi.first.wpilibj.trajectory.Trajectory.State;
 import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 import frc.robot.Constants;
 import frc.robot.FieldMapHome;
+import frc.robot.Robot;
+import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.DriveTrain;
 
 public class AutoNavPaths extends SequentialCommandGroup implements AutoCommandInterface {
@@ -31,11 +34,12 @@ public enum Path{
     // Define the initial pose to be used by this command. This will be used in the initial trajectory
     // and will allow the system to query for it
     private Pose2d initialPose;
+    private List<Translation2d> waypointList = null;    // set to null to suppress warning
+    private Trajectory forwardTrajectory;
 
-    public AutoNavPaths(DriveTrain robotDrive, DriveCommand drivecommand, Path autoID) {
+    public AutoNavPaths(DriveTrain robotDrive, Path autoID, Climber climber) {
         final Rotation2d rotation = Rotation2d.fromDegrees(0.0);
 
-        List<Translation2d> waypointList = null;    // set to null to suppress warning
         Pose2d endPose = null;
 
         // Define these here, but we may override them within the case statement so we can tune each
@@ -94,7 +98,7 @@ public AutoNavPaths(DriveTrain robotDrive, DriveCommand drivecommand, Path autoI
         TrajectoryConfig configForward = new TrajectoryConfig(maxSpeed, maxAccel)
                 .setKinematics(Constants.kDriveKinematics).addConstraint(autoVoltageConstraint).addConstraint(centripetalAccelerationConstraint).setReversed(false);
 
-        Trajectory forwardTrajectory = TrajectoryGenerator.generateTrajectory(
+        forwardTrajectory = TrajectoryGenerator.generateTrajectory(
                 // Start at the origin facing the +X direction
                 initialPose,
                 waypointList, 
@@ -107,6 +111,11 @@ public AutoNavPaths(DriveTrain robotDrive, DriveCommand drivecommand, Path autoI
         //     System.out.println("DEBUG: backTrajectory STATE "+ state.poseMeters);
         // }
         System.out.println("Path time = " + forwardTrajectory.getTotalTimeSeconds());
+        if (Robot.isSimulation()) {
+            TrajectoryWriter writer = new TrajectoryWriter(autoID.name());
+            writer.WriteTrajectory(forwardTrajectory);
+            writer.WriteWaypoints(initialPose, waypointList, endPose);
+        }
 
         RamseteCommand ramseteForward = new RamseteCommand(
             forwardTrajectory,
@@ -125,12 +134,17 @@ public AutoNavPaths(DriveTrain robotDrive, DriveCommand drivecommand, Path autoI
 
         addCommands(
             // Run the backward trajectory and then stop when we get to the end
-            ramseteForward.andThen(() -> robotDrive.tankDriveVolts(0, 0))
+            new ParallelCommandGroup(new DeployIntake(climber),ramseteForward).andThen(() -> robotDrive.tankDriveVolts(0, 0))
         );
     }
 
     // Allows the system to get the initial pose of this command
     public Pose2d getInitialPose() {
         return initialPose;
     }
+
+    public void plotTrajectory(TrajectoryPlotter plotter) {
+        plotter.plotTrajectory(forwardTrajectory);
+        plotter.plotWaypoints(waypointList);
+    }
 }