Skip to content
3 changes: 2 additions & 1 deletion .github/workflows/pr.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ jobs:
permissions:
actions: read
contents: write
security-events: write
security-events: write

strategy:
fail-fast: false
Expand Down Expand Up @@ -83,5 +83,6 @@ jobs:
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
SONAR_TOKEN: ${{ secrets.SONAR_TOKEN }}
SONAR_HOST_URL: https://sonarcloud.io
PACKAGE_FETCH_ACTOR: ${{ github.actor }}
PACKAGE_FETCH_TOKEN: ${{ secrets.GITHUB_TOKEN }}
17 changes: 9 additions & 8 deletions src/main/java/poplib/subsytems/pivot/TalonPivot.java
Original file line number Diff line number Diff line change
@@ -1,12 +1,7 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package poplib.subsytems.pivot;

import com.ctre.phoenix6.controls.PositionDutyCycle;
import com.ctre.phoenix6.hardware.TalonFX;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import poplib.control.FFConfig;
import poplib.motor.FollowerConfig;
Expand Down Expand Up @@ -35,7 +30,8 @@ public TalonPivot(MotorConfig leadConfig, FollowerConfig followerConfig, double
position = new PositionDutyCycle(0.0);
position.withSlot(leadMotor.getClosedLoopSlot().getValue());

resetToAbsolutePosition();
leadMotor.setPosition(0.0);

}

public boolean atSetpoint(double error, double setpoint) {
Expand All @@ -46,15 +42,20 @@ public double getError(double setpoint) {
return Math.abs(leadMotor.getPosition().getValueAsDouble() - setpoint);
}

public void updatePID() {
leadMotor.setControl(position.withPosition(super.setpoint.get()).withFeedForward(super.ff.getKg()));
}

@Override
public void log() {
super.log();
SmartDashboard.putNumber("Lead Position 2 " + getName(), leadMotor.getPosition().getValueAsDouble());
SmartDashboard.putNumber("Lead Position " + getName(), leadMotor.getPosition().getValueAsDouble());
}

@Override
public void periodic() {
pid.updatePID(leadMotor);
leadMotor.setControl(position.withPosition(super.setpoint.get()).withFeedForward(super.ff.getKg()));
leadMotor.setControl(position.withPosition(super.setpoint.get()).withFeedForward(super.ff.calculate(
Math.toRadians(leadMotor.getPosition().getValueAsDouble()),
0.0
Expand All @@ -65,4 +66,4 @@ public void periodic() {
public void resetToAbsolutePosition() {
leadMotor.setPosition(getAbsolutePosition());
}
}
}
Loading