Skip to content

Commit

Permalink
Final changes before merge
Browse files Browse the repository at this point in the history
  • Loading branch information
keckothedragon committed May 29, 2024
1 parent 04734e6 commit a0e2803
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 9 deletions.
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/RobotState.java
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,13 @@ public class RobotState {
// advantagescope components
private final boolean kComponentsEnabled = true;
private final Pose3d kShooterZeroPose =
new Pose3d(new Translation3d(0.017, 0.0, 0.415), new Rotation3d());
new Pose3d(
new Translation3d(0.017, 0.0, 0.415),
new Rotation3d(0.0, Units.degreesToRadians(-2), 0.0));
private final Pose3d kIntakeZeroPose =
new Pose3d(
new Translation3d(-0.269, -0.01, 0.2428),
new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(170), 0.0));
new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(201), 0.0));

private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake, Led led) {
// subsystems
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,26 +2,32 @@

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants.IntakeConstants;

public class IntakePivotIONeo implements IntakePivotIO {
private CANSparkMax m_motor;
private AbsoluteEncoder m_encoder;
private AbsoluteEncoder m_absEncoder;
private RelativeEncoder m_relEncoder;

public IntakePivotIONeo(int port) {
m_motor = new CANSparkMax(port, CANSparkMax.MotorType.kBrushless);
m_encoder = m_motor.getAbsoluteEncoder();
m_encoder.setPositionConversionFactor(IntakeConstants.kPivotGearRatio);
m_absEncoder = m_motor.getAbsoluteEncoder();

m_encoder.setZeroOffset(IntakeConstants.kPivotOffset.getRadians());
// do not ask me how, but the gear ratio causes the position unit to be in radians
// all future times that units are used for this encoder are in radians
m_absEncoder.setPositionConversionFactor(IntakeConstants.kPivotGearRatio);
m_absEncoder.setZeroOffset(IntakeConstants.kPivotOffset.getRadians());

m_relEncoder = m_motor.getEncoder();
}

@Override
public void updateInputs(IntakePivotInputs inputs) {
inputs.curAngle = Units.rotationsToRadians(m_encoder.getPosition());
inputs.curVelocity =
Units.rotationsPerMinuteToRadiansPerSecond(m_motor.getEncoder().getVelocity());
inputs.curAngle = m_absEncoder.getPosition();

inputs.curVelocity = Units.rotationsPerMinuteToRadiansPerSecond(m_relEncoder.getVelocity());
inputs.curVoltage = m_motor.getAppliedOutput();
inputs.curAmps = m_motor.getOutputCurrent();
}
Expand Down

0 comments on commit a0e2803

Please sign in to comment.