Skip to content

Commit

Permalink
Revert "Intake"
Browse files Browse the repository at this point in the history
  • Loading branch information
@ProgrammingSR
ProgrammingSR authored Feb 21, 2024
1 parent 497b170 commit 9c777d8
Show file tree
Hide file tree
Showing 5 changed files with 131 additions and 234 deletions.
215 changes: 98 additions & 117 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,121 +25,102 @@
* constants are needed, to reduce verbosity.
*/
public final class Constants {
/**
* Input/Output constants
*/
public static final class IOConstants {
public static final int kDriverControllerPort = 0;

public static final double kControllerDeadband = 0.2;
public static final double kSlowModeScalar = 0.8;
}

public static final class DriveConstants {
// TODO: set motor and encoder constants
public static final int kFrontLeftDriveMotorPort = 32;
public static final int kRearLeftDriveMotorPort = 29;
public static final int kFrontRightDriveMotorPort = 38;
public static final int kRearRightDriveMotorPort = 34;

public static final int kFrontLeftTurningMotorPort = 28;
public static final int kRearLeftTurningMotorPort = 22;
public static final int kFrontRightTurningMotorPort = 37;
public static final int kRearRightTurningMotorPort = 26;

public static final int kFrontLeftTurningEncoderPort = 19;
public static final int kRearLeftTurningEncoderPort = 20;
public static final int kFrontRightTurningEncoderPort = 18;
public static final int kRearRightTurningEncoderPort = 17;

public static final double kFrontLeftTurningEncoderOffset = 0;
public static final double kRearLeftTurningEncoderOffset = 0;
public static final double kFrontRightTurningEncoderOffset = 0;
public static final double kRearRightTurningEncoderOffset = 0;

// TODO: Test motor orientations before driving on an actual robot
public static final boolean kFrontLeftDriveMotorReversed = false;
public static final boolean kRearLeftDriveMotorReversed = false;
public static final boolean kFrontRightDriveMotorReversed = true;
public static final boolean kRearRightDriveMotorReversed = true;

/** Distance between centers of right and left wheels on robot (in meters). */
public static final double kTrackWidth = 0.57785;

/** Distance between front and back wheels on robot (in meters). */
public static final double kWheelBase = 0.57785;

/** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
public static final double kWheelDiameterMeters = 0.1;

/** Gear ratio between the motor and the wheel. */
public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
// public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2
// configuration

// TODO: Tune this PID before running on a robot on the ground
public static final double kPModuleTurningController = -0.3;

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));

/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxSpeedMetersPerSecond = 4.4196;
/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
// ^^ Calculated using the method taken from the old SDS github example

/** Heading Correction */
public static final double kHeadingCorrectionTurningStopTime = 0.2;
// TODO: Tune this PID before running on a robot on the ground
public static final double kPHeadingCorrectionController = 5;
}

// Intake PID and Encoder Constants
public static class IntakeConstants {
public static final double kIntakeLoweredAngle = 9.0;
public static final double kIntakeRaisedAngle = 9.0;
public static final int kIntakeMotorID = 0;
public static final int kArmMotorID = 0;
public static final double kIntakeP = 0;
public static final double kIntakeI = 0;
public static final double kIntakeD = 0;
public static final double kArmP = 0;
public static final double kArmI = 0;
public static final double kArmD = 0;
public static final int kArmEncoderCh = 0;
public static double kIntakeSpeed;
}

// Shooter subsystem speed constants
public static class ShooterConstants {
public static final double kSpinSpeedTrue = 0.75;
public static final double kSpinSpeedFalse = 0;
public static int kBottomShooterMotorPort;
public static int kTopShooterMotorPort;

}

public static final class VisionConstants {
// TODO: Update cam pose relative to center of bot
public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
public static final double[] kLimelightCamPose = {
kCamPose.getX(),
kCamPose.getY(),
kCamPose.getZ(),
kCamPose.getRotation().getX(),
kCamPose.getRotation().getY(),
kCamPose.getRotation().getZ() };

// TODO: Experiment with different std devs in the pose estimator
public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);

// Field size in meters
public static final double kFieldWidth = 8.21055;
public static final double kFieldLength = 16.54175;
}

/**
* Input/Output constants
*/
public static final class IOConstants {
public static final int kDriverControllerPort = 0;

public static final double kControllerDeadband = 0.2;
public static final double kSlowModeScalar = 0.8;
}

