fixes

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index efcd7c24ac97faadb0c11396659fa5f761f2ed23..e4fdaef494f49039ae82ecefe178e792d4e7721f 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -13,8 +13,10 @@ import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.commands.DoNothing;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.commands.vision.ShutdownAllPis;
@@ -56,6 +58,8 @@ public class RobotContainer {
 
   private Command auto = new DoNothing();
 
+  // Auto Command selection
+  private final SendableChooser<Command> autoChooser = new SendableChooser<>();
 
   // Controllers are defined here
   private BaseDriverConfig driver = null;
@@ -67,7 +71,12 @@ public class RobotContainer {
    * Different robots may have different subsystems.
    */
   public RobotContainer(RobotId robotId) {
-    // climb = new Climb();
+    // display the current robot id on smartdashboard
+    SmartDashboard.putString("RobotID", robotId.toString());
+
+    // Filling the SendableChooser on SmartDashboard
+    autoChooserInit();
+
     // dispatch on the robot
     switch (robotId) {
       case TestBed1:
@@ -189,8 +198,32 @@ public class RobotContainer {
     }
   }
 
-  public Command getAutoCommand(){
-    return auto;
+  // Autos 
+
+  /**
+   * Initialize the SendableChooser on the SmartDashbboard.
+   * Fill the SendableChooser with available Commands.
+   */
+  public void autoChooserInit() {
+    // add the options to the Chooser
+    autoChooser.setDefaultOption("Do nothing", new DoNothing());
+    autoChooser.addOption("Do nada", new DoNothing());
+    autoChooser.addOption("Spin my wheels", new DoNothing());
+    autoChooser.addOption("Hello world", new InstantCommand(() -> System.out.println("Hello world")));
+
+    // put the Chooser on the SmartDashboard
+    SmartDashboard.putData("Auto chooser", autoChooser);
+  }
+
+  /**
+   * Gets the auto command from SmartDashboard
+   * @return
+   */
+  public Command getAutoCommand() {
+    // get the selected Command
+    Command autoSelected = autoChooser.getSelected();
+
+    return autoSelected;
   }
 
   public void logComponents(){

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index f171d9afe2fdf59cb554e59e60068cb221e7bbce..426a75bbdb5a25654b0fe41972783ae6d6d47835 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -19,8 +19,10 @@ public class IdConstants {
     // LEDs
     public static final int CANDLE_ID = 1;
 
-    // Motor ID
+    // Turret
     public static final int TURRET_MOTOR_ID = 20;
+    public static final int TURRET_ENCODER_LEFT_ID = 0;
+    public static final int TURRET_ENCODER_RIGHT_ID = 1;
 
     // Shooter
     public static final int SHOOTER_LEFT_ID = 10;

diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java
new file mode 100644 (file)
index 0000000..b4643d6
--- /dev/null
+++ b/src/main/java/frc/robot/constants/ShotInterpolation.java
@@ -0,0 +1,27 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.util.Units;
+
+public class ShotInterpolation {
+    public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
+    public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap();
+    public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap();
+    
+    public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap();
+
+    static{
+        timeOfFlightMap.put(0.0, 0.67);
+        timeOfFlightMap.put(1.0, 0.67);
+
+        shooterPowerMap.put(0.0, 1.0);
+        shooterPowerMap.put(1.0, 1.0);
+
+        hoodAngleMap.put(0.0, Units.degreesToRadians(90));
+        hoodAngleMap.put(1.0, Units.degreesToRadians(90));
+
+        //TODO: find actual values from video motion
+        exitVelocityMap.put(1.0, 2.0);
+        exitVelocityMap.put(2.0, 4.0);
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 89ac487df476e6027544f98ed11f155b68e2e411..70ade0d634917d7ce7846dced5da52c335aca654 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -31,8 +31,6 @@ public class Hood extends SubsystemBase implements HoodIO{
 
        private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02);
 
-       private double FEEDFORWARD_KV = 0.12;
-
        private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
        private double goalVelocityRadPerSec = 0.0;
        private double lastFilteredRad = 0.0;
@@ -116,7 +114,7 @@ public class Hood extends SubsystemBase implements HoodIO{
                motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(MIN_ANGLE_RAD) * GEAR_RATIO, Units.radiansToRotations(MAX_ANGLE_RAD) * GEAR_RATIO);
                
                // Multiply goal velocity by kV
-               double velocityCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV;
+               double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV;
 
                // Set control with feedforward
                motor.setControl(mmVoltageRequest

diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
index 03343b0916562dd029093f77b902965763c1dd0f..e38e6805982e40caa4216ab3267adbd2b4173cab 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
@@ -14,4 +14,6 @@ public class HoodConstants {
 
     public static final double MAX_ANGLE = 82; // degrees
     public static final double MIN_ANGLE = 58.5; // degrees 
+
+    public static final double FEEDFORWARD_KV = 0.12;
 }

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index ad29439a2824d35e4fabc08186db106cdbd0e7e6..2e4423a2eb00f7f92782110e5ed55bf0693e38c3 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -104,6 +104,6 @@ public class Shooter extends SubsystemBase implements ShooterIO {
      */
     @AutoLogOutput(key="Shooter/TargetSpeed")
     public double getTargetVelocity(){
-        return Units.rotationsToRadians(shooterTargetSpeed);
+        return shooterTargetSpeed;
     }
 }

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 0d26d1c70c37fa15d036d310abd2cd242c29af9a..17d72a7000869a382ab5cfbe15564d61e9b9bca1 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -10,7 +10,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID);
 
     private double power = 0.0;
-    public int ballCount = 0;
+    private int ballCount = 0;
     private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
     private boolean wasAboveThreshold = false;
 

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index f8d8885e162546f98eb6f4c4aa6c068be43c7128..c686ceff1b7db06964a6ee9117e482bd2d22ae46 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -48,8 +48,8 @@ public class Turret extends SubsystemBase implements TurretIO{
        /* ---------------- Hardware ---------------- */
 
        private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
-       private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
-    private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
+       private final CANcoder encoderLeft = new CANcoder(IdConstants.TURRET_ENCODER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private final CANcoder encoderRight = new CANcoder(IdConstants.TURRET_ENCODER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
 
        private TalonFXSimState simState;
        private SingleJointedArmSim turretSim;