Merge branch 'dev-2' into feat/intake2

src/main/java/frc/Subsystems/ElevatorSubsystem.java

@@ -0,0 +1,144 @@
+// 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.Subsystems;
+
+
+
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import frc.robot.Constants;
+import frc.robot.Constants.Elevator;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public class ElevatorSubsystem extends SubsystemBase {
+ /** Creates a new ExampleSubsystem. */
+ private TalonFX left_motor;
+ /** leader */
+ private TalonFX right_motor;
+ private PIDController pidController;
+ private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
+
+
+ private double targetHeight;
+ private double motorPower;
+  public ElevatorSubsystem() {
+   left_motor = new TalonFX(14);
+   right_motor = new TalonFX(15);
+   right_motor.configFactoryDefault();
+   left_motor.configFactoryDefault();
+    right_motor.clearStickyFaults();
+   left_motor.clearStickyFaults();
+
+
+   right_motor.configForwardSoftLimitThreshold(
+       0, 0); // this is the bottom limit, we stop AT the bottom
+   // right_motor.configReverseSoftLimitThreshold(
+   //     -heightToTicks(24), 0); // this is the top limit, we stop at the very top
+   right_motor.configForwardSoftLimitEnable(true, 0);
+   right_motor.configReverseSoftLimitEnable(true, 0);
+
+
+   right_motor.configOpenloopRamp(.5);
+
+
+   left_motor.setSelectedSensorPosition(0);
+   right_motor.setSelectedSensorPosition(0);
+
+
+   // make sure we hold our height when we get disabled
+   right_motor.setNeutralMode(NeutralMode.Coast);
+   left_motor.setNeutralMode(NeutralMode.Coast);
+
+
+   right_motor.follow(left_motor);
+
+   targetHeight = 0;
+
+
+   motorPower = 0;
+ pidController = new PIDController(0.4,0, 0.01);
+ //pidController.setTolerance(0.7,0.001);
+   ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
+   ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
+   ElevatorTab.add(pidController);
+
+
+   ElevatorTab.addNumber("Left Motor Speed", left_motor::getSelectedSensorVelocity);
+   ElevatorTab.addNumber("Right Motor Speed", right_motor::getSelectedSensorVelocity);
+   ElevatorTab.addNumber("Position Error", pidController::getPositionError);
+   ElevatorTab.addBoolean("If is at target Height", this::nearTargetHeight);
+// hi nora - sincerely, evelyn =D
+   // ElevatorTab.addNumber("height", () -> this.currentHeight);
+   // ElevatorTab.addNumber("target height", () -> this.targetHeight);
+   // ElevatorTab.addNumber("right motor sensor value", this::getHeight);
+
+
+
+
+
+
+
+
+
+
+ }
+ public void setMotorPower(double x){
+   System.out.println("hello");
+  }
+     public static double inchesToTicks(double height) {
+     return height * ((Elevator.GEAR_RATIO * Elevator.TICKS_PER_REVOLUTION) / (Elevator.GEAR_CIRCUMFERENCE));
+   }
+   public static double ticksToInches(double ticks) {
+     return (ticks * Elevator.GEAR_CIRCUMFERENCE) / (Elevator.TICKS_PER_REVOLUTION * Elevator.GEAR_RATIO);
+   }
+
+
+   public void setTargetHeight(double targetHeight) {    
+     this.targetHeight = targetHeight;                                  
+     pidController.setSetpoint(this.targetHeight); } 
+
+   public double getCurrentHeight(){
+
+
+     return ticksToInches(-left_motor.getSelectedSensorPosition());
+   }
+public boolean nearTargetHeight(){
+   if(targetHeight-0.5<=getCurrentHeight() && getCurrentHeight()<=targetHeight+0.5)return true;
+   return false;
+ }
+ @Override
+ public void periodic() {
+   // This method will be called once per scheduler run
+   motorPower = pidController.calculate(getCurrentHeight());
+   if (!pidController.atSetpoint()){
+     if (getCurrentHeight()<5){
+       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower+0.02,-0.2,0.2)));
+     }
+     else{
+       left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower+0.02,-0.5,0.5)));
+     }
+
+   }
+
+   // left_motor.set(TalonFXControlMode.PercentOutput, -(0.1));
+ }
+
+
+ @Override
+ public void simulationPeriodic() {
+   // This method will be called once per scheduler run during simulation
+ }
+ }

