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;
|
public static final int kBottomLimitSwitchID = 0;
|
||||||
|
|
||||||
// 60/11 gearing multiplied by circumference of sprocket multiplied by 2 for carriage position
|
// 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;
|
public static final int kCurrentLimit = 40;
|
||||||
|
|
||||||
@ -74,27 +75,7 @@ public class ElevatorConstants {
|
|||||||
.idleMode(kIdleMode)
|
.idleMode(kIdleMode)
|
||||||
.inverted(true);
|
.inverted(true);
|
||||||
motorConfig.encoder
|
motorConfig.encoder
|
||||||
.positionConversionFactor(kEncoderConversionFactor)
|
.positionConversionFactor(kEncoderPositionConversionFactor)
|
||||||
.velocityConversionFactor(kEncoderConversionFactor);
|
.velocityConversionFactor(kEncoderVelocityConversionFactor);
|
||||||
/*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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -172,13 +172,13 @@ public class Elevator extends SubsystemBase {
|
|||||||
|
|
||||||
return run(() -> {
|
return run(() -> {
|
||||||
//pidController.reset(encoder.getPosition(), encoder.getVelocity());
|
//pidController.reset(encoder.getPosition(), encoder.getVelocity());
|
||||||
|
|
||||||
elevatorMotor1.setVoltage(
|
elevatorMotor1.setVoltage(
|
||||||
pidControllerUp.calculate(
|
pidControllerUp.calculate(
|
||||||
encoder.getPosition(),
|
encoder.getPosition(),
|
||||||
clampedSetpoint
|
clampedSetpoint
|
||||||
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
) + feedForward.calculate(pidControllerUp.getSetpoint().velocity)
|
||||||
);
|
);
|
||||||
|
|
||||||
});
|
});
|
||||||
|
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user