Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

auto tuning + inverted DT sides

Browse Source
This commit is contained in:
Bradley Bickford 2024-03-30 20:04:59 -04:00
parent 57e308f050
commit cbf973e946
5 changed files with 36 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/deploy/pathplanner/paths/Center Sub to Note 2.path
View File

@ -8,19 +8,19 @@
},
"prevControl": null,
"nextControl": {
"x": 2.3216735004898608,
"y": 5.541275082550166
"x": 1.7449183616315946,
"y": 5.55472336015897
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.289197721252586,
"x": 2.294543620426272,
"y": 5.541275082550166
},
"prevControl": {
"x": 1.2891977212525858,
"x": 1.2945436204262721,
"y": 5.541275082550166
},
"nextControl": null,

6
src/main/java/frc/robot/RobotContainer.java
View File

@ -133,7 +133,7 @@ public class RobotContainer {
1.0,
1.0
).raceWith(
new WaitCommand(0.5)
new WaitCommand(1.0)
).andThen(
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
@ -228,7 +228,7 @@ public class RobotContainer {
drivetrain.teleopCommand(
() -> { return driver.getLeftY() * (invertXYDrive ? -1 : 1); },
() -> { return driver.getLeftX() * (invertXYDrive ? -1 : 1); },
() -> { return -driver.getRightX() * (invertXYDrive ? -1 : 1); },
() -> { return driver.getRightX() * (invertXYDrive ? -1 : 1); },
OIConstants.kTeleopDriveDeadband
)
);
@ -451,6 +451,6 @@ public class RobotContainer {
}
public Command getAutonomousCommand() {
return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
return autoChooser.getSelected();
}
}

24
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -27,10 +27,10 @@ public final class DrivetrainConstants {
// Angular offsets of the modules relative to the chassis in radians
public static final double kFrontLeftChassisAngularOffset = Math.PI;
public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
public static final double kBackRightChassisAngularOffset = 0;
public static final double kFrontLeftChassisAngularOffset = 0.0;// Math.PI
public static final double kFrontRightChassisAngularOffset = Math.PI / 2;//-Math.PI / 2;
public static final double kBackLeftChassisAngularOffset = -Math.PI/2;//Math.PI / 2;
public static final double kBackRightChassisAngularOffset = Math.PI; //0;
/*
public static final double kFrontLeftChassisAngularOffset = -2*Math.PI;
@ -40,15 +40,15 @@ public final class DrivetrainConstants {
*/
// SPARK MAX CAN IDs
public static final int kFrontLeftDrivingCanId = 8;
public static final int kRearLeftDrivingCanId = 3;
public static final int kFrontRightDrivingCanId = 6;
public static final int kRearRightDrivingCanId = 1;
public static final int kFrontLeftDrivingCanId = 1;//8
public static final int kRearLeftDrivingCanId = 6;//3
public static final int kFrontRightDrivingCanId = 3;//6
public static final int kRearRightDrivingCanId = 8;//1
public static final int kFrontLeftTurningCanId = 4;
public static final int kRearLeftTurningCanId = 7;
public static final int kFrontRightTurningCanId = 2;
public static final int kRearRightTurningCanId = 5;
public static final int kFrontLeftTurningCanId = 5;//4
public static final int kRearLeftTurningCanId = 2;//7
public static final int kFrontRightTurningCanId = 7;//2
public static final int kRearRightTurningCanId = 4;//5
public static final double kTurnToleranceDeg = 0;
public static final double kTurnRateToleranceDegPerS = 0;

4
src/main/java/frc/robot/subsystems/Climber.java
View File

@ -29,10 +29,12 @@ public class Climber extends SubsystemBase {
public Command setSpeedWithSupplier(DoubleSupplier speed) {
return run(() -> {
if(shooterAngle.getAsDouble() <= ShooterConstants.kShooterLoadAngle){
if(shooterAngle.getAsDouble() > ShooterConstants.kShooterLoadAngle){
motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
}else{
motor1.set(VictorSPXControlMode.PercentOutput, 0.0);
}
});
}

18
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -207,6 +207,8 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_gyro.setAngleAdjustment(pose.getRotation().getDegrees());
m_poseEstimator.resetPosition(
Rotation2d.fromDegrees(getGyroAngle()),
new SwerveModulePosition[] {
@ -304,21 +306,29 @@ public class Drivetrain extends SubsystemBase {
}
public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
//speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
setModuleStates(newStates);
/*
//speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_rearLeft.setDesiredState(states[2]);
m_rearRight.setDesiredState(states[3]);
*/
}
public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) {
return run(() -> {
drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband),
MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
() -> fieldRelativeControl,
true
);