alpha robot

src/main/java/frc/robot/subsystems/kicker/KickerIO.java

@@ -0,0 +1,27 @@
+// 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.kicker;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/** Add your docs here. */
+public interface KickerIO {
+  @AutoLog
+  public class KickerIOInputs {
+
+    // Motor values
+    public double velocityRPM;
+    public double currentDrawAmps;
+    public double temperatureCelsius;
+    public double motorOutputVolts;
+    public double positionRotations;
+  }
+
+  public abstract void setPosition(double degrees);
+
+  public abstract void reset(double degrees);
+
+  public abstract KickerIOInputsAutoLogged updateInputs();
+}

src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java

@@ -0,0 +1,46 @@
+package frc.robot.subsystems.kicker;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+
+public class KickerIOReal {
+
+    private TalonFX kickerMotor = new TalonFX(Constants.KICKER_MOTOR_ID);
+    private PositionVoltage motorRequest = new PositionVoltage(Constants.KICKER_MOTOR_ID);
+
+    private StatusSignal<Double> supplyVoltageSignal = kickerMotor.getDutyCycle();
+    private StatusSignal<Double> position = kickerMotor.getRotorPosition();
+    private StatusSignal<Double> velocity = kickerMotor.getRotorVelocity();
+    private StatusSignal<Double> currentDraw = kickerMotor.getStatorCurrent();
+
+    public KickerIOReal (){
+        TalonFXConfiguration kickerConfig  = new TalonFXConfiguration();
+        kickerConfig.Slot0.kP = 1;
+        kickerConfig.Slot0.kD = 0.01;
+        kickerConfig.Slot0.kI = 0;
+    }
+
+    public void setPosition(double degrees) {
+        kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees)*Constants.KICKER_GEAR_RATIO));    
+    }
+
+    public void reset(double degrees){ 
+        kickerMotor.setPosition((Units.degreesToRotations(degrees))*Constants.KICKER_GEAR_RATIO);
+    }
+
+    public KickerIOInputsAutoLogged updateInputs() {
+        KickerIOInputsAutoLogged current = new KickerIOInputsAutoLogged();
+
+        current.currentDrawAmps = currentDraw.refresh().getValue();
+        current.positionRotations = position.refresh().getValue() / Constants.KICKER_GEAR_RATIO;
+        current.velocityRPM = velocity.refresh().getValue() / Constants.KICKER_GEAR_RATIO;
+        current.motorOutputVolts = 12 * supplyVoltageSignal.getValue();
+
+        return(current);
+    }
+}

src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems.kicker;
+
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public class KickerSubsystem extends SubsystemBase {
+    KickerIO io;
+    KickerIOInputsAutoLogged inputs;
+
+
+    public KickerSubsystem(KickerIO io) {
+        this.io = io;
+        inputs =  new KickerIOInputsAutoLogged();
+
+        SmartDashboard.putData("reset Kicker value", new InstantCommand(
+            ()->{io.reset(0);}
+            ));
+    }
+
+    public InstantCommand reset (){
+       return new InstantCommand(
+            ()->{io.reset(0);}
+            );
+    }
+
+    public RunCommand run(double degrees) {
+        return new RunCommand(() -> {
+            io.setPosition(degrees);
+        }, this);
+    }   
+
+    @Override
+    public void periodic() {
+        inputs = io.updateInputs();
+        Logger.processInputs("Kicker", inputs);
+    }
+}

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

@@ -0,0 +1,27 @@
+// 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 org.littletonrobotics.junction.AutoLog;
+
+/** Add your docs here. */
+public interface PivotIO {
+  @AutoLog
+  public class PivotIOInputs {
+
+    // Motor values
+    public double velocityRPM;
+    public double currentDrawAmps;
+    public double temperatureCelsius;
+    public double motorOutputVolts;
+    public double positionRotations;
+  }
+
+  public abstract void setPosition(double degrees);
+
+  public abstract void reset(double degrees);
+
+  public abstract PivotIOInputsAutoLogged updateInputs();
+}

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

@@ -0,0 +1,57 @@
+package frc.robot.subsystems.pivot;
+import com.ctre.phoenix6.StatusSignal;
+
+// 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.
+
+
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
+
+public class PivotIOReal implements PivotIO{
+
+    private TalonFX pivotMotor = new TalonFX(Constants.PIVOT_MOTOR_ID);
+    private PositionVoltage motorRequest = new PositionVoltage(Constants.PIVOT_MOTOR_ID);
+
+    private StatusSignal<Double> supplyVoltageSignal = pivotMotor.getDutyCycle();
+    private StatusSignal<Double> position = pivotMotor.getRotorPosition();
+    private StatusSignal<Double> velocity = pivotMotor.getRotorVelocity();
+    private StatusSignal<Double> currentDraw = pivotMotor.getStatorCurrent();
+
+
+    public PivotIOReal (){
+        TalonFXConfiguration pivotConfig  = new TalonFXConfiguration();
+        pivotConfig.Slot0.kP = 1;
+        pivotConfig.Slot0.kD = 0.01;
+        pivotConfig.Slot0.kI = 0;
+    }
+
+    @Override
+    public void setPosition(double degrees) {
+        pivotMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees)*Constants.PIVOT_GEAR_RATIO));    
+    }
+
+    @Override
+    public void reset(double degrees){ 
+        pivotMotor.setPosition((Units.degreesToRotations(degrees))*Constants.PIVOT_GEAR_RATIO);
+    }
+
+
+    @Override
+    public PivotIOInputsAutoLogged updateInputs() {
+        PivotIOInputsAutoLogged current = new PivotIOInputsAutoLogged();
+
+        current.currentDrawAmps = currentDraw.refresh().getValue();
+        current.positionRotations = position.refresh().getValue() / Constants.PIVOT_GEAR_RATIO;
+        current.velocityRPM = velocity.refresh().getValue() / Constants.PIVOT_GEAR_RATIO;
+        current.motorOutputVolts = 12 * supplyVoltageSignal.getValue();
+
+        return(current);
+    }
+}

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

