Corrected the elevator velocity converstion factor and added the controller reset in the right place
This commit is contained in:
parent
42d15ab101
commit
f6aeec7c7e
@ -17,7 +17,8 @@ public class ElevatorConstants {
|
||||
public static final int kBottomLimitSwitchID = 0;
|
||||
|
||||
// 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
|
||||
public static final double kEncoderConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
|
||||
public static final double kEncoderPositionConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0;
|
||||
public static final double kEncoderVelocityConversionFactor = 11.0/60.0 * (22.0*0.25) * 2.0 / 60;
|
||||
|
||||
public static final int kCurrentLimit = 40;
|
||||
|
||||
@ -74,27 +75,7 @@ public class ElevatorConstants {
|
||||
.idleMode(kIdleMode)
|
||||
.inverted(true);
|
||||
motorConfig.encoder
|
||||
.positionConversionFactor(kEncoderConversionFactor)
|
||||
.velocityConversionFactor(kEncoderConversionFactor);
|
||||
/*motorConfig.closedLoop
|
||||
.p(kPositionControllerP)
|
||||
.i(kPositionControllerI)
|
||||
.d(kPositionControllerD)
|
||||
.velocityFF(0.0); // keep at zero for position pid
|
||||
motorConfig.closedLoop.maxMotion
|
||||
.maxAcceleration(kMaxAcceleration)
|
||||
.maxVelocity(kMaxVelocity)
|
||||
.allowedClosedLoopError(kAllowedError);*/
|
||||
}
|
||||
|
||||
public static final SparkMaxConfig motorConfig2 = new SparkMaxConfig();
|
||||
|
||||
static {
|
||||
motorConfig2
|
||||
.smartCurrentLimit(kCurrentLimit)
|
||||
.idleMode(kIdleMode)
|
||||
.inverted(false);
|
||||
motorConfig.encoder
|
||||
.positionConversionFactor(kEncoderConversionFactor);
|
||||
.positionConversionFactor(kEncoderPositionConversionFactor)
|
||||
.velocityConversionFactor(kEncoderVelocityConversionFactor);
|
||||
}
|
||||
}
|
||||
|
@ -172,13 +172,13 @@ public class Elevator extends SubsystemBase {
|
||||
|
||||
return run(() -> {
|
||||
//pidController.reset(encoder.getPosition(), encoder.getVelocity());
|
||||
|
||||
elevatorMotor1.setVoltage(
|
||||
pidControllerUp.calculate(
|
||||
encoder.getPosition(),
|
||||
clampedSetpoint
|
||||
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
||||
);
|
||||
|
||||
});
|
||||
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user