stuff

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 86104d20bf0f26e7b7827ac032d64247207ea9a4..55ec0ada72ae47118169c1098b7eb82ee287a137 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -24,6 +24,7 @@ import frc.robot.constants.VisionConstants;
 import frc.robot.controls.BaseDriverConfig;
 import frc.robot.controls.Operator;
 import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
 import frc.robot.util.PathGroupLoader;
@@ -48,6 +49,7 @@ public class RobotContainer {
   // Controllers are defined here
   private BaseDriverConfig driver = null;
   private Operator operator = null;
+  private LinearClimb linearClimb = null;
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -74,35 +76,37 @@ public class RobotContainer {
         // fall-through
 
       case Vivace:
+        linearClimb = new LinearClimb();
+        break;
 
       case Phil: // AKA "IHOP"
 
       case PrimeJr:
 
       case Vertigo: // AKA "French Toast"
-        drive = new Drivetrain(vision, new GyroIOPigeon2());
-        driver = new PS5ControllerDriverConfig(drive);
-        operator = new Operator(drive);
+        // drive = new Drivetrain(vision, new GyroIOPigeon2());
+        // driver = new PS5ControllerDriverConfig(drive);
+        // operator = new Operator(drive);
 
         // Detected objects need access to the drivetrain
-        DetectedObject.setDrive(drive);
+        // DetectedObject.setDrive(drive);
         
         // SignalLogger.start();
 
-        driver.configureControls();
-        operator.configureControls();
+        // driver.configureControls();
+        // operator.configureControls();
         
-        initializeAutoBuilder();
-        registerCommands();
-        PathGroupLoader.loadPathGroups();
-        // Load the auto command
-        try {
-          PathPlannerAuto.getPathGroupFromAutoFile("Command Name");
-          auto = new PathPlannerAuto("Path Name");
-        } catch (IOException | ParseException e) {
-          e.printStackTrace();
-        }
-        drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
+        // initializeAutoBuilder();
+        // registerCommands();
+        // PathGroupLoader.loadPathGroups();
+        // // Load the auto command
+        // try {
+        //   PathPlannerAuto.getPathGroupFromAutoFile("Command Name");
+        //   auto = new PathPlannerAuto("Path Name");
+        // } catch (IOException | ParseException e) {
+        //   e.printStackTrace();
+        // }
+        // drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
         break;
       }
 

diff --git a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
index 4b1cdaaf87d2a4b148f1f4d9fe7b2e5d568c415b..64a64fb9d16da5a17ac289a8430e6b7307779648 100644 (file)
--- a/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/Climb/ClimbConstants.java
@@ -3,11 +3,11 @@ package frc.robot.constants.Climb;
 public class ClimbConstants {
     // CHANGE LATER
 
-    public final static double CLIMB_GEAR_RATIO = 1 / 1;
+    public final static double CLIMB_GEAR_RATIO = 25 / 1;
     public final static double MAX_VELOCITY = 0.1;
     public final static double MAX_ACCELERATION = 1;
     public final static double MIN_HEIGHT = 0;
-    public final static double MAX_HEIGHT = 1;
-    public final static double RADIUS = 1;
-    public final static double CLIMB_HEIGHT = 0.5;
+    public final static double MAX_HEIGHT = 8;
+    public final static double RADIUS = 0.514098;
+    public final static double CLIMB_HEIGHT = 4;
 }

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index 1c87cb5195e2da817a17bccd977799190b3ec50d..335f77edee15acd7038b80d5ac870b7d39f753fe 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -20,5 +20,5 @@ public class IdConstants {
     public static final int CANDLE_ID = 1;
 
     // Climb
-    public static final int CLIMB_MOTOR_ID = 1; // change left and right  IDs later
+    public static final int CLIMB_MOTOR_ID = 7; // change left and right  IDs later
 }

diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 82eacd9daac60d508dcc12316a6625beb64b5434..6087cdc4024c3f0cc8223382b63730d66e8853b4 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -23,7 +23,7 @@ public class LinearClimb {
 
     private double rotationalSetpoint = 0;
 
-    private double kP = 0.1;
+    private double kP = 0.5;
     private double kI = 0.0;
     private double kD = 0.0;  
 
@@ -64,6 +64,8 @@ public class LinearClimb {
         SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
         SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown()));
         SmartDashboard.putData("Climb", new InstantCommand(() -> climb()));
+
+        motor.setPosition((ClimbConstants.MAX_HEIGHT/(2 * Math.PI * ClimbConstants.RADIUS)) * ClimbConstants.CLIMB_GEAR_RATIO);
     }
 
     public void setSetpoint(double setpoint) {