diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java index 36b280a..52df380 100644 --- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java @@ -57,6 +57,7 @@ public PivotSubsystem() { pivotMotor.setInverted(true); pivotMotor.clearStickyFaults(); pivotMotor.set(0); + pivotMotor.setPosition(0); pivotMotor.setNeutralMode(NeutralModeValue.Brake); @@ -153,6 +154,11 @@ public double getAngularError() { return targetDegrees - getCurrentAngle(); } + public void setPower(double power){ + this.power = power; + } + + @Override public void periodic() { @@ -170,6 +176,6 @@ public void periodic() { pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10); - pivotMotor.setVoltage(pidVoltageOutput); + pivotMotor.set(getFeedForward()+power); } }