Skip to content

Commit

Permalink
go the Rev stuff working
Browse files Browse the repository at this point in the history
  • Loading branch information
legoguy1000 committed Dec 15, 2024
1 parent 447101c commit f9413c5
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 1 deletion.
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,8 @@ private void configureButtonBindings() {

/* Driver Buttons */
driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));

driver.x().whileTrue(s_Swerve.runNeo(1));
}

/**
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/subsystems/swerve/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.lib.util.FieldConstants;
import frc.lib.util.swerve.SwerveModule;
Expand Down Expand Up @@ -323,4 +325,8 @@ public double distanceFromSpeaker() {
- getPose().getX());
return distance;
}

public Command runNeo(double power) {
return Commands.runEnd(() -> swerveIO.runNeo(power), () -> swerveIO.runNeo(0), this);
}
}
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,9 @@ public static class SwerveInputs {
public double newyaw;
public double newroll;
public double newpitch;

public double neoPosition;
public double neoVelocity;
}

public default void updateInputs(SwerveInputs inputs) {}
Expand All @@ -40,4 +43,6 @@ public default Optional<Pose2d> getInitialPose() {

public default void setPose(Pose2d pose) {}

public default void runNeo(double power) {}

}
22 changes: 21 additions & 1 deletion src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
Original file line number Diff line number Diff line change
@@ -1,6 +1,14 @@
package frc.robot.subsystems.swerve;

import com.reduxrobotics.sensors.canandgyro.Canandgyro;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.studica.frc.AHRS;
import edu.wpi.first.math.geometry.Rotation2d;
import frc.lib.util.swerve.SwerveModule;
Expand All @@ -13,8 +21,14 @@ public class SwerveReal implements SwerveIO {
private AHRS gyro = new AHRS(Constants.Swerve.navXID);
private Canandgyro newGyro = new Canandgyro(1);

private SparkMax neo = new SparkMax(60, MotorType.kBrushless);
private RelativeEncoder neoEncoder = neo.getEncoder();

/** Real Swerve Initializer */
public SwerveReal() {}
public SwerveReal() {
SparkBaseConfig neoConfig = new SparkMaxConfig().inverted(true).idleMode(IdleMode.kCoast);
neo.configure(neoConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
}

@Override
public void updateInputs(SwerveInputs inputs) {
Expand All @@ -25,6 +39,8 @@ public void updateInputs(SwerveInputs inputs) {
inputs.newpitch = newGyro.getPitch();
inputs.newroll = newGyro.getRoll();

inputs.neoPosition = neoEncoder.getPosition();
inputs.neoVelocity = neoEncoder.getVelocity();
}

public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID,
Expand All @@ -50,4 +66,8 @@ public SwerveModule[] createModules() {
Constants.Swerve.Mod3.angleOffset)};
}

public void runNeo(double power) {
neo.set(power);
}

}

0 comments on commit f9413c5

Please sign in to comment.