Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Shooter pivot things #14

Merged
merged 4 commits into from
Feb 7, 2024
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
on wait command not working
  • Loading branch information
stem-bud committed Feb 7, 2024
commit 959b58a5d87a38c44409d0e4911368beb9008cd4
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
@@ -112,7 +112,8 @@ public static final class ShooterPivotConstants {
public static final int SHOOTER_PIVOT_MAX_CURRENT = 30;
public static final double MAX_DEGREES_PER_SECOND = 5;
public static final PIDFFGains SHOOTER_PIVOT_GAINS =
PIDFFGains.builder().name("ShooterPivot Controller").kP(20).kD(0).kG(0.85).build();
PIDFFGains.builder().name("ShooterPivot Controller").kP(0.6).kD(0).kG(0.85).build();
public static final double OFFSET = 118.7;
}

public static final class ElevatorConstants {
8 changes: 3 additions & 5 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -283,11 +283,9 @@ public void robotInit() {
operator
.y()
.whileTrue(
new InstantCommand(
() -> {
shooterPivot.setGoal(20);
elevator.setTargetHeight(20);
}));
new SequentialCommandGroup(
Elevator.Commands.setToHeightAndWait(20),
ShooterPivot.Commands.setTargetAndWait(20)));

// operator.a().whileTrue(autoCommand)

5 changes: 2 additions & 3 deletions src/main/java/frc/robot/subsystems/elevatorIO/Elevator.java
Original file line number Diff line number Diff line change
@@ -8,7 +8,6 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.Constants;
import frc.robot.Robot;
import frc.robot.util.AccelerationCalc;
@@ -83,8 +82,8 @@ public double getCurrentHeight() {
public static class Commands {

public static Command setToHeightAndWait(double targetHeightInches) {
return new SequentialCommandGroup(
setToHeight(targetHeightInches), new WaitUntilCommand(Robot.elevator::atTargetHeight));
return setToHeight(targetHeightInches).until(Robot.elevator::atTargetHeight);
//return new SequentialCommandGroup(new InstantCommand(()->{Robot.elevator});
}

public static Command setToHeight(double height) {
Original file line number Diff line number Diff line change
@@ -63,6 +63,11 @@ public void periodic() {
}

IO.updateInputs(inputs);
Logger.recordOutput("ShooterPivot/isAtTarget", this.isAtTargetAngle());
}

public boolean isAtTargetAngle() {
return (Math.abs(getCurrentAngle() - this.targetDegs) < 0.001);
}

public double getCurrentAngle() {
@@ -87,5 +92,9 @@ public static Command setTargetAngle(double angleDegrees) {
Robot.shooterPivot.setTargetAngle(angleDegrees);
});
}

public static Command setTargetAndWait(double angleDegrees) {
return setTargetAngle(angleDegrees).until(Robot.shooterPivot::isAtTargetAngle);
}
}
}
Original file line number Diff line number Diff line change
@@ -13,6 +13,7 @@ public static class ShooterPivotInputs {
public double currentDrawOne = 0.0;
public double absoluteEncoderVolts = 0.0;
public double absoluteEncoderAdjustedAngle = 0.0;
public double targetAngle = 0.0;
}

public void updateInputs(ShooterPivotInputs inputs);
Original file line number Diff line number Diff line change
@@ -7,6 +7,8 @@
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
import frc.robot.Constants;
import frc.robot.rhr.RHRFeedForward;
import frc.robot.rhr.RHRPIDFFController;

