Skip to content
This repository has been archived by the owner on Feb 3, 2025. It is now read-only.

Commit

Permalink
angle motors still doesnt work
Browse files Browse the repository at this point in the history
  • Loading branch information
gvaldez7206 committed Jul 27, 2024
1 parent 30c072c commit 157eef8
Show file tree
Hide file tree
Showing 3 changed files with 15 additions and 9 deletions.
4 changes: 2 additions & 2 deletions src/main/java/frc/lib/util/swerve/SwerveModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ public void periodic() {
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
desiredState = SwerveModuleState.optimize(desiredState, getCANcoder());
io.setAngleMotor(desiredState.angle.getRadians());
io.setAngleMotor(desiredState.angle.getDegrees());
setSpeed(desiredState, isOpenLoop);
}

Expand Down Expand Up @@ -120,7 +120,7 @@ public SwerveModuleState getState() {
return new SwerveModuleState(
Conversions.rotationPerSecondToMetersPerSecond(inputs.driveMotorSelectedSensorVelocity,
Constants.Swerve.wheelCircumference),
Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder));
Rotation2d.fromRotations(inputs.angleMotorSelectedPosition));
}

/**
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ public interface SwerveModuleIO {
public static class SwerveModuleInputs {
public double driveMotorSelectedPosition;
public double driveMotorSelectedSensorVelocity;
public double angleMotorSelectedPosition;
public double absolutePositionAngleEncoder;
// public double driveMotorTemp;
// public double angleMotorTemp;
Expand Down
19 changes: 12 additions & 7 deletions src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkPIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
Expand All @@ -25,6 +26,7 @@ public class SwerveModuleReal implements SwerveModuleIO {
private TalonFX mDriveMotor;
private SparkPIDController angleController;
private CANcoder angleEncoder;
public RelativeEncoder angleMotorEncoder;
private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration();
private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration();

Expand All @@ -43,17 +45,17 @@ public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, in
mDriveMotor = new TalonFX(driveMotorID);
mAngleMotor = new CANSparkMax(angleMotorID, MotorType.kBrushless);
angleEncoder = new CANcoder(cancoderID);
angleMotorEncoder = mAngleMotor.getEncoder();

configAngleEncoder();
configAngleMotor();
configDriveMotor();

angleEncoder.getPosition();
driveMotorSelectedPosition = mDriveMotor.getPosition();
driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity();
absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition();
mAngleMotor.restoreFactoryDefaults();
desiredState.angle = new Rotation2d(absolutePositionAngleEncoder.getValue());



}
Expand Down Expand Up @@ -137,8 +139,8 @@ private void configAngleEncoder() {

@Override
public void setAngleMotor(double v) {
angleController.setReference(v, ControlType.kPosition);

angleController.setReference(v, ControlType.kPosition);
}


Expand All @@ -152,14 +154,17 @@ public void updateInputs(SwerveModuleInputs inputs) {
BaseStatusSignal.refreshAll(driveMotorSelectedPosition, driveMotorSelectedSensorVelocity);
inputs.driveMotorSelectedPosition = driveMotorSelectedPosition.getValue();
inputs.driveMotorSelectedSensorVelocity = driveMotorSelectedSensorVelocity.getValue();
inputs.angleMotorSelectedPosition = angleMotorEncoder.getPosition();
inputs.absolutePositionAngleEncoder = absolutePositionAngleEncoder.getValue();
System.out.println(inputs.angleMotorSelectedPosition);
// inputs.driveMotorTemp = mDriveMotor.getDeviceTemp().getValueAsDouble();
// inputs.angleMotorTemp = mAngleMotor.getDeviceTemp().getValueAsDouble();
}

// @Override
// public void setPositionAngleMotor(double absolutePosition) {
// angleMotorEncoder.setPosition(absolutePosition);
// }

@Override
public void setPositionAngleMotor(double absolutePosition) {
angleMotorEncoder.setPosition(absolutePosition);
}

}

0 comments on commit 157eef8

Please sign in to comment.