From 105198377a226a2c0ff1a571ecf7f77e2ef6a192 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:55:34 -0800 Subject: [PATCH] WORKING - TO CLEAN UP - velocity is working, steering needs turning --- src/main/java/frc/robot/Constants.java | 8 ++-- .../frc/robot/commands/AutoDriveForward.java | 2 - .../SwerveDriveJoystickControl.java | 8 ++-- .../SwerveDrivePS4Control.java | 1 - .../frc/robot/subsystems/SwerveModule.java | 47 ++++++++++--------- 5 files changed, 33 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 89ddb4c..ac80d3e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,7 +23,7 @@ public static class DriverConstants { public static final double[] maxSpeedOptionsTranslation = {0.2, 2, 4}; // max angular velocity for drivetrain, in radians per second - public static final double[] maxSpeedOptionsRotation = {0.1, 0.2, 0.4}; + public static final double[] maxSpeedOptionsRotation = {0.4, 1, 3}; } public static class ControllerConstants { @@ -40,7 +40,7 @@ public static class OperatorConstants { public static class SwerveModuleConstants { // Values from https://www.swervedrivespecialties.com/products/mk4-swerve-module. We have L1 Modules. - public static final double DRIVE_MOTOR_GEAR_RATIO = 57 / 7; + public static final double DRIVE_MOTOR_GEAR_RATIO = 8.4; public static final double STEERING_MOTOR_GEAR_RATIO = 12.8; public static final double STEERING_ENCODER_SENSOR_COEFFICIENT = 0.000244140625; // if you put 1/4096 it just becomes zero @@ -49,7 +49,7 @@ public static class SwerveModuleConstants { public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER_METERS * Math.PI; // Other settings - public static final double MAX_SPEED_LIMIT = 0.2; + public static final double MAX_SPEED_LIMIT = 1; public static final double SWERVE_MODULE_DRIVE_COSIGN_SCALE = 0.1; @@ -59,7 +59,7 @@ public static class SwerveModuleConstants { public static final double STEERING_PID_D = 0; // Drive PID values - public static final double DRIVE_PID_P = 0.5; + public static final double DRIVE_PID_P = 0.3; public static final double DRIVE_PID_I = 0; public static final double DRIVE_PID_D = 0; public static final double DRIVE_PID_FF = 0; diff --git a/src/main/java/frc/robot/commands/AutoDriveForward.java b/src/main/java/frc/robot/commands/AutoDriveForward.java index 51c09d2..833643f 100644 --- a/src/main/java/frc/robot/commands/AutoDriveForward.java +++ b/src/main/java/frc/robot/commands/AutoDriveForward.java @@ -1,6 +1,4 @@ package frc.robot.commands; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.SwerveDrivetrain; diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java index 809f175..45a00e0 100644 --- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java +++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java @@ -18,7 +18,7 @@ public class SwerveDriveJoystickControl extends Command { private final OptionButton preciseModeButton; private final OptionButton boostModeButton; - private final OptionButton fieldRelieveButton; + private final OptionButton fieldRelativeButton; /** * Creates a new SwerveDriveJoystickControl Command. @@ -33,7 +33,7 @@ public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick d // Create and configure buttons preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE); boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD); - fieldRelieveButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE); + fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE); // Tell the command schedular we are using the drivetrain addRequirements(drivetrain); @@ -58,7 +58,7 @@ public void execute() { // Get joystick inputs final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE); final double speedY = -applyJoystickDeadzone(joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE); - final double speedR = applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE); + final double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE); // // Code for rotating with buttons if driver prefers // double speedOmega = 0; @@ -82,7 +82,7 @@ public void execute() { // Can be changed for testing final int speedCoefficient = 1; - final boolean isFieldRelative = fieldRelieveButton.getState(); + final boolean isFieldRelative = fieldRelativeButton.getState(); final ChassisSpeeds speeds = new ChassisSpeeds( speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel] * speedCoefficient, diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java index 7141d2a..d8679bc 100644 --- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java +++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java @@ -1,7 +1,6 @@ package frc.robot.commands.SwerveRemoteOperation; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import frc.robot.Constants.DriverConstants; diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index f7bb3d5..a07e0b6 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -28,7 +27,7 @@ public class SwerveModule extends SubsystemBase { // the drive motor is the motor that spins the wheel making the robot move across the ground (aka wheel or velocity motor) private final CANSparkMax driveMotor; private final RelativeEncoder driveEncoder; - private final SparkPIDController drivePIDController; + private final PIDController drivePIDController; // the steering motor is the motor that changes the rotation of the wheel allowing the robot to drive in any direction (aka spin or angular motor) // Also allows for spinning in place @@ -70,13 +69,11 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer driveEncoder.setVelocityConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO); // --- Drive PID --- - drivePIDController = driveMotor.getPIDController(); - drivePIDController.setP(SwerveModuleConstants.DRIVE_PID_P); - drivePIDController.setI(SwerveModuleConstants.DRIVE_PID_I); - drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D); - drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF); - drivePIDController.setIZone(SwerveModuleConstants.DRIVE_PID_IZone); - drivePIDController.setOutputRange(-SwerveModuleConstants.MAX_SPEED_LIMIT, SwerveModuleConstants.MAX_SPEED_LIMIT); + drivePIDController = new PIDController( + SwerveModuleConstants.DRIVE_PID_P, + SwerveModuleConstants.DRIVE_PID_I, + SwerveModuleConstants.DRIVE_PID_D + ); // --- Steering Motor --- steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless); @@ -85,7 +82,10 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer steeringEncoder = new CANcoder(steeringAbsoluteEncoderId); // Use 0-1 for rotational - steeringEncoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)); + steeringEncoder.getConfigurator().apply( + new MagnetSensorConfigs() + .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + ); // --- Steering PID --- steeringPIDController = new PIDController( @@ -104,6 +104,8 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer setName(toString()); } + int count = 10; + /** * This is the periodic function of the swerve module. * This method is called periodically by the CommandScheduler, about every 20ms. @@ -111,25 +113,26 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer @Override public void periodic() { if (desiredState != null) { - // Get real and desired angles of steering motor - // In pid the real value is often known as the measure or process variable, while the desired value is the setpoint or reference - final double measuredRotations = getState().angle.getRotations(); final double desiredRotations = desiredState.angle.getRotations(); + final double measuredRotations = getSteeringAngleRotations(); - // Use the steering motor pid controller to calculate speed to turn steering motor to get to desired angle final double steeringMotorSpeed = steeringPIDController.calculate(measuredRotations, desiredRotations); - // set steering motor to calculated value steeringMotor.set(steeringMotorSpeed); - - // the drive motor's PID controller is in RPM so we convert our value from Meters/Second to Rotations/Minute - // Use the drive motors built in pid controller to reach target velocity - if (desiredState.speedMetersPerSecond == 0) { - driveMotor.stopMotor(); + final double desiredDriveRotationsPerMinute = (desiredState.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE; + final double measuredDriveRotationsPerMinute = getDriveSpeedRotationsPerMinute(); + + count++; + if (desiredDriveRotationsPerMinute != 0) { + // final double driveMotorSpeed = drivePIDController.calculate(measuredDriveRotationsPerMinute, desiredDriveRotationsPerMinute); + // if (getName().equals("RightFrontSwerveModule") && count % 100 == 0) { + // System.out.println("Desired=" + desiredDriveRotationsPerMinute + " Measured=" + measuredDriveRotationsPerMinute + " Output=" + driveMotorSpeed); + // } + // driveMotor.set(driveMotorSpeed); + driveMotor.set(desiredState.speedMetersPerSecond / 4); } else { - final double driveVelocityRotationsPerMinute = (desiredState.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE; - drivePIDController.setReference(driveVelocityRotationsPerMinute, CANSparkMax.ControlType.kVelocity); + driveMotor.stopMotor(); } } }