new pot algorithm and changed constants maybe idk pls send nayan or a…

…idan help
Team100 · Mar 28, 2022 · 786415c · 786415c
1 parent ea781f1
commit 786415c
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -330,8 +330,8 @@ public static final class Homing{
             }
             public static final class Tilt{
                 public static final int ID = 2;
-                public static final double POT_ADJUSTMENT_FACTOR = 100;
-                public static final double POT_OFFSET = 0;
+                public static final double POT_ADJUSTMENT_FACTOR = 1;//105;
+                public static final double POT_OFFSET = 0;//-95;
             }
         }
         public static final class ClimberMotionParameters {
@@ -341,11 +341,11 @@ public static final class ClimberMotionParameters {
 
             public static final double TILT_PERCENT_OUTPUT = 0.7;
 
-            public static final int CLIMBER_TOP = -243000;
+            public static final int CLIMBER_TOP = -225000; //-243000;
             public static final int CLIMBER_LOW_BAR_TOP = -110000;
             public static final int CLIMBER_BOTTOM = 5000;
             public static final double TILT_START = 7000;
-            public static final double STATIONARY_LOCK_ANGLE = 22;
+            public static final double STATIONARY_LOCK_ANGLE = 15; //22;
 
             public static final double EXTEND_START_ANGLE = 0;
             public static final double NEXT_BAR_ANGLE = -5;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -66,7 +67,6 @@ public class RobotContainer {
     // private final JoystickButton hookDownButton = new JoystickButton(buttonBoard, 1);
     private final JoystickButton lockStationariesButton = new JoystickButton(buttonBoard, 1);
     private final JoystickButton nextBarButton = new JoystickButton(buttonBoard, 16);
-
     private final JoystickButton hookDownButton = new JoystickButton(rightJoystick, 9);
     //private final JoystickButton hookUpButton = new JoystickButton(rightJoystick, 11);
 
@@ -76,6 +76,7 @@ public class RobotContainer {
     private final JoystickButton stopAll = new JoystickButton(buttonBoard, 4);
 
     private final JoystickButton hookZeroButton = new JoystickButton(rightJoystick, 7);
+
     private final JoystickButton indexTwoButton = new JoystickButton(buttonBoard, gitforcepushorginmaster);
 
     private final JoystickButton climberControlButton = new JoystickButton(leftJoystick, 3);
@@ -148,7 +149,6 @@ private void configureButtonBindings() {
         ejectButton.whileHeld(new ParallelCommandGroup(intakeEjectCommand, indexerEjectCommand, shootEjectCommand));
         hookZeroButton.whileHeld(hookZeroCommand);
 
-
         // hookDownButton.whenPressed(hookDownCommand);
         // hookUpButton.whenPressed(hookUpCommand);
         // hookUpLowButton.whenPressed(new HookUpLow(climber));

diff --git a/src/main/java/frc/robot/commands/climber/HookZero.java b/src/main/java/frc/robot/commands/climber/HookZero.java
@@ -28,13 +28,15 @@ public void initialize() {
   @Override
   public void execute() {
     System.out.println("zeroing");
-      climber.setWinch(Constants.ClimberConstants.ClimberMotionParameters.HOOK_ZERO_SPEED);
+      climber.setWinch(-Constants.ClimberConstants.ClimberMotionParameters.HOOK_ZERO_SPEED);
       climber.zeroWinch();
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    climber.zeroWinch();
+
     climber.setWinch(0);
   }
 

diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java
@@ -110,7 +110,9 @@ public double tiltAngle(){
         // return 90-((tilt.getSelectedSensorPosition()/2048)+45);
         // return 45-(tilt.getSelectedSensorPosition()/2048/2000*360);
         //return tilt.getSelectedSensorPosition()/2048/2000*360;//previously used one
-        return pot.get();
+        //return pot.get();
+        double p = pot.get();
+        return ((p - 0.082) * -345) +256 - 9.3;
     }
 
     @Override
@@ -119,11 +121,15 @@ public void periodic() {
         SmartDashboard.putNumber("TiltAngle", tiltAngle());
         SmartDashboard.putNumber("Legacy TiltAngle", legacyTiltAngle());
 
+        //SmartDashboard.putNumber("Old value testing", ((tiltAngle() - 0.082) * -340) +256 - 10.8);
+
         SmartDashboard.putBoolean("mainLocked", mainLocked());
         SmartDashboard.putBoolean("stationaryLocked", stationaryLocked());
 
         SmartDashboard.putBoolean("sensorLeft", getMainSensorLeft());
         SmartDashboard.putBoolean("sensorRight", getMainSensorRight());
         SmartDashboard.putBoolean("allingment switch", getHomeSwitch());
+
+        SmartDashboard.putNumber("Winch", winch.getSelectedSensorPosition());
     }
 }