public class ShooterPivotIOSim implements ShooterPivotIO {

@@ -20,11 +22,20 @@ public class ShooterPivotIOSim implements ShooterPivotIO {
Constants.ShooterPivotConstants.LENGTH_METERS,
Units.degreesToRadians(Constants.ShooterPivotConstants.RETRACTED_ANGLE_DEGREES),
Units.degreesToRadians(Constants.ShooterPivotConstants.MAX_ANGLE_DEGREES),
Constants.ShooterPivotConstants.SIMULATE_GRAVITY,
!Constants.ShooterPivotConstants.SIMULATE_GRAVITY,
Constants.ShooterPivotConstants.STARTING_ANGLE_RADS);
private double targetAngle;

public ShooterPivotIOSim() {}
private RHRPIDFFController motorController;

private RHRFeedForward feedforward;

private double voltage;

public ShooterPivotIOSim() {
motorController = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createRHRController();
feedforward = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createRHRFeedForward();
}

@Override
public void updateInputs(ShooterPivotInputs inputs) {
@@ -33,8 +44,8 @@ public void updateInputs(ShooterPivotInputs inputs) {
}

sim.update(0.02);
// TODO: WRONG
inputs.outputVoltage = MathUtil.clamp(sim.getOutput(0), -12.0, 12.0);

inputs.outputVoltage = this.voltage;

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

@@ -45,6 +56,10 @@ public void updateInputs(ShooterPivotInputs inputs) {
inputs.tempCelciusOne = 0.0;

inputs.currentDrawOne = sim.getCurrentDrawAmps();

double effort = motorController.calculate(inputs.absoluteEncoderAdjustedAngle, targetAngle);
effort = MathUtil.clamp(effort, -12, 12);
setVoltage(effort);
}

@Override
@@ -54,11 +69,12 @@ public void reseedPosition(double angleDeg) {

@Override
public void setTargetPosition(double angleDeg) {
targetAngle = angleDeg;
this.targetAngle = angleDeg;
}

@Override
public void setVoltage(double volts) {
sim.setInputVoltage(volts);
this.voltage = volts;
}
}
Original file line number Diff line number Diff line change
@@ -5,7 +5,11 @@
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkAnalogSensor;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.rhr.RHRFeedForward;
import frc.robot.rhr.RHRPIDFFController;
import frc.robot.util.RedHawkUtil;
import java.util.HashMap;

@@ -14,6 +18,12 @@ public class ShooterPivotIOSparks implements ShooterPivotIO {
CANSparkMax spark;
SparkAnalogSensor analogSensor;

private double targetAngle;

private RHRPIDFFController motorController;

private RHRFeedForward feedforward;

public ShooterPivotIOSparks() {
spark = new CANSparkMax(0, MotorType.kBrushless);
analogSensor = spark.getAnalog(SparkAnalogSensor.Mode.kAbsolute);
@@ -22,6 +32,7 @@ public ShooterPivotIOSparks() {
spark.getPIDController().setFeedbackDevice(analogSensor);

spark.setSmartCurrentLimit(20);

RedHawkUtil.configureCANSparkMAXStatusFrames(
new HashMap<>() {
{
@@ -38,30 +49,48 @@ public ShooterPivotIOSparks() {

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

motorController = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createRHRController();
feedforward = Constants.ShooterPivotConstants.SHOOTER_PIVOT_GAINS.createRHRFeedForward();
}

@Override
public void updateInputs(ShooterPivotInputs inputs) {
inputs.absoluteEncoderAdjustedAngle =
Units.rotationsToDegrees(spark.getEncoder().getPosition());
// inputs.

inputs.angleDegreesOne = inputs.absoluteEncoderAdjustedAngle;

inputs.velocityDegreesPerSecondOne =
Units.radiansToDegrees(
Units.rotationsPerMinuteToRadiansPerSecond(spark.getEncoder().getVelocity()));

inputs.tempCelciusOne = spark.getMotorTemperature();

inputs.currentDrawOne = spark.getOutputCurrent();

inputs.outputVoltage = spark.getBusVoltage();

double effort =
motorController.calculate(inputs.absoluteEncoderAdjustedAngle, this.targetAngle);

effort = MathUtil.clamp(effort, -12, 12);
setVoltage(effort);
}

@Override
public void reseedPosition(double angleDeg) {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'reseedPosition'");
double trueAngle = angleDeg - Constants.ShooterPivotConstants.OFFSET;
spark.getEncoder().setPosition(trueAngle);
}

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

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