src/main/java/frc/robot/Commands/ElevatorBaseCommand

@@ -0,0 +1,59 @@
+// 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;
+
+
+import frc.robot.subsystems.ElevatorSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+
+/** An example command that uses an example subsystem. */
+public class ElevatorBaseCommand extends CommandBase {
+ @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ private final ElevatorSubsystem elevatorSubsystem;
+public double targetHight;
+
+
+ /**
+  * Creates a new ExampleCommand.
+  *
+  * @param subsystem The subsystem used by this command.
+  */
+ public ElevatorBaseCommand(ElevatorSubsystem elevatorSubsystem, double targetHight) {
+   this.targetHight = targetHight;
+   this.elevatorSubsystem = elevatorSubsystem;
+   // Use addRequirements() here to declare subsystem dependencies.
+   addRequirements(elevatorSubsystem);
+ }
+
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+   elevatorSubsystem.setTargetHeight(targetHight);
+
+
+
+
+ }
+
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {}
+
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {}
+
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+   return elevatorSubsystem.nearTargetHeight();
+ }
+}

src/main/java/frc/robot/Commands/ManualArmCommand

@@ -0,0 +1,63 @@
+// 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;
+
+
+import frc.robot.subsystems.ElevatorSubsystem;
+
+
+import java.util.function.DoubleSupplier;
+
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+
+/** An example command that uses an example subsystem. */
+public class ManualArmCommand extends CommandBase {
+ @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+ private final ElevatorSubsystem elevatorSubsystem;
+private DoubleSupplier speedSupplier;
+ /**
+  * Creates a new ExampleCommand.
+  *
+  * @param subsystem The subsystem used by this command.
+  */
+ public ManualArmCommand(ElevatorSubsystem elevatorSubsystem, DoubleSupplier speedSupplier) {
+   this.elevatorSubsystem = elevatorSubsystem;
+   this.speedSupplier = speedSupplier;
+   // Use addRequirements() here to declare subsystem dependencies.
+   addRequirements(elevatorSubsystem);
+ }
+
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {}
+
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+double speed =speedSupplier.getAsDouble();
+elevatorSubsystem.setMotorPower(speed);
+
+
+
+
+ }
+
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {}
+
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+   return false;
+ }
+}

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

@@ -2,22 +2,36 @@
 // 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;
 
+
 @SuppressWarnings("java:S1118")
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be declared
- * globally (i.e. public static). Do not put anything functional in this class.
- *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
- * constants are needed, to reduce verbosity.
- */
+* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+* constants. This class should not be used for any other purpose. All constants should be declared
+* globally (i.e. public static). Do not put anything functional in this class.
+*
+* <p>It is advised to statically import this class (or one of its inner classes) wherever the
+* constants are needed, to reduce verbosity.
+*/
 public final class Constants {
 
     public static class Intake{
 
         public static final int INTAKE_MOTOR_PORT = 18;
     }
 
+   public static class OperatorConstants {
+       public static final int kDriverControllerPort = 0;
+     }
+     public static class Elevator{
+       /**the gear ratio of the motor to the final gear revolutions */
+       public static final double GEAR_RATIO = 0.1008;
+       /**The number of ticks per motor revolution */
+       public static final double TICKS_PER_REVOLUTION = 2048;
+       /**The gear circumferance for distance */
+       public static final double GEAR_CIRCUMFERENCE = 1.432*Math.PI;
+
 }
+}