DRAFT: Climber encoders. Do not merge

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

@@ -45,6 +45,7 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    robotContainer.periodic();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

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

@@ -21,8 +21,6 @@
 
 import static frc.robot.settings.Constants.Ps4.*;
 
-import org.opencv.core.Point;
-
 import frc.robot.commands.PointAtCargo;
 import frc.robot.commands.climber.ArmPneumaticTipping;
 import frc.robot.commands.climber.AutomatedClimb;
@@ -32,6 +30,7 @@
 // import frc.robot.subsystems.LightsHardware;
 import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.Climber.RungLockState;
+import frc.robot.subsystems.HallEffectSensorSelection;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Pixy2SubSystem;
 import frc.robot.commands.climber.ClimberClimb;
@@ -45,6 +44,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
+
 /**
  * This class is where the bulk of the robot should be declared. Since
  * Command-based is a
@@ -123,19 +123,33 @@ public RobotContainer() {
   private void configureSmartDashboard() {
     SmartDashboard.putData("Test Vision", new PointAtCargo(drivetrain, vision));
     SmartDashboard.putData("drivetrain", drivetrain);
-    SmartDashboard.putData("Burn In", new BurnIn(drivetrain));
-    SmartDashboard.putData("forwardOneSecond", new ForwardDistance(drivetrain, 1, .25));
+    //SmartDashboard.putData("Burn In", new BurnIn(drivetrain));
+    SmartDashboard.putData("forwardOneSecond", new ForwardDistance(drivetrain, .5, .2));
+    //SmartDashboard.putData("forwardOneSecond2", new ForwardDistance(drivetrain, .5, .2));
 
     SmartDashboard.putData("climbUp", new ClimberClimb(climber, ArmExtendState.OUT));
     SmartDashboard.putData("climbDown", new ClimberClimb(climber, ArmExtendState.IN));
     //SmartDashboard.putData("armLock", new WedgePneumatic(climber, RungLockState.Locked));
     SmartDashboard.putData("tiltDown", new ArmPneumaticTipping(climber, ArmTipState.DOWN));
     SmartDashboard.putData("tiltUp", new  ArmPneumaticTipping(climber, ArmTipState.UP));
+
+    SmartDashboard.putData("ArmDown", new MoveArm(intake, IntakeArmState.armDown));
+    SmartDashboard.putData("ArmUp", new MoveArm(intake, IntakeArmState.armUp));
+
+    SmartDashboard.putData("ClimbTestingUp", new ClimberClimb(climber, ArmExtendState.OUT, true));
+    SmartDashboard.putData("ClimbTestingDown", new ClimberClimb(climber, ArmExtendState.IN,true));
+
   }
 
   public void initTelemetry() {
   }
 
+  void periodic() {
+    SmartDashboard.putBoolean("TopRight", climber.isHallEffectSensorClosed(HallEffectSensorSelection.TopRight));
+    SmartDashboard.putBoolean("TopLeft", climber.isHallEffectSensorClosed(HallEffectSensorSelection.TopLeft));
+    SmartDashboard.putBoolean("BottomRight", climber.isHallEffectSensorClosed(HallEffectSensorSelection.BottomRight));
+    SmartDashboard.putBoolean("BottomLeft", climber.isHallEffectSensorClosed(HallEffectSensorSelection.BottomLeft));
+  }
   /**
    * Use this method to define your button->command mappings. Buttons can be
    * created by

src/main/java/frc/robot/commands/climber/ClimberClimb.java

@@ -3,11 +3,13 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Climber;
 import static frc.robot.settings.Constants.Climber.CLIMBER_SPEED;
+import static frc.robot.settings.Constants.Climber.TESTING_CLIMBER_SPEED;
 
 public class ClimberClimb extends CommandBase {
 
   Climber climber;
   ArmExtendState state;
+  double currentSpeed;
 
   public enum ArmExtendState {
     IN,
@@ -26,6 +28,25 @@ public ClimberClimb(Climber climber, ArmExtendState armState) {
     addRequirements(climber);
 
     state = armState;
+    currentSpeed = CLIMBER_SPEED;
+  }
+
+  public ClimberClimb(Climber climber, ArmExtendState armState, boolean testingSpeed)
+  {
+    //new ClimberClimb(climber, armState);
+    this.climber = climber;
+    addRequirements(climber);
+
+    state = armState;
+
+    if (testingSpeed == true)
+    {
+      currentSpeed = TESTING_CLIMBER_SPEED;
+    }
+    else
+    {
+      currentSpeed = CLIMBER_SPEED;
+    }
   }
 
 
@@ -35,10 +56,10 @@ public void execute() {
     switch (state) {
 
       case OUT:
-        climber.climberOut(CLIMBER_SPEED);
+        climber.climberOut(currentSpeed);
         break;
       case IN:
-        climber.climberIn(CLIMBER_SPEED);
+        climber.climberIn(currentSpeed);
         break;
     }
   }

src/main/java/frc/robot/commands/climber/HallEfectArmEncoderReset.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.climber;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Climber;
+
+public class HallEfectArmEncoderReset extends CommandBase {
+  /** Creates a new HallEfectArmEncoderReset. */
+  Climber climber;
+  public HallEfectArmEncoderReset(Climber climber) {
+    this.climber = climber;
+  }
+
+  @Override
+  public void execute() {
+    climber.climberIn(.1);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    climber.stop();
+    climber.resetEncoders();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+
+
+
+
+    return climber.isClimberFullyIn();
+  }
+}

