Simulation

.gitignore

@@ -163,3 +163,8 @@ bin/
 imgui.ini
 
 # End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode
+
+# Ignore Display Settings on WPILib Simulation GUI
+simgui-ds.json
+simgui-window.json
+simgui.json

src/main/java/frc/team3128/Constants.java

@@ -8,6 +8,8 @@
 import frc.team3128.common.utility.interpolation.InterpolatingTreeMap;
 import static frc.team3128.common.hardware.motorcontroller.MotorControllerConstants.*;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
@@ -37,6 +39,9 @@ public static class DriveConstants {
         public static final double WHEEL_RADIUS_METERS = Units.inchesToMeters(3.029);
         public static final double TRACK_WIDTH_METERS = 0.56147;
 
+        public static final Pose2d HUB_POS = new Pose2d(Units.inchesToMeters(324), Units.inchesToMeters(162), new Rotation2d(0)); // meters
+        public static final double HUB_RADIUS = 26.69;
+
         public static final DifferentialDriveKinematics DRIVE_KINEMATICS = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
         public static final double ENCODER_DISTANCE_PER_MARK = WHEEL_RADIUS_METERS * 2 * Math.PI / FALCON_ENCODER_RESOLUTION;
         public static final double DRIVE_NU_TO_METER = ENCODER_DISTANCE_PER_MARK / DRIVE_GEARING; // meters driven per encoder tick
@@ -85,7 +90,7 @@ public static class ShooterConstants {
         public static final int LEFT_SHOOTER_ID = 10; 
         public static final int RIGHT_SHOOTER_ID = 9; 
 
-        public static final double kP = 2e-4; // 3.2e-3;
+        public static final double kP = 2e-4; // 3.2e-3; 
         public static final double kI = 0;
         public static final double kD = 0; // 5e-4;
 
@@ -106,6 +111,7 @@ public static class ShooterConstants {
         public static final double SHOOTER_RADIUS_METERS = 0.0508;
         public static final DCMotor SHOOTER_GEARBOX = DCMotor.getCIM(2);
         public static final double SHOOTER_GEARING = 1.5;
+        public static final double SHOOTER_MOMENT_OF_INERTIA = 0.001271812;
 
         public static final InterpolatingTreeMap<InterpolatingDouble, InterpolatingDouble> shooterSpeedsMap = new InterpolatingTreeMap<InterpolatingDouble, InterpolatingDouble>();
         static {
@@ -148,6 +154,10 @@ public static class HopperConstants {
         public static final double HOPPER_MOTOR_2_POWER = 0.7;
         public static final double REVERSE_HOPPER_MOTOR_POWER = -1;
 
+        public static final DCMotor GEARBOX = DCMotor.getVex775Pro(1);
+        public static final double HOPPER_MOTOR_GEAR_RATIO = 0.25;
+        public static final double HOPPER_MOMENT_OF_INERTIA = 0.0006; // 0.04e-4
+        public static final double HOPPER_MOMENT_OF_INERTIA_BALL = 0.16e-4; // 0.16e-4
     }
 
     public static class IntakeConstants {
@@ -212,6 +222,11 @@ public static class HoodConstants {
         public static final double MAX_ANGLE = 41.4; // deg
         public static final double HOME_ANGLE = 28.5; // deg
 
+        public static final DCMotor HOOD_GEARBOX = DCMotor.getNeo550(1);
+        public static final double HOOD_MOMENT_OF_INERTIA = 0.054195108;
+        public static final double HOOD_ARM_LENGTH_METERS = 0.2400046;
+        public static final double HOOD_ARM_MASS_KG = 0.795601019;
+
         public static InterpolatingTreeMap<InterpolatingDouble, InterpolatingDouble> hoodAngleMap = new InterpolatingTreeMap<InterpolatingDouble, InterpolatingDouble>();
         static {
 
@@ -278,4 +293,10 @@ public static class VisionConstants {
         public static final double BALL_VEL_THRESHOLD = 2.54; // m/s - 100 in/s 
         public static final int BALL_VEL_PLATEAU_THRESHOLD = 10;
     }
+    public static class SimConstants {
+        public static final double SIM_HUB_DISTANCE = 70; //degrees
+        public static final double CLIMBER_LONG_ARM_LENGTH = 0.78 * 39.3701; //inches
+        public static final double CLIMBER_SHORT_ARM_LENGTH = 0.2 * 39.3701; //inches
+        public static final double GRAVITATIONAL_CONSTANT = 9.8; // m/s^2
+    }
 }

src/main/java/frc/team3128/RobotContainer.java

@@ -178,8 +178,8 @@ private void configureButtonBindings() {
         m_leftStick.getButton(13).whenPressed(new InstantCommand(m_climber::bothExtend, m_climber))
                                 .whenReleased(new InstantCommand(m_climber::bothStop, m_climber));
 
-        m_leftStick.getButton(14).whenPressed(new InstantCommand(m_climber::bothRetract, m_climber))
-                                .whenReleased(new InstantCommand(m_climber::bothStop, m_climber));
+        m_leftStick.getButton(14).whenPressed(new InstantCommand(()-> m_hopper.runHopper(.325), m_hopper))
+                                .whenReleased(new InstantCommand(m_hopper::stopHopper, m_hopper));
 
         m_leftStick.getButton(12).whenPressed(new InstantCommand(m_climber::extendPiston, m_climber));
         m_leftStick.getButton(15).whenPressed(new InstantCommand(m_climber::retractPiston, m_climber));

src/main/java/frc/team3128/commands/CmdArcadeDrive.java

@@ -3,6 +3,7 @@
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import static frc.team3128.Constants.DriveConstants.*;
 import frc.team3128.subsystems.NAR_Drivetrain;
@@ -28,6 +29,7 @@ public CmdArcadeDrive(DoubleSupplier xSpeed, DoubleSupplier turn, DoubleSupplier
         m_throttle = throttle;
 
         addRequirements(m_drivetrain);
+
     }
 
     @Override

src/main/java/frc/team3128/commands/CmdClimbEncoder.java

@@ -1,7 +1,11 @@
 package frc.team3128.commands;
 
+import edu.wpi.first.wpilibj.IterativeRobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import static frc.team3128.Constants.ClimberConstants.*;
+
+import frc.team3128.Robot;
 import frc.team3128.subsystems.Climber;
 
 public class CmdClimbEncoder extends CommandBase{
@@ -32,6 +36,7 @@ public CmdClimbEncoder(double distance, double power) {
     @Override
     public void initialize() {
 
+        SmartDashboard.putNumber("Climber Tick Goal", this.distance);
         // want to go higher than current place = go up
         if (distance > m_climber.getCurrentTicks()) { 
             m_climber.bothExtend(power);
@@ -43,9 +48,21 @@ public void initialize() {
         }
     }
 
+
+
+
     @Override
     public void end(boolean interrupted) {
         m_climber.bothStop();
+
+        if(Robot.isSimulation()) {
+            if(distance == CLIMB_ENC_DIAG_EXTENSION) {
+                m_climber.latchLongArm();
+            }
+            else if(distance == m_climber.getDesiredTicks(SMALL_VERTICAL_DISTANCE)) {
+                m_climber.switchToShortArm();
+            }
+        }
     }
 
     @Override

src/main/java/frc/team3128/commands/CmdClimbTraversalGyro.java

@@ -1,5 +1,6 @@
 package frc.team3128.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -25,6 +26,7 @@ public CmdClimbTraversalGyro() {
             new WaitCommand(.25),
 
             new CmdClimbEncoder(m_climber.getDesiredTicks(SMALL_VERTICAL_DISTANCE)),
+            //Sim: Latch Short Arm
 
             new WaitCommand(.25),
             //piston extend
@@ -33,10 +35,12 @@ public CmdClimbTraversalGyro() {
             new WaitCommand(.5),
             //elev extend
             new CmdClimbEncoder(CLIMB_ENC_DIAG_EXTENSION),
+            //Sim: Latch Long Arm
 
             new WaitCommand(0.25),
 
             //piston retract
+            //Sim: Unlatch Short Arm
             new InstantCommand(() -> m_climber.retractPiston()),
             new WaitCommand(.5),
 
@@ -46,7 +50,8 @@ public CmdClimbTraversalGyro() {
             //Climber is manually fully retracted on High Bar
 
             new WaitCommand(0.25),
-
+
+            //Sim: Latch Short Arm            
             new CmdClimbEncoder(m_climber.getDesiredTicks(SMALL_VERTICAL_DISTANCE)),
 
             new InstantCommand(() -> m_climber.extendPiston()),
@@ -57,9 +62,13 @@ public CmdClimbTraversalGyro() {
 
             //elev extend
             new CmdClimbEncoder(CLIMB_ENC_DIAG_EXTENSION),
+            //Sim: Latch Long Arm
+
             new WaitCommand(.25),
+
 
             //piston extend
+            //Sim: Unlatch Short Arm
             new InstantCommand(() -> m_climber.retractPiston()),
 
             //elev retract

src/main/java/frc/team3128/commands/CmdIntakeCargo.java

@@ -1,5 +1,6 @@
 package frc.team3128.commands;
 import frc.team3128.subsystems.Intake;
+import frc.team3128.Robot;
 import frc.team3128.subsystems.Hopper;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -23,6 +24,10 @@ public CmdIntakeCargo(){
     public void initialize() {
         m_intake.runIntake();
         m_hopper.runHopper();
+
+        if (Robot.isSimulation()){
+            m_hopper.addBall();
+        }
     }
 
     // nothing in execute because runIntake() and runHopper() just set speeds - nothing is updated in command

src/main/java/frc/team3128/commands/CmdOuttake.java

@@ -2,6 +2,8 @@
 import frc.team3128.subsystems.Intake;
 import static frc.team3128.Constants.IntakeConstants.*;
 import static frc.team3128.Constants.HopperConstants.*;
+
+import frc.team3128.Robot;
 import frc.team3128.subsystems.Hopper;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -46,6 +48,10 @@ public CmdOuttake(double intakePower, double hopperPower){
     public void initialize() {
         m_intake.runIntake(intakePower);
         m_hopper.reverseHopper(hopperPower);
+
+        if (Robot.isSimulation()){
+            m_hopper.resetBallCount();
+        }
     }
 
     @Override

src/main/java/frc/team3128/commands/CmdShootDist.java

@@ -1,15 +1,24 @@
 package frc.team3128.commands;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.team3128.Robot;
 import frc.team3128.common.utility.Log;
 import frc.team3128.subsystems.Hood;
+import frc.team3128.subsystems.Hopper;
 import frc.team3128.subsystems.LimelightSubsystem;
+import frc.team3128.subsystems.NAR_Drivetrain;
 import frc.team3128.subsystems.Shooter;
+import static frc.team3128.Constants.SimConstants.*;
+import static frc.team3128.Constants.DriveConstants.*;
 
 public class CmdShootDist extends CommandBase {
     private Shooter shooter;
     private LimelightSubsystem limelights;
     private Hood hood;
+    private Hopper hopper;
 
     /**
      * Shoot through calculating the approximate distance to target via the limelight 
@@ -20,6 +29,7 @@ public CmdShootDist() {
         shooter = Shooter.getInstance();
         limelights = LimelightSubsystem.getInstance();
         hood = Hood.getInstance();
+        hopper = Hopper.getInstance();
 
         addRequirements(shooter, hood);
     }
@@ -33,6 +43,14 @@ public void initialize() {
     @Override
     public void execute() {
         double dist = limelights.calculateShooterDistance();
+        if (Robot.isSimulation()) {
+
+            Pose2d pose = NAR_Drivetrain.getInstance().getPose().relativeTo(HUB_POS);
+
+            dist = Units.metersToInches(Math.sqrt(pose.getX() * pose.getX() + pose.getY() * pose.getY())) - HUB_RADIUS; // inches
+
+            SmartDashboard.putNumber("Distance from hub (meters)", Units.inchesToMeters(dist));
+        }
         shooter.beginShoot(shooter.calculateRPMFromDist(dist));
         hood.startPID(hood.calculateAngleFromDist(dist));
     }
@@ -42,6 +60,10 @@ public void end(boolean interrupted) {
         shooter.stopShoot();
         limelights.turnShooterLEDOff();
         Log.info("CmdShootDist", "Cancelling shooting");
+
+        if (Robot.isSimulation()) {
+            hopper.resetBallCount();
+        }
     }
 
     @Override

src/main/java/frc/team3128/common/hardware/motor/NAR_Motor.java

@@ -0,0 +1,33 @@
+package frc.team3128.common.hardware.motor;
+
+public enum NAR_Motor {
+    PRO_775(18730, 0.7, 0.71, 134);
+
+    protected double freeSpeedRPM;
+    protected double freeCurrentAmps;
+    protected double stallCurrentAmps;
+    protected double stallTorqueNM;
+
+    NAR_Motor(double freeSpeedRPM, double freeCurrentAmps, double stallCurrentAmps, double stallTorqueNM) {
+        this.freeSpeedRPM = freeSpeedRPM;
+        this.freeCurrentAmps = freeCurrentAmps;
+        this.stallCurrentAmps = stallCurrentAmps;
+        this.stallTorqueNM = stallTorqueNM;
+    }
+
+    public double getFreeSpeedRPM() {
+        return freeSpeedRPM;
+    }
+
+    public double getFreeCurrentAmps() {
+        return freeCurrentAmps;
+    } 
+
+    public double getStallCurrentAmps() {
+        return stallCurrentAmps;
+    }
+
+    public double getStallTorqueNM() {
+        return stallTorqueNM;
+    }
+}

src/main/java/frc/team3128/common/hardware/motorcontroller/NAR_CANSparkMax.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
-
+import frc.team3128.common.hardware.motor.NAR_Motor;
 import net.thefletcher.revrobotics.CANSparkMax;
 import net.thefletcher.revrobotics.SparkMaxRelativeEncoder;
 import net.thefletcher.revrobotics.enums.MotorType;
@@ -16,14 +16,16 @@ public class NAR_CANSparkMax extends CANSparkMax {
 	private SparkMaxRelativeEncoder encoder;
 	private SimDeviceSim encoderSim;
 	private SimDouble encoderSimVel;
+	private NAR_Motor motor;
 
 	/**
 	 * 
 	 * @param deviceNumber device id
 	 * @param type         kBrushed(0) for brushed motor, kBrushless(1) for brushless motor
 	 */
-	public NAR_CANSparkMax(int deviceNumber, MotorType type) {
+	 public NAR_CANSparkMax(int deviceNumber, MotorType type, NAR_Motor motor) {
 		super(deviceNumber, type);
+		this.motor = motor;
 
 		restoreFactoryDefaults(); // Reset config parameters, unfollow other motor controllers
 
@@ -37,6 +39,11 @@ public NAR_CANSparkMax(int deviceNumber, MotorType type) {
 		}
 	}
 
+	public NAR_CANSparkMax(int deviceNumber, MotorType type) {
+		this(deviceNumber, type, null);
+	}
+
+
 	@Override
 	public void set(double outputValue) {
 		if (outputValue != prevValue) {
@@ -77,4 +84,11 @@ public void setSimVelocity(double vel) {
 	public REVLibError follow(CANSparkMax motor) {
 		return super.follow((CANSparkMax)motor);
 	}
+
+	// public void follow(NAR_EMotor motor) {
+	// 	if(!(motor instanceof CANSparkMax)) {
+	// 		throw new RuntimeException("Bad follow: NAR_CANSparkMax " + getDeviceId() + " attempted to follow non-CANSparkMax motor controller.");
+	// 	}
+	// 	super.follow((CANSparkMax)motor);
+	// }
 }