diff --git a/src/main/java/com/team2502/robot2023/commands/DriveCommand.java b/src/main/java/com/team2502/robot2023/commands/DriveCommand.java index 77d4469..93bb5f8 100644 --- a/src/main/java/com/team2502/robot2023/commands/DriveCommand.java +++ b/src/main/java/com/team2502/robot2023/commands/DriveCommand.java @@ -40,6 +40,9 @@ private enum Drivetype { private double dlxDrift; private double dlyDrift; + private boolean tgoggla = false; + private boolean prevTog = false; + private final SendableChooser typeEntry = new SendableChooser<>(); private final SendableChooser controllerEntry = new SendableChooser<>(); @@ -71,6 +74,14 @@ public void execute() { resetDrift(); } + boolean tog = leftJoystick.getRawButton(OI.RET_MODE); + if (prevTog != tog && tog) { + tgoggla = !tgoggla; + } + prevTog = tog; + + SmartDashboard.putBoolean("iosajioj", tgoggla); + ChassisSpeeds speeds; Translation2d centerOfRotation; drivetrain.setTurnNeutralMode(NeutralMode.Brake); @@ -106,9 +117,9 @@ public void execute() { break; case FieldOrientedTwist: speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - leftJoystick.getY() * (leftJoystick.getRawButton(OI.RET_MODE) ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), - -leftJoystick.getX() * (leftJoystick.getRawButton(OI.RET_MODE) ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), - -rightJoystick.getZ() * (leftJoystick.getRawButton(OI.RET_MODE) ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT), + leftJoystick.getY() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), + -leftJoystick.getX() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), + -rightJoystick.getZ() * (tgoggla ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT), Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset)); centerOfRotation = new Translation2d(0, 0); drivetrain.setSpeeds(speeds, centerOfRotation); diff --git a/src/main/java/com/team2502/robot2023/subsystems/DrivetrainSubsystem.java b/src/main/java/com/team2502/robot2023/subsystems/DrivetrainSubsystem.java index f2ac661..e32e8a7 100644 --- a/src/main/java/com/team2502/robot2023/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/com/team2502/robot2023/subsystems/DrivetrainSubsystem.java @@ -21,17 +21,15 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.team2502.robot2023.Utils; import com.team2502.robot2023.Constants.HardwareMap; import com.team2502.robot2023.Constants.Subsystems.Field; import com.team2502.robot2023.Constants.Subsystems.Drivetrain; import com.team2502.robot2023.Constants.Subsystems.Drivetrain.*; - -import java.lang.Math; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class DrivetrainSubsystem extends SubsystemBase{ +public class DrivetrainSubsystem extends SubsystemBase { private SwerveDriveKinematics kinematics; private SwerveDriveOdometry odometry; @@ -50,10 +48,10 @@ public class DrivetrainSubsystem extends SubsystemBase{ private CANSparkMax drivetrainEncoderBackRight; private CANSparkMax drivetrainEncoderFrontRight;*/ - private CANCoder drivetrainEncoderBackLeft; + /*private CANCoder drivetrainEncoderBackLeft; private CANCoder drivetrainEncoderFrontLeft; private CANCoder drivetrainEncoderBackRight; - private CANCoder drivetrainEncoderFrontRight; + private CANCoder drivetrainEncoderFrontRight;*/ private Field2d field; @@ -93,10 +91,10 @@ public DrivetrainSubsystem(){ drivetrainTurnFrontRight = new WPI_TalonFX(HardwareMap.FR_TURN_MOTOR, "can0"); drivetrainTurnBackRight = new WPI_TalonFX(HardwareMap.BR_TURN_MOTOR, "can0"); - drivetrainEncoderBackLeft = new CANCoder(HardwareMap.BL_TURN_ENCODER, "can0"); + /*drivetrainEncoderBackLeft = new CANCoder(HardwareMap.BL_TURN_ENCODER, "can0"); drivetrainEncoderFrontLeft = new CANCoder(HardwareMap.FL_TURN_ENCODER, "can0"); drivetrainEncoderFrontRight = new CANCoder(HardwareMap.FR_TURN_ENCODER, "can0"); - drivetrainEncoderBackRight = new CANCoder(HardwareMap.BR_TURN_ENCODER, "can0"); + drivetrainEncoderBackRight = new CANCoder(HardwareMap.BR_TURN_ENCODER, "can0");*/ Translation2d m_frontLeftLocation = new Translation2d(Drivetrain.SWERVE_WIDTH, -Drivetrain.SWERVE_LENGTH); Translation2d m_frontRightLocation = new Translation2d(Drivetrain.SWERVE_WIDTH, Drivetrain.SWERVE_LENGTH); @@ -185,6 +183,7 @@ public Pose2d reflectPose(Pose2d input) { * call once after aligning turn motors after motor rebuild * ensures that swerve zeros persist across restarts */ + /* public void setSwerveInit() { drivetrainEncoderFrontLeft.configMagnetOffset( drivetrainEncoderFrontLeft.getAbsolutePosition()); @@ -201,20 +200,24 @@ public void setSwerveInit() { drivetrainEncoderBackRight.configMagnetOffset( drivetrainEncoderBackRight.getAbsolutePosition()); drivetrainEncoderBackRight.setPositionToAbsolute(); - } + }*/ private SwerveModulePosition[] getModulePositions() { Rotation2d FLRotation = Rotation2d.fromDegrees( - drivetrainEncoderFrontLeft.getPosition() + //drivetrainEncoderFrontLeft.getPosition() + drivetrainTurnFrontLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES ); Rotation2d FRRotation = Rotation2d.fromDegrees( - drivetrainEncoderFrontRight.getPosition() + //drivetrainEncoderFrontRight.getPosition() + drivetrainTurnFrontRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES ); Rotation2d BLRotation = Rotation2d.fromDegrees( - drivetrainEncoderBackLeft.getPosition() + //drivetrainEncoderBackLeft.getPosition() + drivetrainTurnBackLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES ); Rotation2d BRRotation = Rotation2d.fromDegrees( - drivetrainEncoderBackRight.getPosition() + //drivetrainEncoderBackRight.getPosition() + drivetrainTurnBackRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES ); SwerveModulePosition FRPosition = new SwerveModulePosition( @@ -300,23 +303,23 @@ public void setSpeeds(ChassisSpeeds speed, Translation2d centerOfRotation) { Rotation2d FLRotation = Rotation2d.fromDegrees( //drivetrainEncoderFrontLeft.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360 - //drivetrainTurnFrontLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES - -drivetrainEncoderFrontLeft.getPosition() + drivetrainTurnFrontLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES + //-drivetrainEncoderFrontLeft.getPosition() ); Rotation2d FRRotation = Rotation2d.fromDegrees( //drivetrainEncoderFrontRight.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360 - //drivetrainTurnFrontRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES - -drivetrainEncoderFrontRight.getPosition() + drivetrainTurnFrontRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES + //-drivetrainEncoderFrontRight.getPosition() ); Rotation2d BLRotation = Rotation2d.fromDegrees( //drivetrainEncoderBackLeft.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360 - //drivetrainTurnBackLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES - -drivetrainEncoderBackLeft.getPosition() + drivetrainTurnBackLeft.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES + //-drivetrainEncoderBackLeft.getPosition() ); Rotation2d BRRotation = Rotation2d.fromDegrees( //drivetrainEncoderBackRight.getAlternateEncoder(Drivetrain.SWERVE_ENCODER_COUNTS_PER_REV).getPosition()/360 - //drivetrainTurnBackRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES - -drivetrainEncoderBackRight.getPosition() + drivetrainTurnBackRight.getSelectedSensorPosition() * Drivetrain.SWERVE_FALCON_ENCODER_COUNTS_TO_DEGREES + //-drivetrainEncoderBackRight.getPosition() ); SwerveModuleState FLState = SwerveModuleState.optimize(moduleStates[0], FLRotation); @@ -459,12 +462,13 @@ public double getMaxTemp() { return Math.max(Math.max(fl, fr), Math.max(bl, br)); } + /* public void zeroTurn() { drivetrainEncoderFrontRight.setPosition(0); drivetrainEncoderFrontLeft.setPosition(0); drivetrainEncoderBackRight.setPosition(0); drivetrainEncoderBackLeft.setPosition(0); - } + }*/ @Override public void periodic(){