Skip to content

Commit

Permalink
Merge branch 'swerve' into AprilTagAlign
Browse files Browse the repository at this point in the history
  • Loading branch information
@ProgrammingSR
ProgrammingSR committed Jan 9, 2024
2 parents 36147a0 + 7eee9bb commit 7939257
Show file tree
Hide file tree
Showing 8 changed files with 1,044 additions and 51 deletions.
94 changes: 87 additions & 7 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,94 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;

/**
* The Constants class provides a convenient place for teams to hold robot-wide
* numerical or boolean
* constants. This class should not be used for any other purpose. All constants
* should be declared
* globally (i.e. public static). Do not put anything functional in this class.
*
* <p>
* It is advised to statically import this class (or one of its inner classes)
* wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public final static class TagAlignConstants {

/**
* Input/Output constants
*/
public static 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 = 1;
public static final int kRearLeftDriveMotorPort = 3;
public static final int kFrontRightDriveMotorPort = 5;
public static final int kRearRightDriveMotorPort = 7;

public static final int kFrontLeftTurningMotorPort = 2;
public static final int kRearLeftTurningMotorPort = 4;
public static final int kFrontRightTurningMotorPort = 6;
public static final int kRearRightTurningMotorPort = 8;

public static final int kFrontLeftTurningEncoderPort = 9;
public static final int kRearLeftTurningEncoderPort = 10;
public static final int kFrontRightTurningEncoderPort = 11;
public static final int kRearRightTurningEncoderPort = 12;

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.5;

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

/** 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

// 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 Neos */
public static final double kMaxSpeedMetersPerSecond = 3.6576;
/** For a a SDS Mk4i L1 swerve base with Neos */
public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3;

/** 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 final static class TagAlignConstants {
public final static double kTagPIDkPxy = 0; // xy PID constants
public final static double kTagPIDkIxy = 0;
public final static double kTagPIDkDxy = 0;
Expand Down Expand Up @@ -71,9 +156,4 @@ public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new i
// direction is correct. Needs to be implemented after drive since it uses pose
// estimator.
}

public final static class DriveConstants {
//TODO: init pose estimator with actual values
public static final SwerveDrivePoseEstimator kPoseEstimator = new SwerveDrivePoseEstimator(null, null, null, null);
}
}
}
57 changes: 56 additions & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,15 +4,70 @@

package frc.robot;

import edu.wpi.first.math.MathUtil;
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;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.IOConstants;
import frc.robot.subsystems.DriveSubsystem;


public class RobotContainer {
// The robot's subsystems and commands are defined here
private final DriveSubsystem m_robotDrive = new DriveSubsystem();

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_robotDrive.setDefaultCommand(
new RunCommand(
() -> m_robotDrive.drive(
MathUtil.applyDeadband(
-m_driverController.getLeftY(),
IOConstants.kControllerDeadband)
* DriveConstants.kMaxSpeedMetersPerSecond
* (1 - m_driverController
.getLeftTriggerAxis()
* IOConstants.kSlowModeScalar)
* 0.8,
MathUtil.applyDeadband(
-m_driverController.getLeftX(),
IOConstants.kControllerDeadband)
* DriveConstants.kMaxSpeedMetersPerSecond
* (1 - m_driverController
.getLeftTriggerAxis()
* IOConstants.kSlowModeScalar)
* 0.8,
MathUtil.applyDeadband(
m_driverController.getRightX(),
IOConstants.kControllerDeadband)
* DriveConstants.kMaxAngularSpeedRadiansPerSecond
* (1 - m_driverController
.getLeftTriggerAxis()
* IOConstants.kSlowModeScalar)
/ 2,
!m_driverController.getRightBumper()),
m_robotDrive));
}

private void configureBindings() {}
/**
* 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));
}

public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
Expand Down
50 changes: 7 additions & 43 deletions src/main/java/frc/robot/commands/AlignToTagCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,6 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.DoubleArrayEntry;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
Expand All @@ -15,10 +13,11 @@
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.TagAlignConstants;
import frc.robot.Constants.TagAlignConstants.AlignPosition;
import frc.robot.subsystems.DriveSubsystem;

public class AlignToTagCommand extends CommandBase {

final Subsystem m_subsystem; // TODO: change type to drive subsystem type
final DriveSubsystem m_subsystem;
final Pose2d m_target; // Desired pose
final PIDController m_PIDxy;
final PIDController m_PIDomega; // Omega is used to denote robot heading in radians
Expand All @@ -27,11 +26,10 @@ public class AlignToTagCommand extends CommandBase {

/**
*
* @param subsystem Subsystem to require. Subsystem must implement a drive
* method and a getPosition method.
* @param subsystem Subsystem to require
* @param alignPos Position to align to
*/
public AlignToTagCommand(Subsystem subsystem, AlignPosition alignPos) {
public AlignToTagCommand(DriveSubsystem subsystem, AlignPosition alignPos) {
m_subsystem = subsystem;
addRequirements(m_subsystem);

Expand Down Expand Up @@ -65,7 +63,7 @@ public void initialize() {

@Override
public void execute() {
Pose2d current = poseFromAprilTag();
Pose2d current = m_subsystem.getPose();

double xSpeed = MathUtil.clamp(m_PIDxy.calculate(current.getX(), m_target.getX()),
-0, 0); // TODO: change clamp parameters and units
Expand All @@ -87,7 +85,7 @@ public void execute() {
} else
m_consecCount = 0;

// TODO: call drive function
m_subsystem.drive(xSpeed, ySpeed, omegaSpeed, true);
}

@Override
Expand All @@ -99,43 +97,9 @@ public void end(boolean interrupted) {

@Override
public boolean isFinished() {
Pose2d current = poseFromAprilTag();
Pose2d current = m_subsystem.getPose();
return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math
.pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity.
m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint
}

/**
* Gets pose from vision measurements from limelight
*
* @return Robot Pose
*/
Pose2d poseFromAprilTag() { // TODO: move this to drive subsystem
NetworkTable tb = TagAlignConstants.kNTInstance.getTable("limelight");

DoubleArrayEntry botpose = null; // index 0: x, 1: y, 2: z, 3: roll, 4: pitch, 5: yaw, 6: timestamp. all angles
// are in degrees

try {
botpose = tb.getDoubleArrayTopic("botpose").getEntry(new double[0]);
double[] bp = botpose.getAtomic().value; // Not sure how threading works in WPIlib w/ command scheduler. If it is
// thread safe then atomic wont be needed. There is no need to use the
// atomic timestamp since the topic has the timestamp stored in it (7th
// element)

if (botpose.exists()) {
Pose2d visionPose = new Pose2d(new Translation2d(bp[0], bp[1]), new Rotation2d(bp[5] * Math.PI / 180));
DriveConstants.kPoseEstimator.addVisionMeasurement(visionPose, Timer.getFPGATimestamp() - (bp[6] / 1000));
} else {
// TODO: log limelight failure
}

} finally {
if (botpose != null && botpose.isValid())
botpose.close(); // TODO: verify this logic: make sure it will never throw an exception,
// otherwise add another trycatch block
}

return DriveConstants.kPoseEstimator.getEstimatedPosition();
}
}
Loading

0 comments on commit 7939257

Please sign in to comment.