climb pivot + algae flywheel

src/main/java/frc/robot/commands/drive/XState.java → src/main/java/frc/robot/commands/ClimbPivot.java

@@ -2,32 +2,42 @@
 // 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.drive;
+package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 
-/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
-public class XState extends Command {
-  /** Creates a new XState. */
-  public XState() {
+public class ClimbPivot extends Command {
+  /** Creates a new ClimbPivot. */
+  private final ClimbPivot climbPivot;
+  private final double targetPosition = 0;
+
+  public ClimbPivot(CoralPivot climbPivot, double targetPosition) {
+    this.climbPivot = climbPivot;
+    this.targetPosition = targetPosition;
+    addRequirements(climbPivot);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    climbPivot.setPivotPosition(targetPosition);
+  }
 
   // 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) {}
-
+  public void end(boolean interrupted) {
+      climbPivot.stop();
+
+}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    double error = Math.abs(climbPivot.getCurrentPosition() - targetPosition);
+        return error < 1.0;
   }
 }

src/main/java/frc/robot/commands/drive/DealgaefierPivot.java

@@ -0,0 +1,45 @@
+// 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.drive;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.pivotforcoralorsomeshit.CoralPivot;
+
+public class CoralPivot extends Command {
+  /** Creates a new CoralPivot. */
+  private final CoralPivot coralPivot;
+  private final double targetPosition = 0;
+
+  public CoralPivot(CoralPivot coralPivot, double targetPosition) {
+    this.coralPivot = coralPivot;
+    this.targetPosition = targetPosition;
+    addRequirements(coralPivot);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+     coralPivot.setPivotPosition(targetPosition);
+  }
+
+  // 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) {
+      coralPivot.stop();
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    double error = Math.abs(coralPivot.getCurrentPosition() - targetPosition);
+        return error < 1.0;
+  }
+}

src/main/java/frc/robot/subsystems/flywheel/Flywheel.java

@@ -0,0 +1,42 @@
+// 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.subsystems.flywheel;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.flywheel.FlywheelConstants;
+
+public class FlywheelSubsystem extends SubsystemBase {
+  /** Creates a new FlywheelSubsystem. */
+   private FlywheelInterface flywheelInterface;
+
+  private FlywheelInputsAutoLogged inputs = new FlywheelInputsAutoLogged();
+
+  public Flywheel() {
+
+  }
+
+  public double getFlywheelSpeed() {
+    return flywheelInterface.getFlywheelSpeed();
+  }
+
+  public double getVolts() {
+    return flywheelInterface.getVolts();
+  }
+
+  public void setFlywheelSpeed(double speed) {
+    flywheelInterface.setFlywheelSpeed(speed);
+  }
+
+  public void setVolts(double volts) {
+    flywheelInterface.setVolts(volts);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    flywheelInterface.updateInputs(inputs);
+    Logger.processInputs("Flywheel/", inputs);
+  }
+}

src/main/java/frc/robot/subsystems/flywheel/FlywheelConstants.java

@@ -0,0 +1,16 @@
+// 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.subsystems.flywheel;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+/** Add your docs here. */
+public class FlywheelConstants {
+    public static final int FLYWHEEL_MOTOR_ID = 0-9;
+    public static final double FLYWHEEL_GEAR_RATIO = 0;
+    public static final double FLYWHEEL_P = 0;
+    public static final double FLYWHEEL_I = 0;
+    public static final double FLYWHEEL_D = 0;
+}

src/main/java/frc/robot/subsystems/flywheel/FlywheelInterface.java

@@ -0,0 +1,30 @@
+// 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.subsystems.flywheel;
+import org.littletonrobotics.junction.AutoLog;
+/** Add your docs here. */
+public class FlywheelInterface {
+    @AutoLog
+  public static class FlywheelInterface {
+    public double leaderMotorPosition = 0.0;
+  }
+
+
+  public default void updateInputs(FlywheelInterface inputs) {}
+
+
+  public default double getFlywheelSpeed() {
+    return 0.0;
+  }
+
+  public default void setFlywheelSpeed(double speed) {}
+
+  public default void setVolts(double volts) {}
+
+  public default double getVolts() {
+    return 0.0;
+  }
+
+}

src/main/java/frc/robot/subsystems/flywheel/PhysicalFlywheel.java

@@ -0,0 +1,8 @@
+// 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.subsystems.flywheel;
+
+/** Add your docs here. */
+public class PhysicalFlywheel {}

src/main/java/frc/robot/subsystems/flywheel/SimulatedFlywheel.java

