Skip to content

Commit

Permalink
added more constants and made each implmemetation have its own pid
Browse files Browse the repository at this point in the history
  • Loading branch information
SourWasHere committed Jan 25, 2025
1 parent 9a914d5 commit 3e2508a
Show file tree
Hide file tree
Showing 4 changed files with 16 additions and 10 deletions.
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/constants/ClawConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,5 +7,7 @@ public class ClawConstants {
public static DCMotor smallGearBox;
public static double gearing = 0.1;
public static double kP = 0.0;
public static double kI = 0.0;
public static double kD = 0.0;
public static double mainSetpoint = 1.0;
}
Original file line number Diff line number Diff line change
Expand Up @@ -6,20 +6,18 @@

public interface EndEffectorIO {

final PIDController pid = new PIDController(ClawConstants.kP, 0, 0);

@AutoLog
public class EndEffectorInputsAutoLogged { //autolog volts
public double centralVolts = 0.0;
public double gripperVolts = 0.0;
}

public default void setupPID(double errorTolerance, double errorDerivativeTolerance, double minimumIntegral, double maximumIntegral) {
public default void setupPID(PIDController pid, double errorTolerance, double errorDerivativeTolerance, double minimumIntegral, double maximumIntegral) {
pid.setTolerance(errorTolerance, errorDerivativeTolerance);
pid.setIntegratorRange(minimumIntegral, maximumIntegral);
}

public default double pidCalculate(EndEffectorEncoderIO encoder, double setpoint) {
public default double pidCalculate(PIDController pid, EndEffectorEncoderIO encoder, double setpoint) {
return pid.calculate(encoder.getDistance(), setpoint);
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot.subsystems.ClawSubsystem;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import frc.robot.constants.ClawConstants;
Expand All @@ -16,20 +17,22 @@ public class EndEffectorIOSim implements EndEffectorIO {
ClawConstants.smallGearBox
);

final PIDController pid = new PIDController(ClawConstants.kP, ClawConstants.kI, ClawConstants.kD);

public EndEffectorIOSim() {
setupPID(3.0, 5.0, -0.5, 0.5); //change pid settings
setupPID(pid, 3.0, 5.0, -0.5, 0.5); //change pid settings
}

private final EndEffectorEncoderIOSim encoder = new EndEffectorEncoderIOSim();

@Override
public void centralToSetpoint(double setpoint) { //move wheels to setpoint
setCentralVelocity(pidCalculate(encoder, setpoint));
setCentralVelocity(pidCalculate(pid, encoder, setpoint));
}

@Override
public void grippersToSetpoint(double setpoint) {
setGrippersVelocity(pidCalculate(encoder, setpoint));
setGrippersVelocity(pidCalculate(pid, encoder, setpoint));
}

@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.subsystems.ClawSubsystem;
import com.revrobotics.spark.SparkLowLevel;
import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.controller.PIDController;
import frc.robot.constants.ClawConstants;


Expand All @@ -10,20 +11,22 @@ public class EndEffectorIOSparkMax implements EndEffectorIO {
private final EndEffectorEncoderIO encoder;

public EndEffectorIOSparkMax(int canID, int smallCanID, EndEffectorEncoderIOSim encoder) {
setupPID(3.0, 5.0, -0.5, 0.5); //change pid setting
setupPID(pid, 3.0, 5.0, -0.5, 0.5); //change pid setting
centralWheel = new SparkMax(canID, SparkLowLevel.MotorType.kBrushless); //big
gripperWheels = new SparkMax(smallCanID, SparkLowLevel.MotorType.kBrushless); //small
this.encoder = encoder;
}

final PIDController pid = new PIDController(ClawConstants.kP, ClawConstants.kI, ClawConstants.kD);

@Override
public void centralToSetpoint(double setpoint) { //move wheels to setpoint
setCentralVelocity(pidCalculate(encoder, setpoint));
setCentralVelocity(pidCalculate(pid, encoder, setpoint));
}

@Override
public void grippersToSetpoint(double setpoint) {
setGrippersVelocity(pidCalculate(encoder, setpoint));
setGrippersVelocity(pidCalculate(pid, encoder, setpoint));
}

@Override
Expand Down

0 comments on commit 3e2508a

Please sign in to comment.