Skip to content

Commit

Permalink
WORKING - TO CLEAN UP - velocity is working, steering needs turning
Browse files Browse the repository at this point in the history
  • Loading branch information
@MichaelLesirge
MichaelLesirge committed Jan 20, 2024
1 parent f39b864 commit 1051983
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 33 deletions.
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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
Expand All @@ -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;


Expand All @@ -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;
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/commands/AutoDriveForward.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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,
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down
47 changes: 25 additions & 22 deletions src/main/java/frc/robot/subsystems/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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(
Expand All @@ -104,32 +104,35 @@ 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.
*/
@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();
}
}
}
Expand Down

0 comments on commit 1051983

Please sign in to comment.