public static final class DriveConstants {
// TODO: set motor and encoder constants
public static final int kFrontLeftDriveMotorPort = 32;
public static final int kRearLeftDriveMotorPort = 29;
public static final int kFrontRightDriveMotorPort = 38;
public static final int kRearRightDriveMotorPort = 34;

public static final int kFrontLeftTurningMotorPort = 28;
public static final int kRearLeftTurningMotorPort = 22;
public static final int kFrontRightTurningMotorPort = 37;
public static final int kRearRightTurningMotorPort = 26;

public static final int kFrontLeftTurningEncoderPort = 19;
public static final int kRearLeftTurningEncoderPort = 20;
public static final int kFrontRightTurningEncoderPort = 18;
public static final int kRearRightTurningEncoderPort = 17;

public static final double kFrontLeftTurningEncoderOffset = 0;
public static final double kRearLeftTurningEncoderOffset = 0;
public static final double kFrontRightTurningEncoderOffset = 0;
public static final double kRearRightTurningEncoderOffset = 0;

// TODO: Test motor orientations before driving on an actual robot
public static final boolean kFrontLeftDriveMotorReversed = false;
public static final boolean kRearLeftDriveMotorReversed = false;
public static final boolean kFrontRightDriveMotorReversed = true;
public static final boolean kRearRightDriveMotorReversed = true;

/** Distance between centers of right and left wheels on robot (in meters). */
public static final double kTrackWidth = 0.57785;

/** Distance between front and back wheels on robot (in meters). */
public static final double kWheelBase = 0.57785;

/** Diameter of each wheel in the SDS MK4i swerve module (in meters) */
public static final double kWheelDiameterMeters = 0.1;

/** Gear ratio between the motor and the wheel. */
public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 Configuration
// public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration

// TODO: Tune this PID before running on a robot on the ground
public static final double kPModuleTurningController = -0.3;

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));

/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxSpeedMetersPerSecond = 4.4196;
/** For a a SDS Mk4i L1 swerve base with Neo Vortexes */
public static final double kMaxAngularSpeedRadiansPerSecond = 10.8164;
// ^^ Calculated using the method taken from the old SDS github example

/** Heading Correction */
public static final double kHeadingCorrectionTurningStopTime = 0.2;
// TODO: Tune this PID before running on a robot on the ground
public static final double kPHeadingCorrectionController = 5;
}

public static final class ShooterConstants {
public static final int kTopShooterMotorPort = 35;
public static final int kBottomShooterMotorPort = 20;
}

public static final class VisionConstants {
// TODO: Update cam pose relative to center of bot
public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
public static final double[] kLimelightCamPose = {
kCamPose.getX(),
kCamPose.getY(),
kCamPose.getZ(),
kCamPose.getRotation().getX(),
kCamPose.getRotation().getY(),
kCamPose.getRotation().getZ() };

// TODO: Experiment with different std devs in the pose estimator
public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);

// Field size in meters
public static final double kFieldWidth = 8.21055;
public static final double kFieldLength = 16.54175;
}

}
38 changes: 8 additions & 30 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,9 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants.IntakeConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.IOConstants;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;

/*
Expand All @@ -26,22 +24,18 @@
* (including subsystems, commands, and button mappings) should be declared here.
*/
public class RobotContainer {

// The robot's subsystems and commands are defined here
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();

private boolean IntakeDropped = false;
private boolean lastAButton = false;

private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
m_intakeSubsystem.setDefaultCommand(m_driverController.getLeftTriggerAxis() > 0.5
? new InstantCommand(m_intakeSubsystem::intakeDisk, m_intakeSubsystem)
: new InstantCommand(m_intakeSubsystem::stopIntaking, m_intakeSubsystem));

m_robotDrive.setDefaultCommand(
new RunCommand(
Expand Down Expand Up @@ -72,32 +66,16 @@ public RobotContainer() {
/ 2,
!m_driverController.getRightBumper()),
m_robotDrive));

}

public void periodic() {
if (m_driverController.getAButton()) {
lastAButton = true;
if (!lastAButton)
IntakeDropped = !IntakeDropped;
} else {
lastAButton = false;
}
}

/**
* Use this method to define your button->command mappings.
*/
private void configureBindings() {
new JoystickButton(m_driverController, Button.kStart.value)
.onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));

// TODO: Move shoot commands to operator controller
new JoystickButton(m_driverController, Button.kY.value)
.onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeLoweredAngle),
m_intakeSubsystem))
.onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
new JoystickButton(m_driverController, Button.kX.value)
.onTrue(new InstantCommand(() -> m_intakeSubsystem.tiltToAngle(IntakeConstants.kIntakeRaisedAngle),
m_intakeSubsystem))
.onFalse(new InstantCommand(m_intakeSubsystem::stopRotating, m_intakeSubsystem));
new JoystickButton(m_driverController, Button.kB.value)
.onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
.onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
Expand All @@ -114,4 +92,4 @@ private void configureBindings() {
public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
}
}
}
62 changes: 0 additions & 62 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View file

This file was deleted.

2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,4 @@ public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Speed", m_bottom.);
}
}
}
Loading

0 comments on commit 9c777d8

Please sign in to comment.