From a518d76def30d06ebb6a0882c733b212a04059ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=87***=20F***?= <61086421+MorphaxTheDeveloper@users.noreply.github.com> Date: Thu, 7 Mar 2024 15:56:18 +0300 Subject: [PATCH] format --- src/main/java/frc/robot/Constants.java | 242 +++++++++--------- src/main/java/frc/robot/Robot.java | 23 +- src/main/java/frc/robot/RobotContainer.java | 238 +++++++++-------- .../commands/Intake/PidIntakeCommand.java | 23 +- .../commands/Intake/ResetIntakeEncoder.java | 19 +- .../frc/robot/commands/Shooter/PidSetRpm.java | 11 +- .../robot/commands/Shooter/PidSetRpm2.java | 16 +- .../commands/Shooter/ResetShooterEncoder.java | 10 +- .../commands/Shooter/ShooterSetDegree.java | 12 +- .../commands/Swerve/SwerveJoystickCmd.java | 100 ++++---- .../commands/common/FeedingPosition.java | 29 +-- .../frc/robot/commands/common/cinarcan.java | 9 +- .../frc/robot/commands/common/dalhacan.java | 4 +- .../frc/robot/subsystems/FeederSubsystem.java | 59 ++--- .../frc/robot/subsystems/IntakeSubsystem.java | 121 ++++----- .../robot/subsystems/ShooterSubsystem.java | 137 +++++----- .../frc/robot/subsystems/SwerveModule.java | 93 ++++--- .../frc/robot/subsystems/SwerveSubsystem.java | 178 +++++++------ 18 files changed, 660 insertions(+), 664 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3285004..996f068 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,8 +7,8 @@ import edu.wpi.first.math.util.Units; public final class Constants { -//bura acil konf - public static class ModuleConstants{ + // bura acil konf + public static class ModuleConstants { public static final double kWheelDiameterMeters = Units.inchesToMeters(4); public static final double kDriveMotorGearRatio = 1 / 6.75; @@ -20,98 +20,93 @@ public static class ModuleConstants{ public static final double kPTurning = 0.5; } - //bura acil konf - public static final class DriveConstants { - - public static final double kTrackWidth = Units.inchesToMeters(23); - // Distance between right and left wheels - public static final double kWheelBase = Units.inchesToMeters(23); - // Distance between front and back wheels - public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2)); - - //burası tamam - public static final int kFrontLeftDriveMotorPort = 41; - public static final int kBackLeftDriveMotorPort = 3; - public static final int kFrontRightDriveMotorPort = 10; - public static final int kBackRightDriveMotorPort = 11; - - public static final int kFrontLeftTurningMotorPort = 2; - public static final int kBackLeftTurningMotorPort = 1; - public static final int kFrontRightTurningMotorPort = 8; - public static final int kBackRightTurningMotorPort = 9; - - //buraya kadar - - public static final boolean kFrontLeftTurningEncoderReversed = false; - public static final boolean kBackLeftTurningEncoderReversed = false; - public static final boolean kFrontRightTurningEncoderReversed = false; - public static final boolean kBackRightTurningEncoderReversed = false; - - public static final boolean kFrontLeftDriveEncoderReversed = true; - public static final boolean kBackLeftDriveEncoderReversed = true; - public static final boolean kFrontRightDriveEncoderReversed = false; - public static final boolean kBackRightDriveEncoderReversed = false; - - public static final int kFrontLeftDriveAbsoluteEncoderPort = 2; - public static final int kBackLeftDriveAbsoluteEncoderPort = 4; - public static final int kFrontRightDriveAbsoluteEncoderPort = 3; - public static final int kBackRightDriveAbsoluteEncoderPort = 1; - - - - public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = true; - public static final boolean kBackLeftDriveAbsoluteEncoderReversed = true; - public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; - public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; - - public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -0.4101; - public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -0.1276; - public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -0.216064; - public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -0.126221; - - public static final double kPhysicalMaxSpeedMetersPerSecond = 3; - public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = 4 * 2 * Math.PI; - - public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; - public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = // - kPhysicalMaxAngularSpeedRadiansPerSecond / 4; - public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 10; - public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 10; - } - - //bura acil - public static final class OIConstants { - public static final int kDriverControllerPort = 0; - public static final int kDriverYAxis = 1; - public static final int kDriverXAxis = 0; - public static final int kDriverRotAxis = 4; - public static final int kDriverFieldOrientedButtonIdx = 1; - - public static final double kDeadband = 0.05; + // bura acil konf + public static final class DriveConstants { + + public static final double kTrackWidth = Units.inchesToMeters(23); + // Distance between right and left wheels + public static final double kWheelBase = Units.inchesToMeters(23); + // Distance between front and back wheels + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(kWheelBase / 2, kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackWidth / 2), + new Translation2d(-kWheelBase / 2, kTrackWidth / 2)); + + // burası tamam + public static final int kFrontLeftDriveMotorPort = 41; + public static final int kBackLeftDriveMotorPort = 3; + public static final int kFrontRightDriveMotorPort = 10; + public static final int kBackRightDriveMotorPort = 11; + + public static final int kFrontLeftTurningMotorPort = 2; + public static final int kBackLeftTurningMotorPort = 1; + public static final int kFrontRightTurningMotorPort = 8; + public static final int kBackRightTurningMotorPort = 9; + + // buraya kadar + + public static final boolean kFrontLeftTurningEncoderReversed = false; + public static final boolean kBackLeftTurningEncoderReversed = false; + public static final boolean kFrontRightTurningEncoderReversed = false; + public static final boolean kBackRightTurningEncoderReversed = false; + + public static final boolean kFrontLeftDriveEncoderReversed = true; + public static final boolean kBackLeftDriveEncoderReversed = true; + public static final boolean kFrontRightDriveEncoderReversed = false; + public static final boolean kBackRightDriveEncoderReversed = false; + + public static final int kFrontLeftDriveAbsoluteEncoderPort = 2; + public static final int kBackLeftDriveAbsoluteEncoderPort = 4; + public static final int kFrontRightDriveAbsoluteEncoderPort = 3; + public static final int kBackRightDriveAbsoluteEncoderPort = 1; + + public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = true; + public static final boolean kBackLeftDriveAbsoluteEncoderReversed = true; + public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; + public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; + + public static final double kFrontLeftDriveAbsoluteEncoderOffsetRad = -0.4101; + public static final double kBackLeftDriveAbsoluteEncoderOffsetRad = -0.1276; + public static final double kFrontRightDriveAbsoluteEncoderOffsetRad = -0.216064; + public static final double kBackRightDriveAbsoluteEncoderOffsetRad = -0.126221; + + public static final double kPhysicalMaxSpeedMetersPerSecond = 3; + public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = 4 * 2 * Math.PI; + + public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4; + public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = // + kPhysicalMaxAngularSpeedRadiansPerSecond / 4; + public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 10; + public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 10; } + // bura acil + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + public static final int kDriverYAxis = 1; + public static final int kDriverXAxis = 0; + public static final int kDriverRotAxis = 4; + public static final int kDriverFieldOrientedButtonIdx = 1; + public static final double kDeadband = 0.05; + } public static final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4; - public static final double kMaxAngularSpeedRadiansPerSecond = // - DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4; - public static final double kPXController = 1.5; - public static final double kPYController = 1.5; - public static final double kPThetaController = 3; - - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = // - new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, - kMaxAngularAccelerationRadiansPerSecondSquared); - } - + public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4; + public static final double kMaxAngularSpeedRadiansPerSecond = // + DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10; + public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4; + public static final double kPXController = 1.5; + public static final double kPYController = 1.5; + public static final double kPThetaController = 3; + + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = // + new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, + kMaxAngularAccelerationRadiansPerSecondSquared); + } public static class ports { public static final int intake_motor_note = 0; @@ -126,54 +121,51 @@ public static class controller { public static final int controller = 1; } -/* - ___ __ _ _ _ _____ _ _ - / __|___ _ _ / _(_)__ _ _ _ _ _ __ _| |_(_)___ _ _ |_ _|_ _| |__| |___ -| (__/ _ \ ' \| _| / _` | || | '_/ _` | _| / _ \ ' \ | |/ _` | '_ \ / -_) - \___\___/_||_|_| |_\__, |\_,_|_| \__,_|\__|_\___/_||_| |_|\__,_|_.__/_\___| - |___/ - - */ + /* + * ___ __ _ _ _ _____ _ _ + * / __|___ _ _ / _(_)__ _ _ _ _ _ __ _| |_(_)___ _ _ |_ _|_ _| |__| |___ + * | (__/ _ \ ' \| _| / _` | || | '_/ _` | _| / _ \ ' \ | |/ _` | '_ \ / -_) + * \___\___/_||_|_| |_\__, |\_,_|_| \__,_|\__|_\___/_||_| |_|\__,_|_.__/_\___| + * |___/ + * + */ public static class values { public static class intake { - public static final double GetNoteValue = 1; - public static final double PidIntakeTolerance = 1; - public static final double PidIntakeKP = 1; - public static final double PidIntakeKI = 0; - public static final double PidIntakeKD = 0; -} - - + public static final double GetNoteValue = 1; + public static final double PidIntakeTolerance = 1; + public static final double PidIntakeKP = 1; + public static final double PidIntakeKI = 0; + public static final double PidIntakeKD = 0; + } -public static class shooter { - public static final double AngleMotorOpenLoopRampRate = 0.5; - public static final double PidShooterAngleTolerance = 0.1; - public static final double PidShooterAngleKP = 0.05; - public static final double PidShooterAngleKI = 0; - public static final double PidShooterAngleKD = 0; + public static class shooter { + public static final double AngleMotorOpenLoopRampRate = 0.5; + public static final double PidShooterAngleTolerance = 0.1; + public static final double PidShooterAngleKP = 0.05; + public static final double PidShooterAngleKI = 0; + public static final double PidShooterAngleKD = 0; - public static final double PidShooterShootKP = 1; - public static final double PidShooterShootKI = 0; - public static final double PidShooterShootKD = 0; - public static final double PidShooterRPMTolerance = 25; - -} + public static final double PidShooterShootKP = 1; + public static final double PidShooterShootKI = 0; + public static final double PidShooterShootKD = 0; + public static final double PidShooterRPMTolerance = 25; + } -public static class positions { + public static class positions { - public static final double FeedingPositionIntake = 1; - public static final double FeedingPositionShooter = 1; + public static final double FeedingPositionIntake = 1; + public static final double FeedingPositionShooter = 1; -} + } -public static class feeder { + public static class feeder { - public static final double FeederForwardSpeed = 0.7; - public static final double FeederBackwardSpeed = -0.7; + public static final double FeederForwardSpeed = 0.7; + public static final double FeederBackwardSpeed = -0.7; -} + } } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a523782..cde5374 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.subsystems.ShooterSubsystem; - public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -21,20 +20,19 @@ public void robotInit() { m_robotContainer = new RobotContainer(); } - @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); } @Override - public void disabledInit() {} + public void disabledInit() { + } @Override public void disabledPeriodic() { - - } + } @Override public void autonomousInit() { @@ -46,7 +44,8 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void teleopInit() { @@ -56,7 +55,8 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override public void testInit() { @@ -64,11 +64,14 @@ public void testInit() { } @Override - public void testPeriodic() {} + public void testPeriodic() { + } @Override - public void simulationInit() {} + public void simulationInit() { + } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 33d599a..5ac4907 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,4 +1,5 @@ package frc.robot; + import java.util.List; import edu.wpi.first.math.controller.PIDController; @@ -32,115 +33,134 @@ import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.SwerveSubsystem; public class RobotContainer { - private final IntakeSubsystem m_intake = new IntakeSubsystem(); - private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); - private final XboxController driverJoytick = new XboxController(0); - private final FeederSubsystem m_feeder = new FeederSubsystem(); - - public RobotContainer() { - - swerveSubsystem.setDefaultCommand(new SwerveJoystickCmd( - swerveSubsystem, - () -> -driverJoytick.getRawAxis(OIConstants.kDriverYAxis), - () -> driverJoytick.getRawAxis(OIConstants.kDriverXAxis), - () -> driverJoytick.getRawAxis(OIConstants.kDriverRotAxis), - () -> !driverJoytick.getRawButton(OIConstants.kDriverFieldOrientedButtonIdx))); - - configureBindings(); - - } - - - - - private void configureBindings() { - - new JoystickButton(driverJoytick, 7).whileTrue(new InstantCommand(() -> swerveSubsystem.zeroHeading())); - //new JoystickButton(driverJoytick, 3).whileTrue(new PidIntakeCommand(m_intake, 20)); - //new JoystickButton(driverJoytick, 3).whileFalse(new InstantCommand(() -> m_intake.StopAngleMotor())); - //new JoystickButton(driverJoytick, 4).whileTrue(new InstantCommand(()-> m_intake.reset())); - //new JoystickButton(driverJoytick, 5).whileTrue(new InstantCommand(()-> m_shooter.AngleEncoderReset())); - //new JoystickButton(driverJoytick, 6).whileTrue(new InstantCommand(()-> m_feeder.backward())); - new JoystickButton(driverJoytick, 10).whileTrue(new InstantCommand(()->m_shooter.ShooterThrowMotorOutput(0.8))); - - new JoystickButton(driverJoytick, 10).whileFalse(new InstantCommand(()->m_shooter.ShooterThrowAllMotorStop())); - - new JoystickButton(driverJoytick, 1).whileTrue(new dalhacan(m_intake, m_shooter)); - new JoystickButton(driverJoytick, 1).whileTrue(new InstantCommand(()->m_intake.degistirmee())); - new JoystickButton(driverJoytick, 3).whileTrue(new ShooterSetDegree(m_shooter, 160.0)); - //new JoystickButton(driverJoytick, 4).whileTrue(new cinarcan(m_intake, m_feeder)); - new JoystickButton(driverJoytick, 4).whileTrue(new PidIntakeCommand(m_intake, 1.2)); - new JoystickButton(driverJoytick, 4).whileTrue(new InstantCommand(()-> m_intake.degistir())); - new JoystickButton(driverJoytick, 1).whileFalse(new InstantCommand(()->m_intake.StopAngleMotor())); - - new JoystickButton(driverJoytick, 2).whileTrue(new InstantCommand(()->m_intake.reset())); - new JoystickButton(driverJoytick, 2).whileTrue(new InstantCommand(()->m_shooter.AngleEncoderReset())); - - // new JoystickButton(driverJoytick, 1).whileTrue(new InstantCommand(()->m_shooter.AngleEncoderReset())); - //new JoystickButton(driverJoytick, 2).whileTrue(new IntakeInputPosition(m_intake)); - //new JoystickButton(driverJoytick, 3).whileTrue(new FeedingPosition(m_shooter,m_intake)); - // new JoystickButton(driverJoytick, 4).whileTrue(new RunCommand(()-> m_feeder.forward())); - // new JoystickButton(driverJoytick, 5).whileTrue(new RunCommand(()-> m_feeder.backward())); - // new JoystickButton(driverJoytick, 4).whileFalse(new RunCommand(()-> m_feeder.stop())); - new JoystickButton(driverJoytick, 9).whileTrue(new cinarcan(m_intake, m_feeder)); - new JoystickButton(driverJoytick, 9).whileFalse(new cinarcan(m_intake, m_feeder)); - new JoystickButton(driverJoytick, 5).whileTrue(new InstantCommand(()-> m_intake.runpickupmotorswitch(0.5))); - new JoystickButton(driverJoytick, 5).whileFalse(new InstantCommand(()-> m_intake.runpickupmotorswitch(0))); - new JoystickButton(driverJoytick, 5).onTrue(new InstantCommand(()-> m_intake.degistir())); - - new JoystickButton(driverJoytick, 6).whileTrue(new InstantCommand(()-> m_feeder.runtillswitch())); - new JoystickButton(driverJoytick, 6).whileFalse(new InstantCommand(()-> m_feeder.stop())); - new JoystickButton(driverJoytick, 8).whileTrue(new InstantCommand(()-> m_intake.runpickupmotor(0.8))); - new JoystickButton(driverJoytick, 8).whileFalse(new InstantCommand(()-> m_intake.runpickupmotor(0))); - new JoystickButton(driverJoytick, 10).whileTrue(new InstantCommand(()-> m_feeder.backward())); - new JoystickButton(driverJoytick, 10).whileFalse(new InstantCommand(()-> m_feeder.stop())); - } - - - - public Command getAutonomousCommand() { -/* // 1. Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) - .setKinematics(DriveConstants.kDriveKinematics); - - // 2. Generate trajectory - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(1, 0), - new Translation2d(1, -1)), - new Pose2d(2, -1, Rotation2d.fromDegrees(180)), - trajectoryConfig); - - - // 3. Define PID controllers for tracking trajectory - PIDController xController = new PIDController(AutoConstants.kPXController, 0, 0); - PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0); - ProfiledPIDController thetaController = new ProfiledPIDController( - AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - // 4. Construct command to follow trajectory - SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - trajectory, - swerveSubsystem::getPose, - DriveConstants.kDriveKinematics, - xController, - yController, - thetaController, - swerveSubsystem::setModuleStates, - swerveSubsystem); - - // 5. Add some init and wrap-up, and return everything - return new SequentialCommandGroup( - new InstantCommand(() -> swerveSubsystem.resetOdometry(trajectory.getInitialPose())), - swerveControllerCommand, - new InstantCommand(() -> swerveSubsystem.stopModules())); */ + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); + private final XboxController driverJoytick = new XboxController(0); + private final FeederSubsystem m_feeder = new FeederSubsystem(); + + public RobotContainer() { + + swerveSubsystem.setDefaultCommand(new SwerveJoystickCmd( + swerveSubsystem, + () -> -driverJoytick.getRawAxis(OIConstants.kDriverYAxis), + () -> driverJoytick.getRawAxis(OIConstants.kDriverXAxis), + () -> driverJoytick.getRawAxis(OIConstants.kDriverRotAxis), + () -> !driverJoytick.getRawButton(OIConstants.kDriverFieldOrientedButtonIdx))); + + configureBindings(); + + } + + private void configureBindings() { + + new JoystickButton(driverJoytick, 7).whileTrue(new InstantCommand(() -> swerveSubsystem.zeroHeading())); + // new JoystickButton(driverJoytick, 3).whileTrue(new PidIntakeCommand(m_intake, + // 20)); + // new JoystickButton(driverJoytick, 3).whileFalse(new InstantCommand(() -> + // m_intake.StopAngleMotor())); + // new JoystickButton(driverJoytick, 4).whileTrue(new InstantCommand(()-> + // m_intake.reset())); + // new JoystickButton(driverJoytick, 5).whileTrue(new InstantCommand(()-> + // m_shooter.AngleEncoderReset())); + // new JoystickButton(driverJoytick, 6).whileTrue(new InstantCommand(()-> + // m_feeder.backward())); + new JoystickButton(driverJoytick, 10) + .whileTrue(new InstantCommand(() -> m_shooter.ShooterThrowMotorOutput(0.8))); + + new JoystickButton(driverJoytick, 10) + .whileFalse(new InstantCommand(() -> m_shooter.ShooterThrowAllMotorStop())); + + new JoystickButton(driverJoytick, 1).whileTrue(new dalhacan(m_intake, m_shooter)); + new JoystickButton(driverJoytick, 1).whileTrue(new InstantCommand(() -> m_intake.degistirmee())); + new JoystickButton(driverJoytick, 3).whileTrue(new ShooterSetDegree(m_shooter, 160.0)); + // new JoystickButton(driverJoytick, 4).whileTrue(new cinarcan(m_intake, + // m_feeder)); + new JoystickButton(driverJoytick, 4).whileTrue(new PidIntakeCommand(m_intake, 1.2)); + new JoystickButton(driverJoytick, 4).whileTrue(new InstantCommand(() -> m_intake.degistir())); + new JoystickButton(driverJoytick, 1).whileFalse(new InstantCommand(() -> m_intake.StopAngleMotor())); + + new JoystickButton(driverJoytick, 2).whileTrue(new InstantCommand(() -> m_intake.reset())); + new JoystickButton(driverJoytick, 2).whileTrue(new InstantCommand(() -> m_shooter.AngleEncoderReset())); + + // new JoystickButton(driverJoytick, 1).whileTrue(new + // InstantCommand(()->m_shooter.AngleEncoderReset())); + // new JoystickButton(driverJoytick, 2).whileTrue(new + // IntakeInputPosition(m_intake)); + // new JoystickButton(driverJoytick, 3).whileTrue(new + // FeedingPosition(m_shooter,m_intake)); + // new JoystickButton(driverJoytick, 4).whileTrue(new RunCommand(()-> + // m_feeder.forward())); + // new JoystickButton(driverJoytick, 5).whileTrue(new RunCommand(()-> + // m_feeder.backward())); + // new JoystickButton(driverJoytick, 4).whileFalse(new RunCommand(()-> + // m_feeder.stop())); + new JoystickButton(driverJoytick, 9).whileTrue(new cinarcan(m_intake, m_feeder)); + new JoystickButton(driverJoytick, 9).whileFalse(new cinarcan(m_intake, m_feeder)); + new JoystickButton(driverJoytick, 5) + .whileTrue(new InstantCommand(() -> m_intake.runpickupmotorswitch(0.5))); + new JoystickButton(driverJoytick, 5) + .whileFalse(new InstantCommand(() -> m_intake.runpickupmotorswitch(0))); + new JoystickButton(driverJoytick, 5).onTrue(new InstantCommand(() -> m_intake.degistir())); + + new JoystickButton(driverJoytick, 6).whileTrue(new InstantCommand(() -> m_feeder.runtillswitch())); + new JoystickButton(driverJoytick, 6).whileFalse(new InstantCommand(() -> m_feeder.stop())); + new JoystickButton(driverJoytick, 8).whileTrue(new InstantCommand(() -> m_intake.runpickupmotor(0.8))); + new JoystickButton(driverJoytick, 8).whileFalse(new InstantCommand(() -> m_intake.runpickupmotor(0))); + new JoystickButton(driverJoytick, 10).whileTrue(new InstantCommand(() -> m_feeder.backward())); + new JoystickButton(driverJoytick, 10).whileFalse(new InstantCommand(() -> m_feeder.stop())); + } + + public Command getAutonomousCommand() { + /* + * // 1. Create trajectory settings + * TrajectoryConfig trajectoryConfig = new TrajectoryConfig( + * AutoConstants.kMaxSpeedMetersPerSecond, + * AutoConstants.kMaxAccelerationMetersPerSecondSquared) + * .setKinematics(DriveConstants.kDriveKinematics); + * + * // 2. Generate trajectory + * Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + * new Pose2d(0, 0, new Rotation2d(0)), + * List.of( + * new Translation2d(1, 0), + * new Translation2d(1, -1)), + * new Pose2d(2, -1, Rotation2d.fromDegrees(180)), + * trajectoryConfig); + * + * + * // 3. Define PID controllers for tracking trajectory + * PIDController xController = new PIDController(AutoConstants.kPXController, 0, + * 0); + * PIDController yController = new PIDController(AutoConstants.kPYController, 0, + * 0); + * ProfiledPIDController thetaController = new ProfiledPIDController( + * AutoConstants.kPThetaController, 0, 0, + * AutoConstants.kThetaControllerConstraints); + * thetaController.enableContinuousInput(-Math.PI, Math.PI); + * + * // 4. Construct command to follow trajectory + * SwerveControllerCommand swerveControllerCommand = new + * SwerveControllerCommand( + * trajectory, + * swerveSubsystem::getPose, + * DriveConstants.kDriveKinematics, + * xController, + * yController, + * thetaController, + * swerveSubsystem::setModuleStates, + * swerveSubsystem); + * + * // 5. Add some init and wrap-up, and return everything + * return new SequentialCommandGroup( + * new InstantCommand(() -> + * swerveSubsystem.resetOdometry(trajectory.getInitialPose())), + * swerveControllerCommand, + * new InstantCommand(() -> swerveSubsystem.stopModules())); + */ return null; - } } + } +} diff --git a/src/main/java/frc/robot/commands/Intake/PidIntakeCommand.java b/src/main/java/frc/robot/commands/Intake/PidIntakeCommand.java index c34dd00..fe48a5a 100644 --- a/src/main/java/frc/robot/commands/Intake/PidIntakeCommand.java +++ b/src/main/java/frc/robot/commands/Intake/PidIntakeCommand.java @@ -8,31 +8,30 @@ public class PidIntakeCommand extends PIDCommand { double tolerance = 5; - + public PidIntakeCommand(IntakeSubsystem m_intake, double position) { super( - + new PIDController(0.05, - Constants.values.intake.PidIntakeKI, - Constants.values.intake.PidIntakeKD), + Constants.values.intake.PidIntakeKI, + Constants.values.intake.PidIntakeKD), () -> m_intake.getRawEncoderOutput(), () -> position, - + output -> { if (position > -m_intake.getRawEncoderOutput()) { m_intake.NewIntakeMotorOutput(output * 0.8); - } - - else if (position < -m_intake.getRawEncoderOutput()) { + } + + else if (position < -m_intake.getRawEncoderOutput()) { m_intake.NewIntakeMotorOutput(-output * 0.8); } - + }); - addRequirements(m_intake); - getController().setTolerance(Constants.values.intake.PidIntakeTolerance); + addRequirements(m_intake); + getController().setTolerance(Constants.values.intake.PidIntakeTolerance); } - @Override public boolean isFinished() { return false; diff --git a/src/main/java/frc/robot/commands/Intake/ResetIntakeEncoder.java b/src/main/java/frc/robot/commands/Intake/ResetIntakeEncoder.java index 6031614..92e7321 100644 --- a/src/main/java/frc/robot/commands/Intake/ResetIntakeEncoder.java +++ b/src/main/java/frc/robot/commands/Intake/ResetIntakeEncoder.java @@ -1,25 +1,28 @@ package frc.robot.commands.Intake; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; public class ResetIntakeEncoder extends Command { - - IntakeSubsystem m_intake; - public ResetIntakeEncoder(IntakeSubsystem m_intake){ - this.m_intake = m_intake; - } + IntakeSubsystem m_intake; + + public ResetIntakeEncoder(IntakeSubsystem m_intake) { + this.m_intake = m_intake; + } @Override - public void initialize() {} - + public void initialize() { + } + @Override public void execute() { m_intake.reset(); } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/Shooter/PidSetRpm.java b/src/main/java/frc/robot/commands/Shooter/PidSetRpm.java index dbc7d08..23354af 100644 --- a/src/main/java/frc/robot/commands/Shooter/PidSetRpm.java +++ b/src/main/java/frc/robot/commands/Shooter/PidSetRpm.java @@ -5,17 +5,16 @@ import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; - public class PidSetRpm extends PIDCommand { public PidSetRpm(ShooterSubsystem m_shooter, double estimatedRpm) { super( new PIDController(Constants.values.shooter.PidShooterShootKP, - Constants.values.shooter.PidShooterShootKI, - Constants.values.shooter.PidShooterShootKD), + Constants.values.shooter.PidShooterShootKI, + Constants.values.shooter.PidShooterShootKD), () -> m_shooter.getRpmOutput1(), - + () -> estimatedRpm, output -> { @@ -24,11 +23,11 @@ public PidSetRpm(ShooterSubsystem m_shooter, double estimatedRpm) { m_shooter.ShooterThrow1MotorOutput(output); m_shooter.ShooterThrow2MotorOutput(output); - } + } }); - getController().setTolerance(Constants.values.shooter.PidShooterRPMTolerance); + getController().setTolerance(Constants.values.shooter.PidShooterRPMTolerance); } @Override diff --git a/src/main/java/frc/robot/commands/Shooter/PidSetRpm2.java b/src/main/java/frc/robot/commands/Shooter/PidSetRpm2.java index 47958da..6beb780 100644 --- a/src/main/java/frc/robot/commands/Shooter/PidSetRpm2.java +++ b/src/main/java/frc/robot/commands/Shooter/PidSetRpm2.java @@ -5,33 +5,31 @@ import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystem; - public class PidSetRpm2 extends PIDCommand { public PidSetRpm2(ShooterSubsystem m_shooter, double estimatedRpm) { super( new PIDController(Constants.values.shooter.PidShooterShootKP, - Constants.values.shooter.PidShooterShootKI, - Constants.values.shooter.PidShooterShootKD), + Constants.values.shooter.PidShooterShootKI, + Constants.values.shooter.PidShooterShootKD), () -> m_shooter.getRpmOutput2(), - + () -> estimatedRpm, output -> { if (estimatedRpm < m_shooter.getMappedOutput()) { m_shooter.ShooterThrow2MotorOutput(output); - } - - else if (estimatedRpm > m_shooter.getMappedOutput()) { + } + + else if (estimatedRpm > m_shooter.getMappedOutput()) { m_shooter.ShooterThrow2MotorOutput(-output); } - }); - getController().setTolerance(Constants.values.shooter.PidShooterRPMTolerance); + getController().setTolerance(Constants.values.shooter.PidShooterRPMTolerance); } @Override diff --git a/src/main/java/frc/robot/commands/Shooter/ResetShooterEncoder.java b/src/main/java/frc/robot/commands/Shooter/ResetShooterEncoder.java index 6ea5f95..cb60aa7 100644 --- a/src/main/java/frc/robot/commands/Shooter/ResetShooterEncoder.java +++ b/src/main/java/frc/robot/commands/Shooter/ResetShooterEncoder.java @@ -12,17 +12,17 @@ public ResetShooterEncoder(ShooterSubsystem m_shooter) { } @Override - public void initialize() {} - + public void initialize() { + } @Override public void execute() { - m_shooter.AngleEncoderReset(); + m_shooter.AngleEncoderReset(); } - @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/Shooter/ShooterSetDegree.java b/src/main/java/frc/robot/commands/Shooter/ShooterSetDegree.java index f94c164..c8e7ad1 100644 --- a/src/main/java/frc/robot/commands/Shooter/ShooterSetDegree.java +++ b/src/main/java/frc/robot/commands/Shooter/ShooterSetDegree.java @@ -10,18 +10,18 @@ public class ShooterSetDegree extends PIDCommand { public ShooterSetDegree(ShooterSubsystem m_shooter, Double angle) { super( - new PIDController(0.04, - 0.01, - Constants.values.shooter.PidShooterAngleKD), + new PIDController(0.04, + 0.01, + Constants.values.shooter.PidShooterAngleKD), () -> m_shooter.getMappedOutput(), () -> angle, output -> { - m_shooter.ShooterAngleMotorOutput(output*-.13); + m_shooter.ShooterAngleMotorOutput(output * -.13); }); - addRequirements(m_shooter); - getController().setTolerance(Constants.values.shooter.PidShooterAngleTolerance); + addRequirements(m_shooter); + getController().setTolerance(Constants.values.shooter.PidShooterAngleTolerance); } @Override diff --git a/src/main/java/frc/robot/commands/Swerve/SwerveJoystickCmd.java b/src/main/java/frc/robot/commands/Swerve/SwerveJoystickCmd.java index 1490d15..32ea21d 100644 --- a/src/main/java/frc/robot/commands/Swerve/SwerveJoystickCmd.java +++ b/src/main/java/frc/robot/commands/Swerve/SwerveJoystickCmd.java @@ -11,75 +11,73 @@ import frc.robot.subsystems.SwerveSubsystem; public class SwerveJoystickCmd extends Command { - + private final SwerveSubsystem swerveSubsystem; private final Supplier xSpdFunction, ySpdFunction, turningSpdFunction; private final Supplier fieldOrientedFunction; private final SlewRateLimiter xLimiter, yLimiter, turningLimiter; public SwerveJoystickCmd(SwerveSubsystem swerveSubsystem, - Supplier xSpdFunction, Supplier ySpdFunction, Supplier turningSpdFunction, - Supplier fieldOrientedFunction) { - - this.swerveSubsystem = swerveSubsystem; - this.xSpdFunction = xSpdFunction; - this.ySpdFunction = ySpdFunction; - this.turningSpdFunction = turningSpdFunction; - this.fieldOrientedFunction = fieldOrientedFunction; - this.xLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond); - this.yLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond); - this.turningLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAngularAccelerationUnitsPerSecond); - addRequirements(swerveSubsystem); + Supplier xSpdFunction, Supplier ySpdFunction, Supplier turningSpdFunction, + Supplier fieldOrientedFunction) { + + this.swerveSubsystem = swerveSubsystem; + this.xSpdFunction = xSpdFunction; + this.ySpdFunction = ySpdFunction; + this.turningSpdFunction = turningSpdFunction; + this.fieldOrientedFunction = fieldOrientedFunction; + this.xLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond); + this.yLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAccelerationUnitsPerSecond); + this.turningLimiter = new SlewRateLimiter(DriveConstants.kTeleDriveMaxAngularAccelerationUnitsPerSecond); + addRequirements(swerveSubsystem); } - @Override - public void initialize() {} - + public void initialize() { + } @Override public void execute() { - // 1. Get real-time joystick inputs - double xSpeed = xSpdFunction.get(); - double ySpeed = ySpdFunction.get(); - double turningSpeed = turningSpdFunction.get(); - - // 2. Apply deadband - xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; - ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; - turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; - - // 3. Make the driving smoother - xSpeed = xLimiter.calculate(xSpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - ySpeed = yLimiter.calculate(ySpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; - turningSpeed = turningLimiter.calculate(turningSpeed) - * DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond; - - // 4. Construct desired chassis speeds - ChassisSpeeds chassisSpeeds; - if (fieldOrientedFunction.get()) { - // Relative to field - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d()); - } else { - // Relative to robot - chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed); - } - - // 5. Convert chassis speeds to individual module states - SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - - // 6. Output each module states to wheels - swerveSubsystem.setModuleStates(moduleStates); + // 1. Get real-time joystick inputs + double xSpeed = xSpdFunction.get(); + double ySpeed = ySpdFunction.get(); + double turningSpeed = turningSpdFunction.get(); + + // 2. Apply deadband + xSpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? xSpeed : 0.0; + ySpeed = Math.abs(ySpeed) > OIConstants.kDeadband ? ySpeed : 0.0; + turningSpeed = Math.abs(turningSpeed) > OIConstants.kDeadband ? turningSpeed : 0.0; + + // 3. Make the driving smoother + xSpeed = xLimiter.calculate(xSpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + ySpeed = yLimiter.calculate(ySpeed) * DriveConstants.kTeleDriveMaxSpeedMetersPerSecond; + turningSpeed = turningLimiter.calculate(turningSpeed) + * DriveConstants.kTeleDriveMaxAngularSpeedRadiansPerSecond; + + // 4. Construct desired chassis speeds + ChassisSpeeds chassisSpeeds; + if (fieldOrientedFunction.get()) { + // Relative to field + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, turningSpeed, swerveSubsystem.getRotation2d()); + } else { + // Relative to robot + chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turningSpeed); + } + + // 5. Convert chassis speeds to individual module states + SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + + // 6. Output each module states to wheels + swerveSubsystem.setModuleStates(moduleStates); } - @Override - public void end(boolean interrupted) {} - + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/common/FeedingPosition.java b/src/main/java/frc/robot/commands/common/FeedingPosition.java index e29b20f..fa23f88 100644 --- a/src/main/java/frc/robot/commands/common/FeedingPosition.java +++ b/src/main/java/frc/robot/commands/common/FeedingPosition.java @@ -9,23 +9,22 @@ import frc.robot.subsystems.ShooterSubsystem; public class FeedingPosition extends SequentialCommandGroup { - public FeedingPosition(ShooterSubsystem m_shooter,FeederSubsystem m_feeder , IntakeSubsystem m_intake) { + public FeedingPosition(ShooterSubsystem m_shooter, FeederSubsystem m_feeder, IntakeSubsystem m_intake) { addCommands( - //1.8 - ( new PidIntakeCommand(m_intake, 1.2) - - //-3.8 - .andThen( - new InstantCommand(()->m_feeder.runtillswitch()).alongWith(new InstantCommand(() -> m_intake.NewIntakeMotorOutput(0.4))) - //ShooterSetDegree(m_shooter, -1.15) - - ) - - - ) - - + // 1.8 + (new PidIntakeCommand(m_intake, 1.2) + + // -3.8 + .andThen( + new InstantCommand(() -> m_feeder.runtillswitch()) + .alongWith(new InstantCommand(() -> m_intake.NewIntakeMotorOutput(0.4))) + // ShooterSetDegree(m_shooter, -1.15) + + ) + + ) + ); } } diff --git a/src/main/java/frc/robot/commands/common/cinarcan.java b/src/main/java/frc/robot/commands/common/cinarcan.java index 04374f3..4027e11 100644 --- a/src/main/java/frc/robot/commands/common/cinarcan.java +++ b/src/main/java/frc/robot/commands/common/cinarcan.java @@ -8,12 +8,13 @@ import frc.robot.subsystems.IntakeSubsystem; public class cinarcan extends SequentialCommandGroup { - + public cinarcan(IntakeSubsystem m_intake, FeederSubsystem m_feeder) { - + addCommands( - new PidIntakeCommand(m_intake, 1.5) - .andThen(new InstantCommand(()-> m_feeder.runtillswitch()).alongWith(new InstantCommand(()-> m_intake.NewIntakeMotorOutput(0.7)))) + new PidIntakeCommand(m_intake, 1.5) + .andThen(new InstantCommand(() -> m_feeder.runtillswitch()) + .alongWith(new InstantCommand(() -> m_intake.NewIntakeMotorOutput(0.7)))) ); } diff --git a/src/main/java/frc/robot/commands/common/dalhacan.java b/src/main/java/frc/robot/commands/common/dalhacan.java index 2fabec2..3ece839 100644 --- a/src/main/java/frc/robot/commands/common/dalhacan.java +++ b/src/main/java/frc/robot/commands/common/dalhacan.java @@ -20,8 +20,8 @@ public dalhacan(IntakeSubsystem m_intake, ShooterSubsystem m_shooter) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( -new InstantCommand(()->m_shooter.ShootertoFeederPos()) -.alongWith(new PidIntakeCommand(m_intake, 12.0)) + new InstantCommand(() -> m_shooter.ShootertoFeederPos()) + .alongWith(new PidIntakeCommand(m_intake, 12.0)) ); diff --git a/src/main/java/frc/robot/subsystems/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/FeederSubsystem.java index 427ffee..5d253a1 100644 --- a/src/main/java/frc/robot/subsystems/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FeederSubsystem.java @@ -1,4 +1,5 @@ package frc.robot.subsystems; + import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DigitalInput; //import edu.wpi.first.wpilibj.DigitalInput; @@ -7,68 +8,60 @@ import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; public class FeederSubsystem extends SubsystemBase { - + public WPI_VictorSPX feedmotor = new WPI_VictorSPX(7); public DigitalInput detector = new DigitalInput(6); public boolean magstatus; public boolean count; IntakeSubsystem m_intake; - //public AnalogInput intechedetector = new AnalogInput(0); - + // public AnalogInput intechedetector = new AnalogInput(0); public FeederSubsystem() { - + } - public void forward(){ + public void forward() { feedmotor.set(0.8); } - public void backward(){ + public void backward() { feedmotor.set(-0.8); } - public void runtillswitch(){ - - while((detector.get())){ - feedmotor.set(-0.8); + public void runtillswitch() { - -} -feedmotor.set(0); + while ((detector.get())) { + feedmotor.set(-0.8); + } + feedmotor.set(0); } - //while((detector.getValue() > 250)&&(intechedetector.getValue() < 2000)){ - //feedmotor.set(-0.8); + // while((detector.getValue() > 250)&&(intechedetector.getValue() < 2000)){ + // feedmotor.set(-0.8); - -//} -//feedmotor.set(0); + // } + // feedmotor.set(0); + // } - //} + // public boolean ismagfilled(){ + // if (detector.getValue() >){ + // return false; + // } + // else{ + // return true; + // } + // } - //public boolean ismagfilled(){ - //if (detector.getValue() >){ - //return false; - //} - //else{ - //return true; - //} - //} - - - public void stop(){ + public void stop() { feedmotor.set(0); } @Override public void periodic() { - SmartDashboard.putBoolean("bakacaz", detector.get()); - - + SmartDashboard.putBoolean("bakacaz", detector.get()); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ba356f1..fe7e8a5 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,118 +4,99 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class IntakeSubsystem extends SubsystemBase { -// intake nota motorları -public static CANSparkMax intakeMotor1 = new CANSparkMax(12, MotorType.kBrushless); -//intake main motor -public static CANSparkMax intakeMotor = new CANSparkMax(Constants.ports.intake_motor_angle, MotorType.kBrushless); -public RelativeEncoder IntakeEncoder; + // intake nota motorları + public static CANSparkMax intakeMotor1 = new CANSparkMax(12, MotorType.kBrushless); + // intake main motor + public static CANSparkMax intakeMotor = new CANSparkMax(Constants.ports.intake_motor_angle, MotorType.kBrushless); + public RelativeEncoder IntakeEncoder; public DigitalInput intakedetector1 = new DigitalInput(8); public DigitalInput intakedetector2 = new DigitalInput(9); -public boolean tekcalisma = true; - + public boolean tekcalisma = true; public IntakeSubsystem() { intakeMotor.setIdleMode(IdleMode.kBrake); IntakeEncoder = intakeMotor.getEncoder(); } - public void brakemode(){ + public void brakemode() { intakeMotor.setIdleMode(IdleMode.kBrake); } - public void coastmode(){ + + public void coastmode() { intakeMotor.setIdleMode(IdleMode.kCoast); } - public double getRawEncoderOutput(){ + public double getRawEncoderOutput() { return IntakeEncoder.getPosition(); } - public double getMappedOutput(){ + + public double getMappedOutput() { return IntakeEncoder.getPosition() * -10; } + public void reset() { IntakeEncoder.setPosition(0); } - - public void NewIntakeMotorOutput(double value){ - intakeMotor.set(value); - } - - public void getNote(){ - intakeMotor1.set(Constants.values.intake.GetNoteValue); - } - - - -public void degistir(){ -tekcalisma = false; -} - -public void degistirmee(){ -tekcalisma = true; -} - - - - - public void runpickupmotorswitch(double a){ -if(tekcalisma){ -while((intakedetector1.get() && intakedetector2.get())){ - intakeMotor1.set(a); + public void NewIntakeMotorOutput(double value) { + intakeMotor.set(value); } + public void getNote() { + intakeMotor1.set(Constants.values.intake.GetNoteValue); + } - intakeMotor1.set(0); -} - - -} + public void degistir() { + tekcalisma = false; + } + public void degistirmee() { + tekcalisma = true; + } + public void runpickupmotorswitch(double a) { + if (tekcalisma) { + while ((intakedetector1.get() && intakedetector2.get())) { + intakeMotor1.set(a); + } - + intakeMotor1.set(0); + } + } + public void runpickupmotor(double a) { + intakeMotor1.set(a); + } + public void StopNoteMotor() { + intakeMotor1.set(0); + } - - public void runpickupmotor(double a){ - intakeMotor1.set(a); - + public void StopAngleMotor() { + intakeMotor.set(0); + } - } + public Boolean intakeswitch1() { + return intakedetector1.get(); + } - public void StopNoteMotor(){ - intakeMotor1.set(0); - } + public Boolean intakeswitch2() { + return intakedetector2.get(); + } - public void StopAngleMotor(){ - intakeMotor.set(0); - } - - public Boolean intakeswitch1(){ - return intakedetector1.get(); - } - public Boolean intakeswitch2(){ - return intakedetector2.get(); - } @Override public void periodic() { -SmartDashboard.putNumber("intake", getRawEncoderOutput()); -SmartDashboard.putBoolean("intake switch1", intakeswitch1()); -SmartDashboard.putBoolean("intake switch2", intakeswitch2()); - - -} + SmartDashboard.putNumber("intake", getRawEncoderOutput()); + SmartDashboard.putBoolean("intake switch1", intakeswitch1()); + SmartDashboard.putBoolean("intake switch2", intakeswitch2()); } - +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index fd0d509..988e40c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -13,19 +13,19 @@ public class ShooterSubsystem extends SubsystemBase { -public static CANSparkMax ShooterAngleMotor = new CANSparkMax(Constants.ports.shooter_motor_3, MotorType.kBrushless); -public static CANSparkMax ShooterThrowMotor1 = new CANSparkMax(Constants.ports.shooter_motor_1, MotorType.kBrushless); -public static CANSparkMax ShooterThrowMotor2 = new CANSparkMax(Constants.ports.shooter_motor_2, MotorType.kBrushless); + public static CANSparkMax ShooterAngleMotor = new CANSparkMax(Constants.ports.shooter_motor_3, MotorType.kBrushless); + public static CANSparkMax ShooterThrowMotor1 = new CANSparkMax(Constants.ports.shooter_motor_1, MotorType.kBrushless); + public static CANSparkMax ShooterThrowMotor2 = new CANSparkMax(Constants.ports.shooter_motor_2, MotorType.kBrushless); -public RelativeEncoder ShooterThrow1Encoder; -public RelativeEncoder ShooterThrow2Encoder; -public RelativeEncoder ShooterAngleEncoder; + public RelativeEncoder ShooterThrow1Encoder; + public RelativeEncoder ShooterThrow2Encoder; + public RelativeEncoder ShooterAngleEncoder; -DigitalInput shooterswitch = new DigitalInput(7); + DigitalInput shooterswitch = new DigitalInput(7); -public void setencodervalue(double value){ - ShooterAngleEncoder.setPosition(value); -} + public void setencodervalue(double value) { + ShooterAngleEncoder.setPosition(value); + } public ShooterSubsystem() { ShooterAngleMotor.setIdleMode(IdleMode.kBrake); @@ -35,106 +35,113 @@ public ShooterSubsystem() { ShooterAngleMotor.setOpenLoopRampRate(Constants.values.shooter.AngleMotorOpenLoopRampRate); setencodervalue(0.525); - } - public boolean getshooterswitchvalue(){ + public boolean getshooterswitchvalue() { return shooterswitch.get(); } - - public void brakemode1(){ + public void brakemode1() { ShooterThrowMotor1.setIdleMode(IdleMode.kBrake); } - public void brakemode2(){ + + public void brakemode2() { ShooterThrowMotor2.setIdleMode(IdleMode.kBrake); } - public void coastmode1(){ + + public void coastmode1() { ShooterThrowMotor1.setIdleMode(IdleMode.kCoast); } - public void coastmode2(){ + + public void coastmode2() { ShooterThrowMotor1.setIdleMode(IdleMode.kCoast); } - public void barkeall(){ + + public void barkeall() { ShooterThrowMotor1.setIdleMode(IdleMode.kBrake); ShooterThrowMotor2.setIdleMode(IdleMode.kBrake); } - public void coastall(){ + + public void coastall() { ShooterThrowMotor1.setIdleMode(IdleMode.kCoast); ShooterThrowMotor2.setIdleMode(IdleMode.kCoast); } - public double getRawEncoderOutput(){ + public double getRawEncoderOutput() { return ShooterAngleEncoder.getPosition(); } - public double getMappedOutput(){ + + public double getMappedOutput() { return ShooterAngleEncoder.getPosition() * -10; } + public void AngleEncoderReset() { ShooterAngleEncoder.setPosition(0); } - - - public double getRpmOutput1(){ + + public double getRpmOutput1() { return ShooterThrow1Encoder.getVelocity(); } - public double getRpmOutput2(){ + + public double getRpmOutput2() { return ShooterThrow2Encoder.getVelocity() * 600 / 4096.0; } - public void ShooterThrow1MotorOutput(double value){ - ShooterThrowMotor1.set(value); - } + public void ShooterThrow1MotorOutput(double value) { + ShooterThrowMotor1.set(value); + } - public void ShooterThrow2MotorOutput(double value){ - ShooterThrowMotor2.set(value); - } + public void ShooterThrow2MotorOutput(double value) { + ShooterThrowMotor2.set(value); + } - public void ShooterThrowMotorOutput(double value){ - ShooterThrowMotor1.set(value); - ShooterThrowMotor2.set(value); - } + public void ShooterThrowMotorOutput(double value) { + ShooterThrowMotor1.set(value); + ShooterThrowMotor2.set(value); + } - public void ShooterAngleMotorOutput(double value){ - ShooterAngleMotor.set(value); - } - - public void ShooterThrow1MotorStop(){ - ShooterThrowMotor1.set(0); - } - public void ShooterThrow2MotorStop(){ - ShooterThrowMotor2.set(0); - } - public void ShooterThrowAllMotorStop(){ - ShooterThrowMotor1.set(0); - ShooterThrowMotor2.set(0); - } + public void ShooterAngleMotorOutput(double value) { + ShooterAngleMotor.set(value); + } - public void ShooterAngleMotorStop(){ - ShooterAngleMotor.set(0); - } - public void ShootertoFeederPos(){ - if(!shooterswitch.get()){ - ShooterAngleMotor.set(0.1); - }else{ - ShooterAngleMotor.stopMotor(); - } + public void ShooterThrow1MotorStop() { + ShooterThrowMotor1.set(0); + } + + public void ShooterThrow2MotorStop() { + ShooterThrowMotor2.set(0); + } + + public void ShooterThrowAllMotorStop() { + ShooterThrowMotor1.set(0); + ShooterThrowMotor2.set(0); + } + + public void ShooterAngleMotorStop() { + ShooterAngleMotor.set(0); + } + + public void ShootertoFeederPos() { + if (!shooterswitch.get()) { + ShooterAngleMotor.set(0.1); + } else { + ShooterAngleMotor.stopMotor(); } + } @Override public void periodic() { - SmartDashboard.putNumber("shooter rpm 1",getRpmOutput1()); - SmartDashboard.putNumber("shooter rpm 2",getRpmOutput2()); + SmartDashboard.putNumber("shooter rpm 1", getRpmOutput1()); + SmartDashboard.putNumber("shooter rpm 2", getRpmOutput2()); SmartDashboard.putNumber("Shooter degree", getMappedOutput()); + if (getshooterswitchvalue()) { + ShooterAngleEncoder.setPosition(0); + } + SmartDashboard.putBoolean("deger", getshooterswitchvalue()); -if(getshooterswitchvalue()){ - ShooterAngleEncoder.setPosition(0); } -SmartDashboard.putBoolean("deger", getshooterswitchvalue()); - -} } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 365fcdc..60712ed 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -15,23 +15,22 @@ import frc.robot.Constants.ModuleConstants; import frc.robot.Constants.DriveConstants; - public class SwerveModule extends SubsystemBase { -CANSparkMax driveMotor; -CANSparkMax turningMotor; + CANSparkMax driveMotor; + CANSparkMax turningMotor; -RelativeEncoder driveEncoder; -RelativeEncoder turningEncoder; + RelativeEncoder driveEncoder; + RelativeEncoder turningEncoder; -PIDController turningPidController; + PIDController turningPidController; -CANcoder absoluteEncoder; -boolean absoluteEncoderReversed; -double absoluteEncoderOffsetRad; + CANcoder absoluteEncoder; + boolean absoluteEncoderReversed; + double absoluteEncoderOffsetRad; public SwerveModule(int driveMotorId, int turningMotorId, boolean driveMotorReversed, boolean turningMotorReversed, - int absoluteEncoderId, double absoluteEncoderOffset, boolean absoluteEncoderReversed) { + int absoluteEncoderId, double absoluteEncoderOffset, boolean absoluteEncoderReversed) { this.absoluteEncoderOffsetRad = absoluteEncoderOffset; this.absoluteEncoderReversed = absoluteEncoderReversed; @@ -47,20 +46,19 @@ public SwerveModule(int driveMotorId, int turningMotorId, boolean driveMotorReve driveEncoder = driveMotor.getEncoder(); turningEncoder = turningMotor.getEncoder(); - driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); - driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); - turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Rad); - turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2RadPerSec); - - turningPidController = new PIDController(ModuleConstants.kPTurning, 0, 0); - turningPidController.enableContinuousInput(-Math.PI, Math.PI); - driveMotor.setOpenLoopRampRate(0.5); - turningMotor.setOpenLoopRampRate(0.5); + driveEncoder.setPositionConversionFactor(ModuleConstants.kDriveEncoderRot2Meter); + driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveEncoderRPM2MeterPerSec); + turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Rad); + turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2RadPerSec); + turningPidController = new PIDController(ModuleConstants.kPTurning, 0, 0); + turningPidController.enableContinuousInput(-Math.PI, Math.PI); + driveMotor.setOpenLoopRampRate(0.5); + turningMotor.setOpenLoopRampRate(0.5); - } + } - public double getDrivePosition() { + public double getDrivePosition() { return driveEncoder.getPosition(); } @@ -76,44 +74,41 @@ public double getTurningVelocity() { return turningEncoder.getVelocity(); } - public double getAbsoluteEncoderRad() { + public double getAbsoluteEncoderRad() { StatusSignal absoluteencoderradian = absoluteEncoder.getAbsolutePosition(); - double angle = absoluteencoderradian.getValueAsDouble(); - angle *= 2.0 * Math.PI; - angle -= absoluteEncoderOffsetRad; - return angle * (absoluteEncoderReversed ? -1.0 : 1.0); - } + double angle = absoluteencoderradian.getValueAsDouble(); + angle *= 2.0 * Math.PI; + angle -= absoluteEncoderOffsetRad; + return angle * (absoluteEncoderReversed ? -1.0 : 1.0); + } public void resetEncoders() { driveEncoder.setPosition(0); turningEncoder.setPosition(getAbsoluteEncoderRad()); -} - - public SwerveModuleState getState() { - return new SwerveModuleState(getDriveVelocity(), new Rotation2d(getTurningPosition())); - } - - - public void stop() { - driveMotor.set(0); - turningMotor.set(0); } + public SwerveModuleState getState() { + return new SwerveModuleState(getDriveVelocity(), new Rotation2d(getTurningPosition())); + } + public void stop() { + driveMotor.set(0); + turningMotor.set(0); + } - public void setDesiredState(SwerveModuleState state) { - if (Math.abs(state.speedMetersPerSecond) < 0.001) { - stop(); - return; - } - state = SwerveModuleState.optimize(state, getState().angle); - driveMotor.set( 3 * (state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); - turningMotor.set((turningPidController.calculate(getTurningPosition(), state.angle.getRadians()))); - SmartDashboard.putString("Swerve[" + absoluteEncoder.getDeviceID() + "] state", state.toString()); - SmartDashboard.putNumber("status", (state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond)* 3); - SmartDashboard.putNumber("digeri", turningPidController.calculate(getTurningPosition(), state.angle.getRadians())); + public void setDesiredState(SwerveModuleState state) { + if (Math.abs(state.speedMetersPerSecond) < 0.001) { + stop(); + return; } - + state = SwerveModuleState.optimize(state, getState().angle); + driveMotor.set(3 * (state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + turningMotor.set((turningPidController.calculate(getTurningPosition(), state.angle.getRadians()))); + SmartDashboard.putString("Swerve[" + absoluteEncoder.getDeviceID() + "] state", state.toString()); + SmartDashboard.putNumber("status", + (state.speedMetersPerSecond / DriveConstants.kPhysicalMaxSpeedMetersPerSecond) * 3); + SmartDashboard.putNumber("digeri", turningPidController.calculate(getTurningPosition(), state.angle.getRadians())); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 0678c97..4cc7ddb 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -18,109 +18,117 @@ public class SwerveSubsystem extends SubsystemBase { - private final SwerveModule frontLeft = new SwerveModule( - DriveConstants.kFrontLeftDriveMotorPort, - DriveConstants.kFrontLeftTurningMotorPort, - DriveConstants.kFrontLeftDriveEncoderReversed, - DriveConstants.kFrontLeftTurningEncoderReversed, - DriveConstants.kFrontLeftDriveAbsoluteEncoderPort, - DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetRad, - DriveConstants.kFrontLeftDriveAbsoluteEncoderReversed); - - private final SwerveModule frontRight = new SwerveModule( - DriveConstants.kFrontRightDriveMotorPort, - DriveConstants.kFrontRightTurningMotorPort, - DriveConstants.kFrontRightDriveEncoderReversed, - DriveConstants.kFrontRightTurningEncoderReversed, - DriveConstants.kFrontRightDriveAbsoluteEncoderPort, - DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetRad, - DriveConstants.kFrontRightDriveAbsoluteEncoderReversed); - - private final SwerveModule backLeft = new SwerveModule( - DriveConstants.kBackLeftDriveMotorPort, - DriveConstants.kBackLeftTurningMotorPort, - DriveConstants.kBackLeftDriveEncoderReversed, - DriveConstants.kBackLeftTurningEncoderReversed, - DriveConstants.kBackLeftDriveAbsoluteEncoderPort, - DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetRad, - DriveConstants.kBackLeftDriveAbsoluteEncoderReversed); - - private final SwerveModule backRight = new SwerveModule( - DriveConstants.kBackRightDriveMotorPort, - DriveConstants.kBackRightTurningMotorPort, - DriveConstants.kBackRightDriveEncoderReversed, - DriveConstants.kBackRightTurningEncoderReversed, - DriveConstants.kBackRightDriveAbsoluteEncoderPort, - DriveConstants.kBackRightDriveAbsoluteEncoderOffsetRad, - DriveConstants.kBackRightDriveAbsoluteEncoderReversed); - - - private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); - /*private final SwerveDriveOdometry odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(0), - new SwerveModulePosition[] { - frontLeft.getPosition(), - frontRight.getPosition(), - backLeft.getPosition(), - backRight.getPosition() - }, new Pose2d(5.0, 13.5, new Rotation2d()));*/ + private final SwerveModule frontLeft = new SwerveModule( + DriveConstants.kFrontLeftDriveMotorPort, + DriveConstants.kFrontLeftTurningMotorPort, + DriveConstants.kFrontLeftDriveEncoderReversed, + DriveConstants.kFrontLeftTurningEncoderReversed, + DriveConstants.kFrontLeftDriveAbsoluteEncoderPort, + DriveConstants.kFrontLeftDriveAbsoluteEncoderOffsetRad, + DriveConstants.kFrontLeftDriveAbsoluteEncoderReversed); + + private final SwerveModule frontRight = new SwerveModule( + DriveConstants.kFrontRightDriveMotorPort, + DriveConstants.kFrontRightTurningMotorPort, + DriveConstants.kFrontRightDriveEncoderReversed, + DriveConstants.kFrontRightTurningEncoderReversed, + DriveConstants.kFrontRightDriveAbsoluteEncoderPort, + DriveConstants.kFrontRightDriveAbsoluteEncoderOffsetRad, + DriveConstants.kFrontRightDriveAbsoluteEncoderReversed); + + private final SwerveModule backLeft = new SwerveModule( + DriveConstants.kBackLeftDriveMotorPort, + DriveConstants.kBackLeftTurningMotorPort, + DriveConstants.kBackLeftDriveEncoderReversed, + DriveConstants.kBackLeftTurningEncoderReversed, + DriveConstants.kBackLeftDriveAbsoluteEncoderPort, + DriveConstants.kBackLeftDriveAbsoluteEncoderOffsetRad, + DriveConstants.kBackLeftDriveAbsoluteEncoderReversed); + + private final SwerveModule backRight = new SwerveModule( + DriveConstants.kBackRightDriveMotorPort, + DriveConstants.kBackRightTurningMotorPort, + DriveConstants.kBackRightDriveEncoderReversed, + DriveConstants.kBackRightTurningEncoderReversed, + DriveConstants.kBackRightDriveAbsoluteEncoderPort, + DriveConstants.kBackRightDriveAbsoluteEncoderOffsetRad, + DriveConstants.kBackRightDriveAbsoluteEncoderReversed); + + private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); + /* + * private final SwerveDriveOdometry odometer = new + * SwerveDriveOdometry(DriveConstants.kDriveKinematics, new Rotation2d(0), + * new SwerveModulePosition[] { + * frontLeft.getPosition(), + * frontRight.getPosition(), + * backLeft.getPosition(), + * backRight.getPosition() + * }, new Pose2d(5.0, 13.5, new Rotation2d())); + */ public SwerveSubsystem() { new Thread(() -> { try { - Thread.sleep(1000); - zeroHeading(); + Thread.sleep(1000); + zeroHeading(); } catch (Exception e) { } - }).start(); - + }).start(); } public void zeroHeading() { gyro.reset(); -} - -public double getHeading() { - return Math.IEEEremainder(gyro.getAngle(), 360); -} - -public Rotation2d getRotation2d() { - return Rotation2d.fromDegrees(getHeading()); -} - - /* public Pose2d getPose() { - return odometer.getPoseMeters(); - }*/ - - /* public void resetOdometry(Pose2d pose) { - odometer.resetPosition(getRotation2d(),pose); + } - }*/ + public double getHeading() { + return Math.IEEEremainder(gyro.getAngle(), 360); + } + public Rotation2d getRotation2d() { + return Rotation2d.fromDegrees(getHeading()); + } - public void stopModules() { - frontLeft.stop(); - frontRight.stop(); - backLeft.stop(); - backRight.stop(); + /* + * public Pose2d getPose() { + * return odometer.getPoseMeters(); + * } + */ + + /* + * public void resetOdometry(Pose2d pose) { + * odometer.resetPosition(getRotation2d(),pose); + * + * } + */ + + public void stopModules() { + frontLeft.stop(); + frontRight.stop(); + backLeft.stop(); + backRight.stop(); } -public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); - frontLeft.setDesiredState(desiredStates[0]); - frontRight.setDesiredState(desiredStates[1]); - backLeft.setDesiredState(desiredStates[2]); - backRight.setDesiredState(desiredStates[3]); - } + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kPhysicalMaxSpeedMetersPerSecond); + frontLeft.setDesiredState(desiredStates[0]); + frontRight.setDesiredState(desiredStates[1]); + backLeft.setDesiredState(desiredStates[2]); + backRight.setDesiredState(desiredStates[3]); + } @Override public void periodic() { - /*odometer.update(getRotation2d(), frontLeft.getState(), frontRight.getState(), backLeft.getState(), - backRight.getState()); - SmartDashboard.putNumber("Robot Heading", getHeading()); - SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); */ - SmartDashboard.putNumber("gyro", getHeading()); - - } + /* + * odometer.update(getRotation2d(), frontLeft.getState(), frontRight.getState(), + * backLeft.getState(), + * backRight.getState()); + * SmartDashboard.putNumber("Robot Heading", getHeading()); + * SmartDashboard.putString("Robot Location", + * getPose().getTranslation().toString()); + */ + SmartDashboard.putNumber("gyro", getHeading()); + + } }