src/main/java/frc/robot/commands/drivetrain/GoForwardInInches.java

@@ -0,0 +1,35 @@
+package frc.robot.commands.drivetrain;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Drivetrain;
+
+
+public class GoForwardInInches extends CommandBase{
+    private Drivetrain drivetrain;
+    private double power;
+    private double ticksNeeded;
+    private double ticksTraveled;
+  public GoForwardInInches(Drivetrain drivetrain, double power, double inches){
+    addRequirements(drivetrain);
+    this.drivetrain = drivetrain;
+    this.power = power;
+    ticksNeeded = drivetrain.convertInchesToTicks(inches);
+  }
+
+    @Override
+    public void initialize() {
+      ticksTraveled = drivetrain.getLeftEncoderValue();
+      drivetrain.setDrive(ControlMode.PercentOutput, power, power);      
+    }
+    @Override
+    public void end(boolean interrupted) {
+        drivetrain.setDrive(ControlMode.PercentOutput, 0, 0);
+    }
+    @Override
+    public boolean isFinished() {
+      return drivetrain.getLeftEncoderValue() + ticksNeeded == ticksTraveled;
+    }
+}

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

@@ -37,6 +37,13 @@ private Climber() {
         public static final double TIME_FOR_ARM_TO_GO_UP = 2;
 
         public static final double CLIMBER_SPEED = .2;
+        public static final double TESTING_CLIMBER_SPEED = .1;
+
+        public static final double ENCODER_TICKS_TO_ARMS_LENGTH = 
+            (1/2048.0)*(1/23.73)*(1.25*Math.PI)*(1/24.0);
+        //nu> motor   >  spool  >    inches    > arm lengths
+
+        public static final double ENCODER_TICKS_TO_ARMS_LENGTH_DIVIDED_BY_ONE = ENCODER_TICKS_TO_ARMS_LENGTH / 1;
     }
 
     public final class Ps4 {
@@ -69,6 +76,10 @@ private Drivetrain() {
         public static final int RIGHT_LEAD_ID = 2;
         public static final int RIGHT_FOLLOW_ID = 4;
 
+        public static final double ENCODER_TICKS_TO_DISTANCE = 
+        (1/2048)*(8/1)*(2*Math.PI);
+        //Units, ChainRatio, WheelSize
+        //1/2048, 8:1, 2pi
     }
 
     /**

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

@@ -9,12 +9,16 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
+import org.opencv.objdetect.CascadeClassifier;
+
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import static frc.robot.settings.Constants.Climber.*;
 
+import javax.xml.crypto.dsig.keyinfo.RetrievalMethod;
+
 public class Climber extends SubsystemBase {
 
     public enum RungLockState {
@@ -55,6 +59,9 @@ public Climber() {
 
         rightWinchMotor.setNeutralMode(NeutralMode.Brake);
         leftWinchMotor.setNeutralMode(NeutralMode.Brake);
+
+        rightWinchMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_ARMS_LENGTH_DIVIDED_BY_ONE);
+        leftWinchMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_ARMS_LENGTH_DIVIDED_BY_ONE);
         //negative percent output values bring climber in, positive bring it out.
 
         toggleLock();
@@ -151,6 +158,31 @@ public boolean isClimberFullyOut()
         // if not open(closed)
         return leftWinchMotor.isFwdLimitSwitchClosed() != 0 && rightWinchMotor.isFwdLimitSwitchClosed() != 0;
     }
+
+    public boolean isHallEffectSensorClosed(HallEffectSensorSelection sensor)
+    {
+        switch (sensor) {
+            case TopRight:
+                return rightWinchMotor.isFwdLimitSwitchClosed() != 0;
+            case BottomRight:
+                return rightWinchMotor.isRevLimitSwitchClosed() != 0;
+            case TopLeft:
+                return leftWinchMotor.isFwdLimitSwitchClosed() != 0;
+            case BottomLeft:
+                return leftWinchMotor.isRevLimitSwitchClosed() != 0;
+        }
+        return true;
+    }
+
+    public double getLeftArmPos()
+    {
+        return leftWinchMotor.getSelectedSensorPosition() * ENCODER_TICKS_TO_ARMS_LENGTH;
+    }
+//1.65 1.25 - 26.5 in
+    public double getRightArmPos()
+    {
+        return rightWinchMotor.getSelectedSensorPosition() * ENCODER_TICKS_TO_ARMS_LENGTH;
+    }
     /**
      * use motors to move the climber into extended position
      * 
@@ -170,9 +202,15 @@ public void climberIn(double speed) {
         setMotorSpeed(leftSpeed, rightSpeed);
     }
 
-public boolean isClimberFullyIn(){
+    public boolean isClimberFullyIn(){
     return leftWinchMotor.isRevLimitSwitchClosed() != 0 && rightWinchMotor.isRevLimitSwitchClosed() != 0;
-}
+    }
+
+    public void resetEncoders(){
+        leftWinchMotor.setSelectedSensorPosition(0);
+        rightWinchMotor.setSelectedSensorPosition(0);
+
+    }
 
     public void stop() {
         setMotorSpeed(0, 0);

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -27,6 +27,11 @@ public Drivetrain() {
         leftMotors = new MotorControllerGroup(leftLeadMotor, leftFollowMotor);
         rightMotors = new MotorControllerGroup(rightLeadMotor, rightFollowMotor);
 
+        leftLeadMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE);
+        rightLeadMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE);
+        leftFollowMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE);
+        leftFollowMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE);
+
         leftFollowMotor.follow(leftLeadMotor);
         rightFollowMotor.follow(rightLeadMotor);
 
@@ -68,6 +73,7 @@ public void setDrive(double leftSpeed, double rightSpeed) {
      * Controls motors with control mode. (Left, Right)
      * Also, never gonna give you up, never gonna let you down, never going to annoy
      * Liam by typing this comment. :)
+     * Ha! I put more stuff here --Ben
      */
     public void setDrive(ControlMode mode, double leftSpeed, double rightSpeed) {
         setDriveLeft(mode, leftSpeed);
@@ -96,4 +102,23 @@ public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInP
     public void periodic() {
         bbDriveSystem.feed();
     }
+
+    public void resetEncoders() {
+        leftLeadMotor.setSelectedSensorPosition(0);
+        rightLeadMotor.setSelectedSensorPosition(0);
+        leftFollowMotor.setSelectedSensorPosition(0);
+        rightFollowMotor.setSelectedSensorPosition(0);
+    }
+
+    public double getLeftEncoderValue(){
+        return leftLeadMotor.getSelectedSensorPosition();
+    }
+
+    public double getRightEncoderValue(){
+        return rightLeadMotor.getSelectedSensorPosition();
+    }
+
+    public double convertInchesToTicks(double inches){
+        return inches * ENCODER_TICKS_TO_DISTANCE;
+    }
 }

src/main/java/frc/robot/subsystems/HallEffectSensorSelection.java

@@ -0,0 +1,10 @@
+package frc.robot.subsystems;
+
+
+public enum HallEffectSensorSelection
+{
+    TopRight,
+    TopLeft,
+    BottomRight,
+    BottomLeft
+}