@@ -0,0 +1,55 @@
+// 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 edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+/** Add your docs here. */
+public class PivotIOSim implements PivotIO {
+
+  SingleJointedArmSim arm =
+      new SingleJointedArmSim(
+          DCMotor.getFalcon500(1),
+          27 * (24 / 18),
+          SingleJointedArmSim.estimateMOI(Units.inchesToMeters(30), 3),
+          Units.inchesToMeters(30),
+          0,
+          3.5,
+          true,
+          0);
+  PIDController pid = new PIDController(1, 0, 0);
+
+  public PivotIOInputsAutoLogged updateInputs() {
+
+    PivotIOInputsAutoLogged input = new PivotIOInputsAutoLogged();
+
+    // var motorSimState = motor.getSimState();
+
+    input.velocityRPM = arm.getVelocityRadPerSec() * 60;
+    input.currentDrawAmps = arm.getCurrentDrawAmps();
+    input.temperatureCelsius = 0;
+    input.motorOutputVolts = arm.getOutput(0);
+    input.positionRotations = Units.radiansToDegrees(arm.getAngleRads());
+
+    arm.update(0.020);
+
+    return input;
+  }
+
+  @Override
+  public void setPosition(double degrees) {
+    double outputVolts = pid.calculate(Units.radiansToDegrees(arm.getAngleRads()), degrees);
+    arm.setInput(outputVolts);
+  }
+
+  @Override
+  public void reset(double degrees) {
+    // TODO Auto-generated method stub
+
+  }
+}

src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.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.subsystems.pivot;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** Add your docs here. */
+public class PivotSubsystem extends SubsystemBase {
+    PivotIO io;
+    PivotIOInputsAutoLogged inputs;
+
+    public PivotSubsystem(PivotIO io) {
+        this.io = io;
+        inputs =  new PivotIOInputsAutoLogged();
+
+        SmartDashboard.putData("reset Pivot value", new InstantCommand(
+            ()->{io.reset(0);}
+            ));
+    }
+
+    public InstantCommand reset (){
+       return new InstantCommand(
+            ()->{io.reset(0);}
+            );
+    }
+
+    public RunCommand run(double degrees) {
+        return new RunCommand(() -> {
+            io.setPosition(degrees);
+        }, this);
+    }   
+
+    @Override
+    public void periodic() {
+        inputs = io.updateInputs();
+        Logger.processInputs("Pivot", inputs);
+    }
+}

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -0,0 +1,25 @@
+// 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.shooter;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/** Add your docs here. */
+public interface ShooterIO {
+  @AutoLog
+  public class ShooterIOInputs {
+
+    // Motor values
+    public double velocityRPM;
+    public double currentDrawAmps;
+    public double temperatureCelsius;
+    public double motorOutputVolts;  }
+
+  public abstract void setVelocity(double velocity);
+
+  public abstract void reset(double velocity);
+
+  public abstract ShooterIOInputsAutoLogged updateInputs();
+}

src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import frc.robot.Constants;
+
+public class ShooterIOReal {
+
+    private TalonFX bottomShooterMotor = new TalonFX(Constants.BOTTOM_SHOOTER_MOTOR_ID);
+    private VelocityVoltage bottomShooterMotorVelocity = new VelocityVoltage(0);
+
+
+    private StatusSignal<Double> supplyVoltageSignal = bottomShooterMotor.getDutyCycle();
+    private StatusSignal<Double> velocity = bottomShooterMotor.getRotorVelocity();
+    private StatusSignal<Double> currentDraw = bottomShooterMotor.getStatorCurrent();
+
+    public ShooterIOReal(){
+        TalonFXConfiguration pivotConfig  = new TalonFXConfiguration();
+        pivotConfig.Slot0.kP = 1;
+        pivotConfig.Slot0.kD = 0.01;
+        pivotConfig.Slot0.kI = 0;
+    }
+
+    public void setVelocity(double velocity) {
+        bottomShooterMotor.setControl(bottomShooterMotorVelocity.withVelocity(velocity));
+    }
+
+
+    public ShooterIOInputsAutoLogged updateInputs() {
+        ShooterIOInputsAutoLogged current = new ShooterIOInputsAutoLogged();
+
+        current.currentDrawAmps = currentDraw.refresh().getValue();
+        current.velocityRPM = velocity.refresh().getValue() / Constants.BOTTOM_SHOOTER_GEAR_RATIO;
+        current.motorOutputVolts = 12 * supplyVoltageSignal.getValue();
+
+        return(current);
+    }
+}