@@ -0,0 +1,51 @@
+// 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.subsystems.flywheel;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+
+/** Add your docs here. */
+public class SimulatedFlywheel implements FlywheelInterface {
+    private SimulatedFlywheel simulatedflywheel =
+      new SimulatedFlywheel(
+          DCMotor.getFalcon500(2),
+          FlywheelConstants.FLYWHEEL_GEAR_RATIO,
+          true,
+          0.0);
+  private PIDController simPID;
+  private double currentVolts;
+
+  public SimulatedFlywheel(DCMotor dcMotor, double flywheelGearRatio, boolean b, double d) {
+    simPID =
+        new PIDController(
+            FlywheelConstants.FLYWHEEL_P,
+            FlywheelConstants.FLYWHEEL_I,
+            FlywheelConstants.FLYWHEEL_D);
+  }
+
+  public void updateInputs(FlywheelInputs inputs) {
+    inputs.leaderMotorPosition = getFlywheelPosition();
+    inputs.followerMotorPosition = getFlywheelPosition();
+  }
+
+  public void setFlywheelspeed(double speed) {
+    setVolts(simPID.calculate(getFlywheelSpeed(), speed));
+  }
+
+  public double getFlywheelSpeed() {
+    return flywheelSim.getAngularVelocityRPM(); 
+  }
+
+  public void setVolts(double volts) {
+    currentVolts = simPID.calculate(volts);
+    elevatorSim.setInputVoltage(currentVolts);
+  }
+
+  public double getVolts() {
+    return currentVolts;
+  }
+}

src/main/java/frc/robot/subsystems/pivot/ClimbPivot.java

@@ -0,0 +1,57 @@
+// 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.subsystems.pivot;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ClimbPivot extends SubsystemBase {
+  /** Creates a new ClimbPivot. */
+  public TalonFX climbPivotMotor = new TalonFX(Constants.CLIMB_PIVOT_MOTOR);
+  private static final int TICKS_PER_ROTATION = 2048;
+  private static final double GEAR_RATIO = 10.0;
+
+  public ClimbPivot() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+    config.slot0.kP = 0.0;
+    config.slot0.kI = 0.0;
+    config.slot0.kD = 0.0;
+    config.slot0.kF = 0.0;
+    config.motionCruiseVelocity = 15000;
+    climb.motionAcceleration = 6000;
+    climbPivotMotor.configAllSettings(config);
+    climbPivotMotor.setNeutralMode(NeutralMode.Brake);  
+    climbPivotMotor.setSelectedSensorPosition(0);
+  }
+
+  public void setPivotPosition(double degrees) {
+    double ticks = degreesToTicks(degrees);
+    climbPivotMotor.set(ControlMode.MotionMagic, ticks);
+}
+
+public double getCurrentPosition() {
+    return ticksToDegrees(climbPivotMotor.getSelectedSensorPosition());
+}
+
+public void stop() {
+    climbPivotMotor.set(ControlMode.PercentOutput, 0);
+}
+
+private double degreesToTicks(double degrees) {
+    return degrees / 360.0 * TICKS_PER_ROTATION * GEAR_RATIO;
+}
+
+private double ticksToDegrees(double ticks) {
+    return ticks / (TICKS_PER_ROTATION * GEAR_RATIO) * 360.0;
+}
+
+@Override
+public void periodic() {
+    System.out.println("Current Position: " + getCurrentPosition());
+}
+}

src/main/java/frc/robot/subsystems/pivot/DealgaefierPivot.java

@@ -0,0 +1,57 @@
+// 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.subsystems.pivot;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class CoralPivot extends SubsystemBase {
+  /** Creates a new Cpivot. */
+  public TalonFX dealgaefierPivotMotor = new TalonFX(Constants.CORAL_PIVOT_MOTOR);
+  private static final int TICKS_PER_ROTATION = 2048;
+  private static final double GEAR_RATIO = 10.0;
+
+  public CoralPivot() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+    config.slot0.kP = 0.0;
+    config.slot0.kI = 0.0;
+    config.slot0.kD = 0.0;
+    config.slot0.kF = 0.0;
+    config.motionCruiseVelocity = 15000;
+    config.motionAcceleration = 6000;
+    dealgaefierPivotMotor.configAllSettings(config);
+    dealgaefierPivotMotor.setNeutralMode(NeutralMode.Brake);  
+    dealgaefierPivotMotor.setSelectedSensorPosition(0);
+    }
+
+    public void setPivotPosition(double degrees) {
+        double ticks = degreesToTicks(degrees);
+        dealgaefierPivotMotor.set(ControlMode.MotionMagic, ticks);
+    }
+
+    public double getCurrentPosition() {
+        return ticksToDegrees(dealgaefierPivotMotor.getSelectedSensorPosition());
+    }
+
+    public void stop() {
+        dealgaefierPivotMotor.set(ControlMode.PercentOutput, 0);
+    }
+
+    private double degreesToTicks(double degrees) {
+        return degrees / 360.0 * TICKS_PER_ROTATION * GEAR_RATIO;
+    }
+
+    private double ticksToDegrees(double ticks) {
+        return ticks / (TICKS_PER_ROTATION * GEAR_RATIO) * 360.0;
+    }
+
+    @Override
+    public void periodic() {
+        System.out.println("Current Position: " + getCurrentPosition());
+    }
+}

src/main/java/frc/robot/subsystems/pivot/PivotConstants.java

@@ -0,0 +1,12 @@
+// 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.subsystems.pivot;
+
+/** Add your docs here. */
+public class PivotConstants {
+    public static final TalonFX CORAL_PIVOT_MOTOR = 0-9;
+    public static final TalonFX CLIMB_PIVOT_MOTOR = 0-9;
+    public static final TalonFX FLYWHEEL_MOTOR = 0-9;
+}