Climber jeremy

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

@@ -10,8 +10,10 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Util.Constants.ClimberConstants.ClimberModes;
 
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
@@ -62,6 +64,7 @@ public void disabledExit() {
 
   @Override
   public void autonomousInit() {
+    m_robotContainer.align_climber();
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {
@@ -79,6 +82,7 @@ public void autonomousExit() {
 
   @Override
   public void teleopInit() {
+    m_robotContainer.align_climber();
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

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

@@ -18,8 +18,9 @@
 
 public class RobotContainer {
 
-  private final Systems systems = new Systems();
-  private final Intake intake = systems.getIntake();
+  private Systems systems = new Systems();
+  private Intake intake = systems.getIntake();
+  private Climber climber = systems.getClimber();
 
   private TitanController driver = new TitanController(ControllerConstants.driverPort, ControllerConstants.deadzone);
   private TitanController operator = new TitanController(ControllerConstants.operatorPort, ControllerConstants.deadzone);
@@ -34,18 +35,22 @@ public class RobotContainer {
   private Trigger intakeCoral = driver.a();
 
 
+  // Climber Controls
+  private Trigger climberAim = driver.b();
+
   public RobotContainer() {
 
     configureBindings();
   }
 
   public void periodicSubsystems() {
     intake.periodic();
-
+    climber.periodic();
   }
 
   public void periodic() {
     SmartDashboard.putData("Scheduler", CommandScheduler.getInstance());
+    climber.periodic();
 
   }
 
@@ -57,11 +62,20 @@ private void configureOperatorControls() {
 
     // Intake Controls
     intakeCoral.whileTrue(intake.runIntakeCommand(IntakeModes.INTAKE));
+    climberAim.onTrue(climber.runClimberCommand(ClimberModes.ALIGN));
+
+  }
 
+  public void configureBindings() {
+    configureOperatorControls();
   }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
+
+  public void align_climber(){
+    climber.alignClimber();
+  }
 
 }

src/main/java/frc/robot/Subsytems/Climber/Climber.java

@@ -20,7 +20,7 @@ public class Climber extends REVMechanism {
 
     private ClimberConfig config;
     private SparkMax motor;
-    public boolean attachted;
+    public boolean attached;
     public SysIdRoutine routine;
 
     private ClimberModes mode;
@@ -42,8 +42,8 @@ public ClimberConfig() {
         }
     }
 
-    public Climber(SparkMax motor, boolean attachted) {
-        super(motor, attachted);
+    public Climber(SparkMax motor, boolean attached) {
+        super(motor, attached);
         ClimberConfig config = new ClimberConfig();
         this.motor = motor;
         this.mode = ClimberModes.STOW;
@@ -64,7 +64,7 @@ protected Config setConfig() {
         if (attached) {
             config.applySparkConfig(motor);
         }
-        return this.config;
+        return config;
     }
     @Override
     public void periodic() {
@@ -112,4 +112,7 @@ public Command runClimberCommand(ClimberModes climberModes) {
         return run(() -> runEnum(climberModes)).withName("Climber.runEnum");
     }
 
+    public void alignClimber(){
+        runEnum(ClimberModes.ALIGN);
+    }
 }

src/main/java/frc/robot/Subsytems/Manipulator/Manipulator.java

@@ -15,7 +15,7 @@
 public class Manipulator extends REVMechanism {
 	private ManipulatorConfig config;
 	private SparkMax motor;
-	public Boolean attachted;
+	public boolean attached;
 
 	private ManipulatorModes mode;
 	private ManipulatorStates state;
@@ -57,7 +57,7 @@ public Manipulator(SparkMax motor, boolean attached) {
 
 	@Override
 	protected Config setConfig() {
-		if (attachted) {
+		if (attached) {
 			config.applySparkConfig(motor);
 		}
 		return this.config;

src/main/java/frc/robot/Systems.java

@@ -7,6 +7,7 @@
 import frc.robot.Subsytems.Elevator.Elevator;
 import frc.robot.Subsytems.Intake.Intake;
 import frc.robot.Subsytems.Manipulator.Manipulator;
+import frc.robot.Subsytems.Climber.Climber;
 import frc.robot.Util.Constants;
 import frc.robot.Util.Constants.*;
 
@@ -15,6 +16,7 @@ public class Systems {
     private Intake intake;
     private Elevator elevator;
     private Manipulator manipulator;
+    private Climber climber;
 
 
     /* Kraken X60s */
@@ -23,6 +25,7 @@ public class Systems {
 
     /* Neo 1.1s */
     private SparkMax intakeMotor;
+    private SparkMax climberMotor;
 
     /* Neo 550s */
     private SparkMax manipulatorMotor;
@@ -36,12 +39,14 @@ public Systems() {
 
         /* Neo 1.1s */
         intakeMotor = new SparkMax(IntakeConstants.id, MotorType.kBrushless);
+        climberMotor = new SparkMax(ClimberConstants.id, MotorType.kBrushless);
 
         /* Neo 550s */
         manipulatorMotor = new SparkMax(ManipulatorConstants.id, MotorType.kBrushless);
 
         /*----------*/
         intake = new Intake(intakeMotor, IntakeConstants.attached);
+        climber = new Climber(climberMotor, ClimberConstants.attached);
         elevator = new Elevator(elevatorLeft, elevatorRight, ElevatorConstants.attached);
         manipulator = new Manipulator(manipulatorMotor, ManipulatorConstants.attached);   
 
@@ -53,6 +58,10 @@ public Intake getIntake() {
         return intake;
     }
 
+    public Climber getClimber() {
+        return climber;
+    }
+
     public Elevator getElevator() {
         return elevator;
     }

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

@@ -239,9 +239,12 @@ public enum ElevatorStates {
         public static final boolean breakType = true;
         public static final boolean useFMaxRotation = true;
         public static final boolean useRMaxRotation = true;
+        public static final boolean canRangeAttached = true;
 
-        public static final int leftId = 3;
-        public static final int rightId = 4;
+        public static final int leftId = 14;
+        public static final int rightId = 15;
+        public static final int canCoderId = 00;
+        public static final int canRangeId = 16;
         public static final double gearRatio = 1 / 1;
         public static final Current forwardCurrentLimit = Units.Amps.of(0);
         public static final Current reverseCurrentLimit = Units.Amps.of(0);
@@ -409,57 +412,4 @@ public enum ClimberModes {
         }
 
     }
-
-
-    public static class ClimberConstants {
-
-        public enum ClimberStates {
-            STOW,
-            ALIGN,
-            CLIMB
-        }
-
-        public static final boolean attached = true;
-        public static final boolean isInverted = false;
-        public static final int id = 2;
-        public static final double gearRatio = 1 / 1;
-        public static final Current supplyLimit = Units.Amps.of(0);
-        public static final Current stallLimit = Units.Amps.of(0);
-        public static final Angle offset = Units.Rotation.of(0);
-        public static final double maxForwardOutput = 0;
-        public static final double maxReverseOutput = 0;
-
-        public static final IdleMode idleMode = IdleMode.kCoast;
-        public static final FeedbackSensor sensorType = FeedbackSensor.kPrimaryEncoder;
-        public static final MAXMotionPositionMode mm_positionMode = MAXMotionPositionMode.kMAXMotionTrapezoidal;
-
-        public static final double p = 0;
-        public static final double i = 0;
-        public static final double d = 0;
-        public static final double maxIAccum = 0;
-
-        public static final Angle stow_angle = Units.Rotation.of(0);
-        public static final Angle align_angle = Units.Rotation.of(0);
-        public static final Angle climb_angle = Units.Rotation.of(0);
-        public static final Angle error = Units.Rotation.of(0);
-
-        public static final AngularVelocity mm_maxAccel = Units.RPM.of(0);
-        public static final AngularVelocity mm_velocity = Units.RPM.of(0);
-        public static final AngularVelocity mm_error = Units.RPM.of(0);
-
-        public enum ClimberModes {
-            STOW(stow_angle),
-            ALIGN(align_angle),
-            CLIMB(climb_angle);
-
-            public Angle angle;
-
-            ClimberModes(Angle angle) {
-                this.angle = angle;
-            }
-
-        }
-
-    }
-
 }

src/main/java/frc/robot/Util/RobotConfig.java

@@ -0,0 +1,23 @@
+package frc.robot.Util;
+
+public final class RobotConfig {
+
+    public static class RobotPropertiesConfig{
+        public double mass = 68;
+        public double moi = 15;
+    }
+
+    public static class BumberConfig{
+        public double bumpWidth = 0.2;
+        public double bumpLength = 1.5;
+        public double xOffset = 1;
+        public double yOffset = 1;
+    }
+
+    public static class ModuleConfig{
+        public double wheelRadius = 0.25;
+        public double driveGearing = 6.12;
+        public double maxDriveSpeed = 5;
+    }
+
+}