encoder weird

This commit is contained in:
2026-03-01 14:50:55 -05:00
parent 866e6b99df
commit 80ef3a3431
2 changed files with 14 additions and 6 deletions

View File

@@ -37,20 +37,20 @@ public class ShooterConstants {
public static final int kRightShooterMotorCANID = 5; public static final int kRightShooterMotorCANID = 5;
public static final boolean kLeftShooterMotorInverted = true; public static final boolean kLeftShooterMotorInverted = true;
public static final boolean kRightShooterMotorInverted = true; public static final boolean kRightShooterMotorInverted = false;
public static final double kLeftP = 0;//0.1; public static final double kLeftP = 0;//0.1;
public static final double kLeftI = 0; public static final double kLeftI = 0;
public static final double kLeftD = 0; public static final double kLeftD = 0;
public static final double kLeftS = 0; public static final double kLeftS = 0;
public static final double kLeftV = 0.21; public static final double kLeftV = 0.0;
public static final double kLeftA = 0; public static final double kLeftA = 0;
public static final double kRightP = 0;//0.1; public static final double kRightP = 0;//0.1;
public static final double kRightI = 0; public static final double kRightI = 0;
public static final double kRightD = 0; public static final double kRightD = 0;
public static final double kRightS = 0; public static final double kRightS = 0;
public static final double kRightV = 0.21; public static final double kRightV = 0.1;
public static final double kRightA = 0; public static final double kRightA = 0;
public static final double kMaxManualSpeedMultiplier = 1; public static final double kMaxManualSpeedMultiplier = 1;
@@ -75,7 +75,7 @@ public class ShooterConstants {
.inverted(kLeftShooterMotorInverted); .inverted(kLeftShooterMotorInverted);
kLeftMotorConfig.absoluteEncoder kLeftMotorConfig.absoluteEncoder
.positionConversionFactor(1) .positionConversionFactor(1)
.velocityConversionFactor(1); .velocityConversionFactor(60);
kLeftMotorConfig.closedLoop kLeftMotorConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kLeftP, kLeftI, kLeftD) .pid(kLeftP, kLeftI, kLeftD)
@@ -89,7 +89,7 @@ public class ShooterConstants {
.inverted(kRightShooterMotorInverted); .inverted(kRightShooterMotorInverted);
kRightMotorConfig.absoluteEncoder kRightMotorConfig.absoluteEncoder
.positionConversionFactor(1) .positionConversionFactor(1)
.velocityConversionFactor(1) .velocityConversionFactor(60)
.inverted(true); .inverted(true);
kRightMotorConfig.closedLoop kRightMotorConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)

View File

@@ -7,6 +7,7 @@ import org.littletonrobotics.junction.Logger;
import com.revrobotics.AbsoluteEncoder; import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.PersistMode; import com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode; import com.revrobotics.ResetMode;
import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkMax;
@@ -25,6 +26,8 @@ public class Shooter extends SubsystemBase {
private AbsoluteEncoder leftEncoder; private AbsoluteEncoder leftEncoder;
private AbsoluteEncoder rightEncoder; private AbsoluteEncoder rightEncoder;
private RelativeEncoder rightRelative;
private SparkClosedLoopController leftClosedLoopController; private SparkClosedLoopController leftClosedLoopController;
private SparkClosedLoopController rightClosedLoopController; private SparkClosedLoopController rightClosedLoopController;
@@ -54,6 +57,8 @@ public class Shooter extends SubsystemBase {
// TODO Set this to the initial startup speed // TODO Set this to the initial startup speed
targetSpeeds = null; targetSpeeds = null;
rightRelative = rightMotor.getEncoder();
} }
@Override @Override
@@ -66,6 +71,9 @@ public class Shooter extends SubsystemBase {
Logger.recordOutput("Shooter/LeftRollers/CurrentRPM", leftEncoder.getVelocity()); Logger.recordOutput("Shooter/LeftRollers/CurrentRPM", leftEncoder.getVelocity());
Logger.recordOutput("Shooter/RightRollers/CurrentRPM", rightEncoder.getVelocity()); Logger.recordOutput("Shooter/RightRollers/CurrentRPM", rightEncoder.getVelocity());
Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity());
// TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance? // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint()); Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
Logger.recordOutput("Shooter/RightRollers/AtSetpoint", rightClosedLoopController.isAtSetpoint()); Logger.recordOutput("Shooter/RightRollers/AtSetpoint", rightClosedLoopController.isAtSetpoint());
@@ -85,7 +93,7 @@ public class Shooter extends SubsystemBase {
); );
rightClosedLoopController.setSetpoint( rightClosedLoopController.setSetpoint(
-targetSpeeds.getSpeedRPM(), targetSpeeds.getSpeedRPM(),
ControlType.kVelocity ControlType.kVelocity
); );
} }