Skip to content

Commit

Permalink
Add sysid values, add drive motor gearing
Browse files Browse the repository at this point in the history
  • Loading branch information
Prog694 committed Nov 29, 2023
1 parent 5dd50a0 commit 681ea02
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 16 deletions.
33 changes: 22 additions & 11 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,26 +31,26 @@ public interface Swerve {
double LENGTH = Units.inchesToMeters(20.508); // temp for simulatoin

SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02);
SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed (rad per s)", 5); // Ask Harry for max theoretical speed
SmartNumber MAX_MODULE_TURN = new SmartNumber("Swerve/Maximum module turn (m per s)", 6.28); // Ask Harry for max theoretical turn
SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed (m per s)", 5.06);
SmartNumber MAX_MODULE_TURN = new SmartNumber("Swerve/Maximum module turn (rad per s)", 6.28); // Ask Harry for max theoretical turn

public interface Turn {
SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 0.5);
SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 6.0);
SmartNumber kI = new SmartNumber("Swerve/Turn/kI", 0.0);
SmartNumber kD = new SmartNumber("Swerve/Turn/kD", 0.05);
SmartNumber kD = new SmartNumber("Swerve/Turn/kD", 0.15);

SmartNumber kV = new SmartNumber("Swerve/Turn/kV", 0.0);
SmartNumber kA = new SmartNumber("Swerve/Turn/kA", 0.0);
}

public interface Drive {
SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 0.1);
SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 0.018327);
SmartNumber kI = new SmartNumber("Swerve/Drive/kI", 0.0);
SmartNumber kD = new SmartNumber("Swerve/Drive/kD", 0.0);

SmartNumber kS = new SmartNumber("Swerve/Drive/kV", 0);
SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 0);
SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0);
SmartNumber kS = new SmartNumber("Swerve/Drive/kS", 0.098993);
SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 2.4495);
SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.089872);
}

public interface Motion {
Expand Down Expand Up @@ -86,14 +86,25 @@ public interface BackRight {
// .plus(Rotation2d.fromDegrees(90));
Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5);
}

public interface Encoder {
public interface Drive {
double WHEEL_DIAMETER = Units.inchesToMeters(4);
double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
double GEAR_RATIO = 1.0 / 6.12;

double POSITION_CONVERSION = WHEEL_CIRCUMFERENCE * GEAR_RATIO;
double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0;
}
}
}

public interface Driver {
public interface Drive {
SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03);

SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2);
SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 1.5);
SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.1);
SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2);

SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get());
SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 15);
Expand All @@ -102,7 +113,7 @@ public interface Drive {
public interface Turn {
SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05);

SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15);
SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.1);
SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2);

SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 6.0);
Expand Down
12 changes: 7 additions & 5 deletions src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModuleImpl.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import com.stuypulse.robot.Robot.MatchState;
import com.stuypulse.robot.constants.Settings.Swerve;
import com.stuypulse.robot.constants.Settings.Swerve.Drive;
import com.stuypulse.robot.constants.Settings.Swerve.Encoder;
import com.stuypulse.robot.constants.Settings.Swerve.Turn;
import com.stuypulse.stuylib.control.Controller;
import com.stuypulse.stuylib.control.angle.AngleController;
Expand Down Expand Up @@ -53,13 +54,17 @@ public SwerveModuleImpl(String id, Translation2d translationOffset, Rotation2d a

turnEncoder = new CANCoder(encoderID);
driveEncoder = driveMotor.getEncoder();

driveEncoder.setPositionConversionFactor(Encoder.Drive.POSITION_CONVERSION);
driveEncoder.setVelocityConversionFactor(Encoder.Drive.VELOCITY_CONVERSION);

driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD)
.setOutputFilter(x -> Robot.getMatchState() == MatchState.TELEOP ? 0 : x)
.add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity());

turnController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD)
.setSetpointFilter(new ARateLimit(Swerve.MAX_MODULE_TURN));
.setSetpointFilter(new ARateLimit(Swerve.MAX_MODULE_TURN))
.setOutputFilter(x -> -x);

targetState = new SwerveModuleState();
}
Expand Down Expand Up @@ -98,10 +103,7 @@ public void periodic() {
Angle.fromRotation2d(targetState.angle),
Angle.fromRotation2d(getAngle()));

// if (turnController.isDoneDegrees(10))
// turnMotor.setVoltage(0);
// else
turnMotor.setVoltage(turnController.getOutput());
turnMotor.setVoltage(turnController.getOutput());

driveMotor.setVoltage(driveController.update(
targetState.speedMetersPerSecond,
Expand Down

0 comments on commit 681ea02

Please sign in to comment.