Skip to content

Commit

Permalink
Merge branch 'main' into climber
Browse files Browse the repository at this point in the history
  • Loading branch information
@ProgrammingSR
ProgrammingSR committed Feb 24, 2024
2 parents c093305 + 346f602 commit f7c11ff
Show file tree
Hide file tree
Showing 9 changed files with 256 additions and 201 deletions.
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.1.1"
id "edu.wpi.first.GradleRIO" version "2024.3.1"
}

java {
Expand Down
208 changes: 92 additions & 116 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,104 +25,80 @@
* 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 class ClimberConstants {
/**
* Input/Output constants
*/
public static final class IOConstants {
public static final int kDriverControllerPort = 0;
public static final int kOperatorControllerPort = 1;

public static final double kControllerDeadband = 0.05;
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 = 5;
public static final int kRearLeftTurningEncoderPort = 6;
public static final int kFrontRightTurningEncoderPort = 3;
public static final int kRearRightTurningEncoderPort = 4;

// 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 class ClimberConstants {
public final static int leftForwardChannel = 0;
public final static int rightForwardChannel = 0;
public final static int leftReverseChannel = 1;
Expand All @@ -133,24 +109,24 @@ public static class ClimberConstants {


}
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.01);
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;
}

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;
}
}
70 changes: 52 additions & 18 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,20 @@

package frc.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
Expand All @@ -27,16 +36,28 @@
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();
private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();

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

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
* The container for the robot. Contains subsystems, IO devices, and commands.
*/
public RobotContainer() {

AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry,
m_robotDrive::getChassisSpeeds,
m_robotDrive::autonDrive,
new HolonomicPathFollowerConfig(
new PIDConstants(5, 0.0, 0.0), // Translation PID constants
new PIDConstants(5, 0.0, 0.0), // Rotation PID constants
DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in meters. Distance from robot center to furthest module.
new ReplanningConfig(true, true)),
() -> false, m_robotDrive);

// Configure the trigger bindings
configureBindings();

Expand All @@ -49,18 +70,18 @@ public RobotContainer() {
* DriveConstants.kMaxSpeedMetersPerSecond
* (1 - m_driverController
.getLeftTriggerAxis()
* IOConstants.kSlowModeScalar)
* 0.8,
* IOConstants.kSlowModeScalar),
// * 0.8,
MathUtil.applyDeadband(
-m_driverController.getLeftX(),
IOConstants.kControllerDeadband)
* DriveConstants.kMaxSpeedMetersPerSecond
* (1 - m_driverController
.getLeftTriggerAxis()
* IOConstants.kSlowModeScalar)
* 0.8,
* IOConstants.kSlowModeScalar),
// * 0.8,
MathUtil.applyDeadband(
m_driverController.getRightX(),
-m_driverController.getRightX(),
IOConstants.kControllerDeadband)
* DriveConstants.kMaxAngularSpeedRadiansPerSecond
* (1 - m_driverController
Expand All @@ -78,17 +99,16 @@ 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.kB.value)
// new JoystickButton(m_driverController, Button.kA.value).whileTrue(
// AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints(
// DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));

new JoystickButton(m_operatorController, Button.kX.value)
.onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
.onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
new JoystickButton(m_driverController, Button.kA.value)
new JoystickButton(m_operatorController, Button.kY.value)
.onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
.onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
new JoystickButton(m_operatorController, Button.kA.value)
.onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_shooterSubsystem));
new JoystickButton(m_operatorController, Button.kB.value)
.onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_shooterSubsystem));
}

/**
Expand All @@ -97,6 +117,20 @@ private void configureBindings() {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path");

var alliance = DriverStation.getAlliance();
PathPlannerPath autonPath = path;
if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) {
autonPath = autonPath.flipPath();
}
m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose());

// return AutoBuilder.followPath(autonPath);
return null;

// PathPlannerAuto pathPlannerAuto = new PathPlannerAuto("New Auto");

// return pathPlannerAuto;
}
}
}
Loading

0 comments on commit f7c11ff

Please sign in to comment.