Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Revert "Intake" #13

Merged
merged 1 commit into from
Feb 21, 2024
Merged

Revert "Intake" #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
215 changes: 98 additions & 117 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
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
Open in desktop
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
Open in desktop

This file was deleted.

2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View file
Open in desktop
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
Loading