Emma elevator

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

@@ -16,7 +16,13 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final class Gripper {
-    }
+    public static final double DRUM_RADIUS= 0.03;//[meter]
+    public static final double TICKS_PER_METER= 2 * Math.PI * DRUM_RADIUS / 4096;//[ticks\second]
+    public static final int CONFIG_VOLT= 12;
+    public static final boolean ENABLE_VOLT_COMP= true;
+    public static final int MOTION_ACCELERATION=0;
+    public static final int CRUISE_VELOCITY=0;
+    public static final double DEAD_BEND=0.05;//[%]
+    public static final double ELEVATOR_THRESHOLD=0.3;//[meters]
 
 }

src/main/java/frc/robot/Ports.java

@@ -1,15 +1,10 @@
 package frc.robot;
 
+import com.ctre.phoenix.motorcontrol.InvertType;
+import com.ctre.phoenix.motorcontrol.TalonFXInvertType;
+
 public class Ports {
-    public static class Gripper {
-        public static final int LEFTMOTOR = 0;
-        public static final int RIGHTMOTOR = 0;
-        public static final int SOLENOID = 0;
+    public static final int MOTOR=0;
+    public static final TalonFXInvertType INVERTED = TalonFXInvertType.Clockwise;
 
-        public static final boolean RIGHT_MOTOR_INVERTED = false;
-        public static final boolean LEFT_MOTOR_INVERTED = false;
-    }
-    public static class Controller {
-        public static final int XBOX_CONTROLLER = 0;
-    }
 }

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

@@ -20,21 +20,14 @@
 import static frc.robot.Ports.Controller.*;
 
 public class RobotContainer {
-    private final XboxController Xbox = new XboxController(XBOX_CONTROLLER);
-    private final Trigger lt = new Trigger(() -> Xbox.getTriggerAxis(GenericHID.Hand.kLeft) > 0.3);
-    private final Trigger rt = new Trigger(() -> Xbox.getTriggerAxis(GenericHID.Hand.kRight) > 0.3);
-    private final JoystickButton lb = new JoystickButton(Xbox, XboxController.Button.kBumperLeft.value);
-    private final JoystickButton rb = new JoystickButton(Xbox, XboxController.Button.kBumperRight.value);
-    private final Gripper gripper = Gripper.getInstance();
 
 
     public RobotContainer() {
         configureButtonBindings();
     }
     //robot will intake when left trigger is held and outtake while right trigger is held
     private void configureButtonBindings() {
-        lt.whileActiveContinuous(new Intake(gripper, 1));
-        rt.whileActiveContinuous(new Outtake(gripper, -1));
+
     }
 
 

src/main/java/frc/robot/commands/MoveElevator.java

@@ -0,0 +1,35 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ExampleSubsystem.Elevator;
+
+import java.util.function.DoubleSupplier;
+
+public class MoveElevator extends CommandBase {
+    private final Elevator elevator;
+    private final DoubleSupplier joystickValue;
+
+    public MoveElevator(Elevator elevator, DoubleSupplier joystickValue) {
+        this.elevator = elevator;
+        this.joystickValue = joystickValue;
+        addRequirements(elevator);
+    }
+
+    /**
+     * sets the power of the elevator to the desired value with a joystick
+     */
+    @Override
+    public void execute() {
+        elevator.setPower(elevator.DeadZone(joystickValue.getAsDouble()));
+    }
+
+    /**
+     * stops the elevator
+     *
+     * @param interrupted
+     */
+    @Override
+    public void end(boolean interrupted) {
+        elevator.setPower(0);
+    }
+}

src/main/java/frc/robot/commands/PositionControl.java

@@ -0,0 +1,47 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ExampleSubsystem.Elevator;
+
+public class PositionControl extends CommandBase {
+    private final Elevator elevator;
+    private final double height;
+
+    public PositionControl(Elevator elevator, double height) {
+        this.elevator = elevator;
+        this.height = height;
+        addRequirements(elevator);
+    }
+
+    /**
+     * sets the position of the elevator
+     */
+    @Override
+    public void execute() {
+        elevator.setPosition(height);
+    }
+
+    /**
+     * stops the elevator
+     *
+     * @param interrupted
+     */
+    @Override
+    public void end(boolean interrupted) {
+        elevator.setPower(0);
+    }
+
+    /**
+     * checks if the elevator reached the desired height
+     *
+     * @return
+     */
+    @Override
+    public boolean isFinished() {
+        if (elevator.heightCheck(elevator.getPosition(), height)) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+}

src/main/java/frc/robot/subsystems/ExampleSubsystem/Elevator.java

@@ -0,0 +1,103 @@
+package frc.robot.subsystems.ExampleSubsystem;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.Ports;
+import frc.robot.subsystems.UnitModel;
+
+public class Elevator extends SubsystemBase {
+    private final WPI_TalonFX motor = new WPI_TalonFX(Ports.MOTOR);
+    private static Elevator INSTANCE = null;
+    public static final UnitModel unitModel = new UnitModel(Constants.TICKS_PER_METER);
+
+    public Elevator() {
+        motor.enableVoltageCompensation(Constants.ENABLE_VOLT_COMP);
+        motor.configVoltageCompSaturation(Constants.CONFIG_VOLT);
+        motor.configMotionAcceleration(Constants.MOTION_ACCELERATION);
+        motor.configMotionCruiseVelocity(Constants.CRUISE_VELOCITY);
+        motor.setInverted(Ports.INVERTED);
+        motor.setNeutralMode(NeutralMode.Brake);
+
+
+    }
+
+    /**
+     * creates an instance if there isn't one
+     *
+     * @return
+     */
+    public Elevator getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new Elevator();
+        }
+        return INSTANCE;
+    }
+
+    /**
+     * gets the power of the elevator
+     *
+     * @return
+     */
+    public double getPower() {
+        return motor.get();
+    }
+
+    /**
+     * sets the power of the elevator
+     *
+     * @param Power
+     */
+    public void setPower(double Power) {
+        motor.set(Power);
+    }
+
+    /**
+     * checks if the joystick is in the defined dead zone and stops the elevator from moving if it's in the dead zone
+     *
+     * @param value
+     * @return
+     */
+    public double DeadZone(double value) {
+        if (value >= Constants.DEAD_BEND || value <= Constants.DEAD_BEND) {
+            return 0;
+        } else {
+            return value;
+        }
+    }
+
+    /**
+     * sets the position of the elevator
+     *
+     * @param height
+     */
+    public void setPosition(double height) {
+        motor.set(ControlMode.Position, unitModel.toTicks(height));
+    }
+
+    /**
+     * ges the position of the elevator
+     *
+     * @return
+     */
+    public double getPosition() {
+        return unitModel.toUnits(motor.getSelectedSensorPosition());
+    }
+
+    /**
+     * checks if the elevator reached the desired position
+     *
+     * @param currentHeight
+     * @param desiredHeight
+     * @return
+     */
+    public boolean heightCheck(double currentHeight, double desiredHeight) {
+        if (currentHeight >= desiredHeight - 3 && currentHeight <= desiredHeight + 3) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+}