diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e5700d5..8281fd9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,64 +22,66 @@ public final class ArmConstants { public static final double stowShootPosition = 0; public static final double lowShootVoltage = -0.1; public static final double midShootVoltage = -0.2; - public static final double highShootVoltage = -0.5; + public static final double highShootVoltage = -0.75; public static final double intakeVoltage = 0.2; public static final double spitVoltage = 0.5; + public static final double topShootPower = 0.5; + public static final double topIntakePower = -0.75; + } public static final class DriveConstants { - public static final double trackWidth = Units.inchesToMeters(22.75); //need to measure - public static final double wheelBase = Units.inchesToMeters(20.75); //need to measure - - public static final int frontLeftID = 52; - public static final int backLeftID = 53; - public static final int frontRightID = 14; - public static final int backRightID = 16; - - public static final int frontLeftRotationMotorID = 0; //placeholder - public static final int frontRightRotationMotorID = 0; //placeholder - public static final int backLeftRotationMotorID = 0; //placeholder - public static final int backRightRotationMotorID = 0; //placeholder - - public static final int frontLeftRotationEncoderID = 0; //placeholder - public static final int frontRightRotationEncoderID = 0; //placeholder - public static final int backLeftRotationEncoderID = 0; //placeholder - public static final int backRightRotationEncoderID = 0; //placeholder - - public static final double frontLeftAngleOffset = 0.0; //placeholder - public static final double frontRightAngleOffset = 0.0; //placeholder - public static final double backLeftAngleOffset = 0.0; //placeholder - public static final double backRightAngleOffset = 0.0; //placeholder + public static final double trackWidth = Units.inchesToMeters(20.4); // TODO: Might have to change + public static final double wheelBase = Units.inchesToMeters(22.4); // TODO: Might have to change + + public static final int frontLeftDriveID = 1; + public static final int frontRightDriveID = 3; + public static final int backRightDriveID = 5; + public static final int backLeftDriveID = 7; + + public static final int frontLeftRotationMotorID = 2; + public static final int frontRightRotationMotorID = 4; + public static final int backRightRotationMotorID = 6; + public static final int backLeftRotationMotorID = 8; + + public static final int frontLeftRotationEncoderID = 1; + public static final int frontRightRotationEncoderID = 2; + public static final int backRightRotationEncoderID = 3; + public static final int backLeftRotationEncoderID = 4; + + public static final double frontLeftAngleOffset = -2.3930100291016; + public static final double frontRightAngleOffset = 2.560213934981135; + public static final double backRightAngleOffset = 2.389942067525829; // TODO: Might have to switch these + public static final double backLeftAngleOffset = -0.582912699396544; // TODO: Might have to switch these public static final double autoDrivePercent = 0.6; - public static final int driveWheelGearReduction = 0; - public static final int wheelRadiusM = 0; + public static final double driveWheelGearReduction = 6.12; // placeholder + public static final double wheelRadiusM = Units.inchesToMeters(2); public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics( - new Translation2d(trackWidth / 2.0, wheelBase / 2.0), //front left placeholder - new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), //front right placeholder - new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), //rear left placeholder - new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) //rear right placeholder + new Translation2d(trackWidth / 2.0, wheelBase / 2.0), //TODO: Test + new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), //TODO: Test + new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), //TODO: Test + new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) //TODO: Test ); - public static final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.24, 2.185); //placeholder + public static final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.2, 0.3); // TODO: Not used - public static final int pigeonIMU = 0; //placeholder + public static final int pigeonIMU = 5; - public static final double driveJoystickDeadbandPercent = 0; //placeholder + public static final double driveJoystickDeadbandPercent = 0.12; - public static final double maxDriveSpeed = 0; //placeholder - public static final double maxTurnRate = 0; //placeholder + public static final double maxDriveSpeed = 5; + public static final double maxTurnRate = 2 * Math.PI; - public static final double driveKps = 0.0; //placeholder - public static final double driveKds = 0.0; //placeholder + public static final double driveKp = 0.01; //placeholder + public static final double driveKd = 0.0; //placeholder - public static final double rotationKps = 0.0; //placeholder - public static final double rotationKds = 0.0; //placeholder - + public static final double rotationKp = 7.0; + public static final double rotationKd = 0.0; } diff --git a/src/main/java/frc/robot/RobotConfiguration.java b/src/main/java/frc/robot/RobotConfiguration.java deleted file mode 100644 index 4868b85..0000000 --- a/src/main/java/frc/robot/RobotConfiguration.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot; - -public class RobotConfiguration { - - public boolean armEnabled; - - public RobotConfiguration() { - armEnabled = true; - - } - - public boolean armEnabled() { - return armEnabled; - } - - -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7e8f7eb..4233cd0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,19 +1,23 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.ArmMove; +import frc.robot.commands.DriveWithJoysticks; import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.BasicDriveSubsystem; import frc.robot.subsystems.ArmSubsystem.Mode; +import frc.robot.subsystems.drive.DriveSubsystem; import edu.wpi.first.wpilibj2.command.InstantCommand; + public class RobotContainer { - private final BasicDriveSubsystem drive = new BasicDriveSubsystem(); + private final DriveSubsystem drive = new DriveSubsystem(); private ArmSubsystem arm = new ArmSubsystem(); private Joystick leftJoystick = new Joystick(0); @@ -30,13 +34,14 @@ public class RobotContainer { SendableChooser autoChooser = new SendableChooser(); public RobotContainer() { - drive.setDefaultCommand( - new InstantCommand( - () -> drive.arcadeDrive( - leftJoystick.getY(), - rightJoystick.getX() / 2), - drive) - ); + + drive.setDefaultCommand(new DriveWithJoysticks( + drive, + () -> -leftJoystick.getRawAxis(1), + () -> -leftJoystick.getRawAxis(0), + () -> -rightJoystick.getRawAxis(0), + true + )); configureButtonBindings(); } @@ -52,6 +57,10 @@ private void configureButtonBindings() { .whileTrue(armShootLow); new Trigger(() -> gamepad.getLeftY() < -0.7) .whileTrue(armSpit); + new JoystickButton(rightJoystick, 1) + .whileTrue(new InstantCommand( + () -> {drive.resetHeading(); + })); } diff --git a/src/main/java/frc/robot/commands/drive/SwerveDriveCommand.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java similarity index 79% rename from src/main/java/frc/robot/commands/drive/SwerveDriveCommand.java rename to src/main/java/frc/robot/commands/DriveWithJoysticks.java index 7234815..4e59295 100644 --- a/src/main/java/frc/robot/commands/drive/SwerveDriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveWithJoysticks.java @@ -1,21 +1,23 @@ -package frc.robot.commands.drive; - +package frc.robot.commands; import java.util.function.DoubleSupplier; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.Constants.DriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; -public class SwerveDriveCommand extends CommandBase{ + +public class DriveWithJoysticks extends CommandBase{ private final DriveSubsystem drive; private final DoubleSupplier xPercent; private final DoubleSupplier yPercent; private final DoubleSupplier omegaPercent; private final boolean fieldRelative; - public SwerveDriveCommand(DriveSubsystem drive, DoubleSupplier xPercent, DoubleSupplier yPercent, DoubleSupplier omegaPercent, boolean fieldRelative) { + public DriveWithJoysticks(DriveSubsystem drive, DoubleSupplier xPercent, DoubleSupplier yPercent, DoubleSupplier omegaPercent, + boolean fieldRelative){ this.drive = drive; this.xPercent = xPercent; this.yPercent = yPercent; @@ -25,12 +27,10 @@ public SwerveDriveCommand(DriveSubsystem drive, DoubleSupplier xPercent, DoubleS addRequirements(drive); } - // Called when the command is initially scheduled. @Override public void initialize() { } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { double xMPerS = processJoystickInputs(xPercent.getAsDouble(), false) * DriveConstants.maxDriveSpeed; @@ -45,16 +45,6 @@ public void execute() { drive.setGoalChassisSpeeds(targetSpeeds); } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } - private double processJoystickInputs(double value, boolean square) { double scaledValue = 0.0; double deadband = DriveConstants.driveJoystickDeadbandPercent; @@ -68,4 +58,11 @@ private double processJoystickInputs(double value, boolean square) { } return scaledValue; } + + @Override + public void end(boolean interrupted) { + } + + + } diff --git a/src/main/java/frc/robot/subsystems/BasicDriveSubsystem.java b/src/main/java/frc/robot/subsystems/BasicDriveSubsystem.java deleted file mode 100644 index 5f5b21f..0000000 --- a/src/main/java/frc/robot/subsystems/BasicDriveSubsystem.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.DriveConstants; - -public class BasicDriveSubsystem extends SubsystemBase { - - private MotorControllerGroup leftWheels; - private MotorControllerGroup rightWheels; - - private DifferentialDrive drivetrain; - - public BasicDriveSubsystem() { - CANSparkMax backLeft = new CANSparkMax(DriveConstants.backLeftID, MotorType.kBrushless); - CANSparkMax frontLeft = new CANSparkMax(DriveConstants.frontLeftID, MotorType.kBrushless); - - CANSparkMax backRight = new CANSparkMax(DriveConstants.backRightID, MotorType.kBrushless); - CANSparkMax frontRight = new CANSparkMax(DriveConstants.frontRightID, MotorType.kBrushless); - - backLeft.setInverted(true); - frontLeft.setInverted(true); - backRight.setInverted(false); - frontRight.setInverted(false); - - leftWheels = new MotorControllerGroup(backLeft, frontLeft); - rightWheels = new MotorControllerGroup(backRight, frontRight); - - drivetrain = new DifferentialDrive(leftWheels, rightWheels); - } - - public void tankDrive(double leftSpeed, double rightSpeed) { - drivetrain.tankDrive(leftSpeed, rightSpeed); - } - - public void arcadeDrive(double speed, double rotation) { - drivetrain.arcadeDrive(speed, rotation); - } -} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 5aa995b..d6618e9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; + import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import frc.robot.Constants.ArmConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -9,7 +11,7 @@ public class Intake { private CANSparkMax leftMotor, rightMotor, topIntakeMotor; private double intakePower; private final double CURRENT_LIMIT = 25.0; - private boolean runTopIntakeMotor = false; + private double topIntakeMotorPower = 0; public Intake() { leftMotor = new CANSparkMax(ArmConstants.leftIntakeMotorID, MotorType.kBrushless); @@ -18,56 +20,58 @@ public Intake() { leftMotor.follow(rightMotor, true); leftMotor.setSmartCurrentLimit(80); rightMotor.setSmartCurrentLimit(80); - topIntakeMotor.setSmartCurrentLimit(80); + topIntakeMotor.setSmartCurrentLimit(10); + + rightMotor.setIdleMode(IdleMode.kCoast); + leftMotor.setIdleMode(IdleMode.kCoast); + topIntakeMotor.setIdleMode(IdleMode.kCoast); } - public void intake () { + public void intake() { intakePower = ArmConstants.intakeVoltage; - runTopIntakeMotor = true; + topIntakeMotorPower = ArmConstants.topIntakePower; } - public void shootLow () { + public void shootLow() { intakePower = ArmConstants.lowShootVoltage; - runTopIntakeMotor = false; + topIntakeMotorPower = ArmConstants.topShootPower; } - public void shootMid () { + public void shootMid() { intakePower = ArmConstants.midShootVoltage; - runTopIntakeMotor = false; + topIntakeMotorPower = ArmConstants.topShootPower; } - public void shootHigh () { + public void shootHigh() { intakePower = ArmConstants.highShootVoltage; - runTopIntakeMotor = false; + topIntakeMotorPower = ArmConstants.topShootPower; } - public void spit () { + public void spit() { intakePower = -ArmConstants.spitVoltage; - runTopIntakeMotor = true; + topIntakeMotorPower = ArmConstants.topShootPower; } - public void off () { + public void off() { intakePower = 0; - runTopIntakeMotor = true; + topIntakeMotorPower = 0; } public void setIntakeMotorPower(double percent) { - rightMotor.set(percent); - if(runTopIntakeMotor == true) { - topIntakeMotor.set(percent); - } else { - topIntakeMotor.set(0); - } + rightMotor.set(-percent); + topIntakeMotor.set(topIntakeMotorPower); } + public double getIntakeMotorAmps() { return rightMotor.getOutputCurrent(); } + private void checkIntakeAmps() { - if(getIntakeMotorAmps() > CURRENT_LIMIT) { + if (getIntakeMotorAmps() > CURRENT_LIMIT) { setIntakeMotorPower(0); } } - + public void run() { setIntakeMotorPower(intakePower); checkIntakeAmps(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index c62f284..e76b6ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -15,8 +15,8 @@ public class DriveSubsystem extends SubsystemBase{ - private final SwerveModules[] driveModules = new SwerveModules[4]; - private final PigeonInterface pigeonInterface = new PigeonInterface(); + private final SwerveModules[] driveModules = new SwerveModules[4]; // TODO: This will be the future format: FL, FR, BR, BL + private final PigeonInterface pigeonInterface = new PigeonInterface(); private boolean babyMode = false; @@ -27,19 +27,20 @@ public class DriveSubsystem extends SubsystemBase{ */ public DriveSubsystem() { - driveModules[0] = new SwerveModules(DriveConstants.frontLeftID, DriveConstants.frontLeftRotationMotorID, + driveModules[0] = new SwerveModules(DriveConstants.frontLeftDriveID, DriveConstants.frontLeftRotationMotorID, DriveConstants.frontLeftRotationEncoderID, DriveConstants.frontLeftAngleOffset, false, false); - driveModules[1] = new SwerveModules(DriveConstants.frontRightID, DriveConstants.frontRightRotationMotorID, + driveModules[1] = new SwerveModules(DriveConstants.frontRightDriveID, DriveConstants.frontRightRotationMotorID, DriveConstants.frontRightRotationEncoderID, DriveConstants.frontRightAngleOffset, true, false); - driveModules[2] = new SwerveModules(DriveConstants.backLeftID, DriveConstants.backLeftRotationMotorID, + driveModules[2] = new SwerveModules(DriveConstants.backRightDriveID, DriveConstants.backRightRotationMotorID, + DriveConstants.backRightRotationEncoderID, DriveConstants.backRightAngleOffset, false, false); + driveModules[3] = new SwerveModules(DriveConstants.backLeftDriveID, DriveConstants.backLeftRotationMotorID, DriveConstants.backLeftRotationEncoderID, DriveConstants.backLeftAngleOffset, false, false); - driveModules[3] = new SwerveModules(DriveConstants.backRightID, DriveConstants.backRightRotationMotorID, - DriveConstants.backRightRotationEncoderID, DriveConstants.backRightAngleOffset, true, false); for (int i = 0; i < 4; i++) { + // TODO: Maybe add specific PID values for each module driveModules[i].zeroEncoders(); - driveModules[i].setDrivePD(DriveConstants.driveKps, DriveConstants.driveKds); + driveModules[i].setDrivePD(DriveConstants.driveKp, DriveConstants.driveKd); driveModules[i].initModulePosition(); } @@ -77,13 +78,22 @@ public void periodic(){ RobotState.getInstance().updateOdometry(getRotation(), getSwerveModulePositions()); SmartDashboard.putNumber("Drive/velocity magnitude", getChassisSpeeds().vxMetersPerSecond); + + SmartDashboard.putNumber("Encoder Position 1", new Rotation2d(driveModules[0].getRotationPosition()).getRotations()); + SmartDashboard.putNumber("Encoder Position 2", new Rotation2d(driveModules[1].getRotationPosition()).getRotations()); + SmartDashboard.putNumber("Encoder Position 3", new Rotation2d(driveModules[2].getRotationPosition()).getRotations()); + SmartDashboard.putNumber("Encoder Position 4", new Rotation2d(driveModules[3].getRotationPosition()).getRotations()); } public void setGoalModuleStates(SwerveModuleState[] states) { + double[] array = new double[8]; for (int i = 0; i < 4; i++) { driveModules[i].setGoalModuleState(states[i]); + array[i*2] = driveModules[i].getRotationPosition(); + array[i*2+1] = driveModules[i].getDrivePosition(); } + SmartDashboard.putNumberArray("Drive/moduleStates/State", array); } /** @@ -93,6 +103,12 @@ public void setGoalModuleStates(SwerveModuleState[] states) { public void setGoalChassisSpeeds(ChassisSpeeds speeds) { speeds = new ChassisSpeeds(speeds.vxMetersPerSecond * (babyMode ? 0.2 : 1), speeds.vyMetersPerSecond * (babyMode ? 0.2 : 1), speeds.omegaRadiansPerSecond * (babyMode ? 0.1 : 1)); SwerveModuleState[] goalModuleStates = DriveConstants.kinematics.toSwerveModuleStates(speeds); + + // Convert back to spiral order. + SwerveModuleState tmp = goalModuleStates[2]; + goalModuleStates[2] = goalModuleStates[3]; + goalModuleStates[3] = tmp; + SwerveDriveKinematics.desaturateWheelSpeeds(goalModuleStates, DriveConstants.maxDriveSpeed); if (speeds.vxMetersPerSecond == 0 && speeds.vyMetersPerSecond == 0 && speeds.omegaRadiansPerSecond == 0) { goalModuleStates = new SwerveModuleState[] { @@ -102,6 +118,17 @@ public void setGoalChassisSpeeds(ChassisSpeeds speeds) { new SwerveModuleState(0, driveModules[3].getModulePosition().angle) }; } + double[] moduleGoalStates = { + driveModules[0].getRotationVelocity(), + driveModules[0].getDriveVelocityMPerS(), + driveModules[1].getRotationVelocity(), + driveModules[1].getDriveVelocityMPerS(), + driveModules[2].getRotationVelocity(), + driveModules[2].getDriveVelocityMPerS(), + driveModules[3].getRotationVelocity(), + driveModules[3].getDriveVelocityMPerS() + }; + SmartDashboard.putNumberArray("Drive/moduleStates/Goal", moduleGoalStates); setGoalModuleStates(goalModuleStates); } @@ -109,7 +136,7 @@ public Rotation2d getRotation() { return new Rotation2d(MathUtil.angleModulus(pigeonInterface.getHeading())); } - public void resetHeading() { + public void resetHeading() { pigeonInterface.resetHeading(); } @@ -145,7 +172,7 @@ public SwerveModuleState[] getModuleStates() { return states; } - private SwerveModulePosition[] getSwerveModulePositions() { + public SwerveModulePosition[] getSwerveModulePositions() { SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { @@ -164,6 +191,8 @@ public void setBrakeMode(boolean braked) { driveModules[i].setBrake(braked); } } + + } diff --git a/src/main/java/frc/robot/subsystems/drive/PigeonInterface.java b/src/main/java/frc/robot/subsystems/drive/PigeonInterface.java index f5ccb57..d53de23 100644 --- a/src/main/java/frc/robot/subsystems/drive/PigeonInterface.java +++ b/src/main/java/frc/robot/subsystems/drive/PigeonInterface.java @@ -3,6 +3,7 @@ import com.ctre.phoenix.sensors.PigeonIMU; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.DriveConstants; public class PigeonInterface{ @@ -23,6 +24,7 @@ public PigeonInterface() { } public double getHeading() { + SmartDashboard.putNumber("heading", pigeon.getYaw() - degHeadingOffset); return Units.degreesToRadians(pigeon.getYaw() - degHeadingOffset); } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModules.java b/src/main/java/frc/robot/subsystems/drive/SwerveModules.java index 14b67df..60a70cf 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModules.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModules.java @@ -3,6 +3,7 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderStatusFrame; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -22,7 +23,7 @@ public class SwerveModules extends SubsystemBase{ private final CANSparkMax rotationMotor; private final PIDController drivePID = new PIDController(0, 0, 0); - private final PIDController rotationPID = new PIDController(DriveConstants.rotationKps, 0, DriveConstants.rotationKds); + private final PIDController rotationPID = new PIDController(DriveConstants.rotationKp, 0, DriveConstants.rotationKd); private SwerveModulePosition modulePosition = new SwerveModulePosition(); private SwerveModuleState goalModuleState = new SwerveModuleState(); @@ -32,12 +33,12 @@ public class SwerveModules extends SubsystemBase{ private final double initialOffsetRadians; - public SwerveModules(int driveMotorID, int turningMotorID, int cancoderID, double measuredOffsetsRadians, boolean driveModeReversed, boolean turningMotorReversed + public SwerveModules(int driveMotorID, int rotationMotorID, int cancoderID, double measuredOffsetsRadians, boolean driveModeReversed, boolean turningMotorReversed ){ driveMotor = new CANSparkMax(driveMotorID, MotorType.kBrushless); - rotationMotor = new CANSparkMax(turningMotorID, MotorType.kBrushless); - rotationEncoder = new CANCoder(cancoderID); + rotationMotor = new CANSparkMax(rotationMotorID, MotorType.kBrushless); + rotationEncoder = new CANCoder(cancoderID); driveMotor.setIdleMode(IdleMode.kCoast); rotationMotor.setIdleMode(IdleMode.kCoast); @@ -57,8 +58,8 @@ public SwerveModules(int driveMotorID, int turningMotorID, int cancoderID, doubl // drivePidController = driveMotor.getPIDController(); // drivePidController.setFeedbackDevice(driveMotor.getEncoder()); - drivePID.setP(DriveConstants.driveKps); - drivePID.setD(DriveConstants.driveKds); + drivePID.setP(DriveConstants.driveKp); + drivePID.setD(DriveConstants.driveKd); //drivePidController.setOutputRange(0, 100); @@ -79,15 +80,12 @@ public double getRotationVelocity() { return Units.degreesToRadians(rotationEncoder.getVelocity()); } - /* Velocity gives Rotations per minute, need to go to radians per seconds - * - */ public double getDriveVelocityMPerS() { - return driveMotor.getEncoder().getVelocity() * 2 * Math.PI * DriveConstants.wheelRadiusM / DriveConstants.driveWheelGearReduction; + return driveMotor.getEncoder().getVelocity() * 2 * Math.PI * DriveConstants.wheelRadiusM / 60 / DriveConstants.driveWheelGearReduction; } public double getDriveVelocityRadPerS() { - return driveMotor.getEncoder().getVelocity() * 2 * Math.PI; + return driveMotor.getEncoder().getVelocity() * 2 * Math.PI / 60 / DriveConstants.driveWheelGearReduction; } public void setRotationVoltage(double volts) {