Skip to content

Commit

Permalink
fix drive for once
Browse files Browse the repository at this point in the history
  • Loading branch information
@amb2127
amb2127 committed Mar 3, 2024
1 parent 29cabc6 commit 4b7cb14
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 14 deletions.
15 changes: 8 additions & 7 deletions src/main/java/org/codeorange/frc2024/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import edu.wpi.first.math.util.Units;
import org.codeorange.frc2024.utility.MacAddressUtil;
import org.codeorange.frc2024.utility.MacAddressUtil.RobotIdentity;
import org.codeorange.frc2024.utility.swerve.SecondOrderModuleState;
import org.codeorange.frc2024.utility.swerve.SwerveSetpointGenerator;
import org.codeorange.frc2024.utility.swerve.SecondOrderKinematics;
import org.jetbrains.annotations.NotNull;
Expand Down Expand Up @@ -135,10 +136,10 @@ public static class Ports {

}
case COMPETITION_BOT -> {
FL_ABSOLUTE_ENCODER_OFFSET = -0.962158203125;
BL_ABSOLUTE_ENCODER_OFFSET = -0.38037109375;
FR_ABSOLUTE_ENCODER_OFFSET = -0.038330078125;
BR_ABSOLUTE_ENCODER_OFFSET = -0.699462890625;
FL_ABSOLUTE_ENCODER_OFFSET = -0.669677734375 + 0.5;
BL_ABSOLUTE_ENCODER_OFFSET = -0.02197265625 + 0.5;
FR_ABSOLUTE_ENCODER_OFFSET = -0.378662109375 + 0.5;
BR_ABSOLUTE_ENCODER_OFFSET = -0.949462890625 + 0.5;
WRIST_ABSOLUTE_ENCODER_OFFSET = -0.10498046875;
ARM_ABSOLUTE_ENCODER_OFFSET = 0.45068359375;
}
Expand Down Expand Up @@ -226,7 +227,7 @@ public static class Ports {
public static final double SS_HOMING_WRIST = isPrototype() ? 0 : 0;
public static final double SS_HOMING_CLIMBER = isPrototype() ? 0 : 0;

public static final double SWERVE_DRIVE_P = 50;
public static final double SWERVE_DRIVE_P = 30;
public static final double SWERVE_DRIVE_D = 0;
public static final double SWERVE_DRIVE_I = 0;

Expand Down Expand Up @@ -274,7 +275,7 @@ public static class Ports {
public static final double SWERVE_INCHES_PER_ROTATION = 2*Math.PI*SWERVE_WHEEL_RADIUS;
public static final double SWERVE_METER_PER_ROTATION = Units.inchesToMeters(SWERVE_INCHES_PER_ROTATION);
public static final boolean USE_SECOND_ORDER_KINEMATICS = false;
public static final double STEER_MOTOR_POSITION_CONVERSION_FACTOR = 1 / 12.8;
public static final double STEER_MOTOR_POSITION_CONVERSION_FACTOR = 7.0 / 150.0;
public static final double DRIVE_MOTOR_REDUCTION = 9 / 53.125;

public static final double wheelBaseInches = isPrototype() ? 22.75 : 24.25; // not real number, just example
Expand All @@ -289,7 +290,7 @@ public static class Ports {
SWERVE_RIGHT_BACK_LOCATION
};
// really, figure out if these locations are correct <_<
public static final SwerveDriveKinematics SWERVE_DRIVE_KINEMATICS = new SwerveDriveKinematics(
public static final SecondOrderKinematics SWERVE_DRIVE_KINEMATICS = new SecondOrderKinematics(
SWERVE_MODULE_LOCATIONS
);

Expand Down
14 changes: 7 additions & 7 deletions src/main/java/org/codeorange/frc2024/subsystem/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,11 +188,11 @@ private void setMotorSpeed(int module, double velocity, double acceleration, boo
SwerveModuleState[] wantedStates = new SwerveModuleState[4];
SwerveModuleState[] realStates = new SwerveModuleState[4];

private synchronized void setSwerveModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) {
private synchronized void setSwerveModuleStates(SecondOrderModuleState[] swerveModuleStates, boolean isOpenLoop) {
for (int i = 0; i < 4; i++) {
var moduleState = swerveModuleStates[i];
moduleState = SecondOrderModuleState.optimize(moduleState, Rotation2d.fromDegrees(getWheelRotation(i)));
wantedStates[i] = swerveModuleStates[i];
wantedStates[i] = swerveModuleStates[i].toFirstOrder();

moduleIO[i].setSteerMotorPosition(moduleState.angle.getDegrees());
setMotorSpeed(i, moduleState.speedMetersPerSecond, 0, isOpenLoop);
Expand All @@ -209,7 +209,7 @@ private synchronized void setSwerveModuleStates(SwerveModuleState[] swerveModule
}

public synchronized void drive(@NotNull ControllerDriveInputs inputs, boolean fieldRel, boolean openLoop) {
SwerveModuleState[] states =
SecondOrderModuleState[] states =
SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(
ChassisSpeeds.discretize(fieldRel ?
ChassisSpeeds.fromFieldRelativeSpeeds(
Expand All @@ -222,21 +222,21 @@ public synchronized void drive(@NotNull ControllerDriveInputs inputs, boolean fi
DRIVE_HIGH_SPEED_M * inputs.getY(),
inputs.getRotation() * MAX_TELEOP_TURN_SPEED), 0.02
));
SwerveDriveKinematics.desaturateWheelSpeeds(states, DRIVE_HIGH_SPEED_M);
SecondOrderKinematics.desaturateWheelSpeeds(states, DRIVE_HIGH_SPEED_M);

setSwerveModuleStates(states, openLoop);
}

public void swerveDriveTargetAngle(@NotNull ControllerDriveInputs inputs, double targetAngleRad) {
double turn = turnPID.calculate(gyroInputs.yawPositionRad, targetAngleRad);
Logger.recordOutput("Drive/Wanted Omega", turn);
SwerveModuleState[] states = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(
SecondOrderModuleState[] states = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(
ChassisSpeeds.discretize(
ChassisSpeeds.fromFieldRelativeSpeeds(DRIVE_HIGH_SPEED_M * inputs.getX(),
DRIVE_HIGH_SPEED_M * inputs.getY(),
-turn,
gyroInputs.rotation2d), 0.02));
SwerveDriveKinematics.desaturateWheelSpeeds(states, DRIVE_HIGH_SPEED_M);
SecondOrderKinematics.desaturateWheelSpeeds(states, DRIVE_HIGH_SPEED_M);
setSwerveModuleStates(states, true);
}

Expand Down Expand Up @@ -312,7 +312,7 @@ public Pose2d getPose() {

public void setNextChassisSpeeds(ChassisSpeeds nextChassisSpeeds) {
var states = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(ChassisSpeeds.discretize(nextChassisSpeeds, 0.02));
SwerveDriveKinematics.desaturateWheelSpeeds(states, DRIVE_HIGH_SPEED_M);
SecondOrderKinematics.desaturateWheelSpeeds(states, DRIVE_HIGH_SPEED_M);
setSwerveModuleStates(states, false);
}
}
Expand Down

0 comments on commit 4b7cb14

Please sign in to comment.