Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

FIX

Browse Source
This commit is contained in:
Ryan Manley
2022-03-10 16:47:32 -07:00
parent dce8ab320c
commit e237f14537
7 changed files with 80 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -1
View File
@@ -31,7 +31,7 @@ import frc4388.utility.LEDPatterns;
*/
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 0.5;
public static final double ROTATION_SPEED = 4.0;
public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
@@ -150,6 +150,7 @@ public final class Constants {
public static final int XBOX_OPERATOR_ID = 1;
public static final double LEFT_AXIS_DEADBAND = 0.1;
public static final double RIGHT_AXIS_DEADBAND = 0.6;
public static final boolean SKEW_STICKS = true;
}
public static final class ShooterConstants {
src/main/java/frc4388/robot/RobotContainer.java
+34 -11
View File
@@ -29,12 +29,16 @@ import java.util.regex.Matcher;
import java.util.regex.Pattern;
import java.util.stream.Collectors;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.diffplug.common.base.Errors;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
import org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -62,6 +66,7 @@ import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.Shoot;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
@@ -98,8 +103,11 @@ public class RobotContainer {
public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
// private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);*/
private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30);
private final WPI_TalonFX testSoulderMotor = new WPI_TalonFX(31);
private final Climber m_robotClimber = new Climber(testElbowMotor);
/* Controllers */
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -124,6 +132,7 @@ public class RobotContainer {
*/
public RobotContainer() {
configureButtonBindings();
testSoulderMotor.setNeutralMode(NeutralMode.Brake);
/* Default Commands */
// Swerve Drive with Input
@@ -133,7 +142,7 @@ public class RobotContainer {
getDriverController().getLeftY(),
//getDriverController().getRightX(),
getDriverController().getRightX(),
//getDriverController().getRightY(),
// getDriverController().getRightY(),
false),
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
@@ -142,6 +151,9 @@ public class RobotContainer {
getOperatorController().getLeftTriggerAxis(),
getOperatorController().getRightTriggerAxis()),
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber)
);
// Storage Management
/*m_robotStorage.setDefaultCommand(
new RunCommand(() -> m_robotStorage.manageStorage(),
@@ -155,7 +167,7 @@ public class RobotContainer {
new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
m_robotHood.setDefaultCommand(
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY() * 0.1), m_robotHood));
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
@@ -190,6 +202,14 @@ public class RobotContainer {
// Right Bumper > Shift Up
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025));
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
.whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025));
// .whileHeld(new RunCommand(() -> testElbowMotor.set(-0.2)))
// .whenReleased(new RunCommand(() -> testElbowMotor.set(0)));
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
@@ -198,7 +218,7 @@ public class RobotContainer {
// .whenPressed(() -> m_robotMap.leftFront.reset())
// .whenPressed(() -> m_robotMap.rightFront.reset())
// .whenPressed(() -> m_robotMap.leftBack.reset())
// .whenPressed(() -> m_robotMap.rightBack.reset());
// .whenPressed(() -> m_robotMap.rightBack.reset());
/* Operator Buttons */
@@ -231,10 +251,11 @@ public class RobotContainer {
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))).whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45)))
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525)))
.whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
@@ -251,12 +272,14 @@ public class RobotContainer {
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// A > Shoot with Odo
/*new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
// B > Shoot with Lime
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));*/
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
//B > Shoot with Lime
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
}
/**
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+2 -1
View File
@@ -18,6 +18,7 @@ import java.util.stream.IntStream;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -50,7 +51,6 @@ public class BoomBoom extends SubsystemBase {
}
private ShooterTableEntry[] m_shooterTable;
/*
* Creates new BoomBoom subsystem, has drum shooter and angle adjuster
*/
@@ -165,6 +165,7 @@ public class BoomBoom extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
// speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
src/main/java/frc4388/robot/subsystems/Climber.java
+26
View File
@@ -0,0 +1,26 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Climber extends SubsystemBase {
WPI_TalonFX m_climberElbow;
/** Creates a new Climber. */
public Climber(WPI_TalonFX climberElbow) {
m_climberElbow = climberElbow;
}
public void runWithInput(double input){
m_climberElbow.set(input);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+12 -16
View File
@@ -37,14 +37,10 @@ public class SwerveDrive extends SubsystemBase {
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
@@ -108,29 +104,29 @@ public class SwerveDrive extends SubsystemBase {
ignoreAngles = true;
else
ignoreAngles = false;
Translation2d speed = new Translation2d(-speedY, -speedX);
Translation2d speed = new Translation2d(speedX, speedY);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
double xSpeedMetersPerSecond = -speed.getX();
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED * 8, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * 8);
-rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(-leftX, leftY);
Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).plus(new Rotation2d(0, 1));
rotTarget = new Rotation2d(rightY, rightX);
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java
+2 -1
View File
@@ -3,6 +3,7 @@ package frc4388.utility.controller;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.Constants.OIConstants;
public class DeadbandedRawXboxController extends RawXboxController {
public DeadbandedRawXboxController(int port) { super(port); }
@@ -13,7 +14,7 @@ public class DeadbandedRawXboxController extends RawXboxController {
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+2 -1
View File
@@ -4,6 +4,7 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import frc4388.robot.Constants.OIConstants;
public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); }
@@ -14,7 +15,7 @@ public class DeadbandedXboxController extends XboxController {
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}