Skip to content

Commit

Permalink
add pivot template
Browse files Browse the repository at this point in the history
  • Loading branch information
@tervay
tervay committed Feb 4, 2024
1 parent 9bcbad0 commit de0413c
Show file tree
Hide file tree
Showing 7 changed files with 92 additions and 113 deletions.
1 change: 0 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
import frc.robot.commands.fullRoutines.ThreePiece;
import frc.robot.commands.fullRoutines.ThreePieceChoreo;
import frc.robot.commands.otf.OTF;
import frc.robot.commands.otf.OTF.OTFOptions;
import frc.robot.commands.otf.RotateScore;
import frc.robot.subsystems.elevatorIO.Elevator;
import frc.robot.subsystems.elevatorIO.ElevatorIOSim;
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@ public Elevator(ElevatorIO IO) {
this.feedforward = Constants.ElevatorConstants.ELEVATOR_GAINS.createElevatorFeedforward();
this.inputs = new ElevatorInputsAutoLogged();
this.IO.updateInputs(inputs);
System.out.println(Constants.ElevatorConstants.ELEVATOR_GAINS.toString());

this.leftMotor = new LoggableMotor("Elevator Left", DCMotor.getNEO(1));
this.rightMotor = new LoggableMotor("Elevator Right", DCMotor.getNEO(1));
Expand Down
70 changes: 6 additions & 64 deletions src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivot.java
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,7 @@
package frc.robot.subsystems.shooterPivot;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
Expand All @@ -18,30 +14,23 @@
import org.littletonrobotics.junction.Logger;

public class ShooterPivot extends SubsystemBase {
public enum FourBarMode {
public enum ControlMode {
CLOSED_LOOP,
OPEN_LOOP,
}

private FourBarMode mode;
private ControlMode mode;

private final ProfiledPIDController ShooterPivotController;
public final ShooterPivotInputsAutoLogged inputs;
private final ShooterPivotIO IO;
private final ArmFeedforward ff;
private double targetDegs = 20;
private double targetDegs = 0;
private LoggableMotor motor = new LoggableMotor("ShooterPivot", DCMotor.getNEO(1));

public ShooterPivot(ShooterPivotIO IO) {

this.ff = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createArmFeedForward();
this.ShooterPivotController =
Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createProfiledPIDController(
new Constraints(100, 200));
this.inputs = new ShooterPivotInputsAutoLogged();
this.IO = IO;
this.IO.updateInputs(inputs);
mode = FourBarMode.CLOSED_LOOP;
mode = ControlMode.CLOSED_LOOP;
}

public void setTargetAngle(double angleInDegrees) {
Expand All @@ -65,57 +54,14 @@ public void periodic() {
Logger.recordOutput("ShooterPivot/Mode", mode);
switch (mode) {
case CLOSED_LOOP:
/*boolean shouldReset =
Math.abs(inputs.absoluteEncoderAdjustedAngle - inputs.angleDegreesOne) > 3;
if (shouldReset) {
// reseed();
}*/

double effort =
ShooterPivotController.calculate(
Units.degreesToRadians(inputs.absoluteEncoderAdjustedAngle),
Units.degreesToRadians(targetDegs));

var goal = ShooterPivotController.getGoal();
var setpoint = ShooterPivotController.getSetpoint();

Logger.recordOutput("ShooterPivot/Goal/Position", Units.radiansToDegrees(goal.position));
Logger.recordOutput("ShooterPivot/Goal/Velocity", Units.radiansToDegrees(goal.velocity));
Logger.recordOutput(
"ShooterPivot/Setpoint/Position", Units.radiansToDegrees(setpoint.position));
Logger.recordOutput(
"ShooterPivot/Setpoint/Velocity", Units.radiansToDegrees(setpoint.velocity));

// Logger.recordOutput("ShooterPivot/Should Reseed", shouldReset);

Logger.recordOutput("ShooterPivot/Control Effort", effort);

double ffEffort = ff.calculate(setpoint.position, setpoint.velocity);
Logger.recordOutput("ShooterPivot/FF Effort", ffEffort);

effort += ffEffort;
effort = MathUtil.clamp(effort, -12, 12);
voltage = effort;
Logger.recordOutput("ShooterPivot/Total Effort", effort);

double current = ShooterPivotConstants.SHOOTER_PIVOT_MAX_CURRENT;
// MathUtil.clamp(
// Math.abs(currentController.calculate(inputs.angleDegreesOne, targetDegs))
// + ShooterPivotConstants.FOUR_BAR_BASE_CURRENT,
// 0,
// ShooterPivotConstants.FOUR_BAR_MAX_CURRENT);
IO.setCurrentLimit((int) current);
// Logger.recordOutput("ShooterPivot/Current Limit", current);
IO.setTargetPosition(this.targetDegs);
break;
case OPEN_LOOP:
// System.out.println(Robot.operator.getLeftX());
// double deltaDegrees = Robot.operator.getLeftX();
// this.targetDegs += deltaDegrees;
voltage = -1 * Robot.operator.getLeftX() * ShooterPivotConstants.MAX_DEGREES_PER_SECOND;
IO.setVoltage(voltage);
break;
}

IO.setVoltage(voltage);
IO.updateInputs(inputs);
}

Expand All @@ -125,7 +71,6 @@ public double getCurrentAngle() {

public void setGoal(double goal) {
this.targetDegs = goal;
this.IO.setPosition(goal);
}

public static class Commands {
Expand All @@ -134,9 +79,6 @@ public static Command setTargetAngle(SuperStructureBuilder structure) {
() -> {
Robot.shooterPivot.setTargetAngle(structure.getShooterPivotAngleDegrees());
});
// if()
// Robot.elevator.s

}
}
}
21 changes: 3 additions & 18 deletions src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,34 +7,19 @@ public interface ShooterPivotIO {
@AutoLog
public static class ShooterPivotInputs {
public double outputVoltage = 0.0;

public double angleDegreesOne = 0.0;
public double angleDegreesTwo = 0.0;
public double angleDegreesRange = 0.0;

public double velocityDegreesPerSecondOne = 0.0;
public double velocityDegreesPerSecondTwo = 0.0;
public double velocityDegreesPerSecondRange = 0.0;

public double tempCelciusOne = 0.0;
public double tempCelciusTwo = 0.0;

public double currentDrawOne = 0.0;
public double currentDrawTwo = 0.0;

public double absoluteEncoderVolts = 0.0;
public double absoluteEncoderAdjustedAngle = 0.0;

public boolean limSwitch = false;
}

public void reseed(double absoluteEncoderVolts);

public void updateInputs(ShooterPivotInputs inputs);

public void setVoltage(double volts);
public void reseedPosition(double angleDeg);

public void setPosition(double angleDeg);
public void setVoltage(double volts);

public void setCurrentLimit(int currentLimit);
public void setTargetPosition(double angleDeg);
}
44 changes: 15 additions & 29 deletions src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,27 +25,6 @@ public class ShooterPivotIOSim implements ShooterPivotIO {

public ShooterPivotIOSim() {}

// TODO
@Override
public void setCurrentLimit(int currentLimit) {}

@Override
public void setPosition(double angleDeg) {
sim.setState(VecBuilder.fill(Units.degreesToRadians(angleDeg), 0.0));
}

@Override
public void setVoltage(double volts) {
sim.setInputVoltage(volts);
}

@Override
public void reseed(double absoluteEncoderVolts) {
sim.setState(
VecBuilder.fill(
Units.degreesToRadians(Constants.ShooterPivotConstants.RETRACTED_ANGLE_DEGREES), 0.0));
}

@Override
public void updateInputs(ShooterPivotInputs inputs) {
if (DriverStation.isDisabled()) {
Expand All @@ -56,22 +35,29 @@ public void updateInputs(ShooterPivotInputs inputs) {
inputs.outputVoltage = MathUtil.clamp(sim.getOutput(0), -12.0, 12.0);

inputs.angleDegreesOne = Units.radiansToDegrees(sim.getAngleRads()) + (Math.random() * 5 - 2.5);
inputs.angleDegreesRange = 0.0;

inputs.absoluteEncoderAdjustedAngle = Units.radiansToDegrees(sim.getAngleRads());

inputs.velocityDegreesPerSecondOne = Units.radiansToDegrees(sim.getVelocityRadPerSec());
inputs.velocityDegreesPerSecondTwo = Units.radiansToDegrees(sim.getVelocityRadPerSec());
inputs.velocityDegreesPerSecondRange = 0.0;

inputs.tempCelciusOne = 0.0;
inputs.tempCelciusTwo = 0.0;

inputs.currentDrawOne = sim.getCurrentDrawAmps();
inputs.currentDrawTwo = sim.getCurrentDrawAmps();
}

@Override
public void reseedPosition(double angleDeg) {
sim.setState(VecBuilder.fill(Units.degreesToRadians(angleDeg), 0.0));
}

inputs.limSwitch =
Units.radiansToDegrees(sim.getAngleRads())
>= Constants.ShooterPivotConstants.RETRACTED_ANGLE_DEGREES;
@Override
public void setTargetPosition(double angleDeg) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'setTargetPosition'");
}

@Override
public void setVoltage(double volts) {
sim.setInputVoltage(volts);
}
}
65 changes: 65 additions & 0 deletions src/main/java/frc/robot/subsystems/shooterPivot/ShooterPivotIOSparks.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
package frc.robot.subsystems.shooterPivot;

import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkAnalogSensor;
import frc.robot.util.RedHawkUtil;
import java.util.HashMap;

public class ShooterPivotIOSparks implements ShooterPivotIO {

CANSparkMax spark;
SparkAnalogSensor analogSensor;

public ShooterPivotIOSparks() {
spark = new CANSparkMax(0, MotorType.kBrushless);
analogSensor = spark.getAnalog(SparkAnalogSensor.Mode.kAbsolute);

spark.restoreFactoryDefaults();
spark.getPIDController().setFeedbackDevice(analogSensor);

spark.setSmartCurrentLimit(20);
RedHawkUtil.configureCANSparkMAXStatusFrames(
new HashMap<>() {
{
put(PeriodicFrame.kStatus0, 60);
put(PeriodicFrame.kStatus1, 40);
put(PeriodicFrame.kStatus2, 40);
put(PeriodicFrame.kStatus3, 65535);
put(PeriodicFrame.kStatus4, 65535);
put(PeriodicFrame.kStatus5, 20);
put(PeriodicFrame.kStatus6, 20);
}
},
spark);

spark.setIdleMode(IdleMode.kBrake);
spark.setInverted(false);
}

@Override
public void updateInputs(ShooterPivotInputs inputs) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'updateInputs'");
}

@Override
public void reseedPosition(double angleDeg) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'reseedPosition'");
}

@Override
public void setVoltage(double volts) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'setVoltage'");
}

@Override
public void setTargetPosition(double angleDeg) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'setTargetPosition'");
}
}
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/util/RedHawkUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,9 @@ public static Pose2d reflectIfRed(Pose2d old) {
}
}

/**
* https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames
*/
public static void configureCANSparkMAXStatusFrames(
HashMap<PeriodicFrame, Integer> config, CANSparkMax... sparks) {
config.forEach(
Expand Down

0 comments on commit de0413c

Please sign in to comment.