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

Merge remote-tracking branch 'origin/testRoboReveal' into cleanup

Browse Source
This commit is contained in:
nathanrsxtn
2022-03-22 17:41:02 -06:00
parent 8e61e1cb9b f8092825f6
commit 393e5b8bb0
46 changed files with 3154 additions and 929 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+161 -39
View File
@@ -4,6 +4,9 @@
package frc4388.robot;
import java.security.PublicKey;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import edu.wpi.first.math.controller.PIDController;
@@ -30,28 +33,31 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048;
public static final double LOOP_TIME = 0.02;
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4;
public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5;
public static final double ROTATION_SPEED = 2.0;
public static final double WIDTH = 23.75;
public static final double HEIGHT = 23.75;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant?
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
public static final int LEFT_BACK_STEER_CAN_ID = 6;
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int LEFT_FRONT_STEER_CAN_ID = 2; // *
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // *
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // *
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // *
public static final int LEFT_BACK_STEER_CAN_ID = 6; // *
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // *
public static final int RIGHT_BACK_STEER_CAN_ID = 8; // *
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // *
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // *
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // *
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // *
public static final int GYRO_ID = 14;
// offsets are in degrees
@@ -65,10 +71,10 @@ public final class Constants {
// public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;//
// 180-2.021484375;//0.0;//134.384765625
public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.;
public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90 ) % 360.;
public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.;
public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 90 - 180) % 360.;
public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 ) % 360.;
public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 ) % 360.;
public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50) % 360.;
public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180) % 360.;
// swerve PID constants
public static final int SWERVE_SLOT_IDX = 0;
@@ -104,7 +110,6 @@ public final class Constants {
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double INCHES_PER_METER = 39.370;
public static final double METERS_PER_INCH = 1 / INCHES_PER_METER;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
@@ -112,6 +117,18 @@ public final class Constants {
public static final double TICK_TIME_TO_SECONDS = 0.1;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
// current limits
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_STEER = new SupplyCurrentLimitConfiguration(
false, 10, 0, 0);
public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_STEER = new StatorCurrentLimitConfiguration(
false, 15, 0, 0);
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL = new SupplyCurrentLimitConfiguration(
false, 10, 0, 0);
public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_WHEEL = new StatorCurrentLimitConfiguration(
false, 15, 0, 0);
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
@@ -129,12 +146,23 @@ public final class Constants {
// CAN IDs
public static final int INTAKE_MOTOR = 15;
public static final int EXTENDER_MOTOR = 16;
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE = new SupplyCurrentLimitConfiguration(
false, 10, 0, 0); //Find
public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration(
false, 15, 0, 0);
public static final double INTAKE_SPEED_MULTIPLIER = 0.4;
}
public static final class ExtenderConstants {
public static final double EXTENDER_FORWARD_LIMIT = 235.0;//250.0;
public static final double EXTENDER_REVERSE_LIMIT = 0.0;
}
public static final class StorageConstants {
public static final int STORAGE_CAN_ID = 18;
public static final int BEAM_SENSOR_SHOOTER = 28; //TODO
public static final int BEAM_SENSOR_INTAKE = 29; //TODO
public static final double STORAGE_SPEED = 0.75;
public static final double STORAGE_SPEED = 0.9;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
@@ -142,14 +170,90 @@ public final class Constants {
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
public static final class ClimberConstants {
public static final int SHOULDER_ID = 30;
public static final int ELBOW_ID = 31;
public static final int GYRO_ID = 14;
public static final double INPUT_MULTIPLIER = 1.0;
public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0;
public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0;
// TODO Update this stuff too
public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
public static final double LOWER_ARM_LENGTH = 27;
public static final double MAX_ARM_LENGTH = 53;
public static final double MIN_ARM_LENGTH = 1;
public static final double MOVE_SPEED = 30000; // ticks per second
public static final double SHOULDER_RESTING_ANGLE = 0;
public static final double ELBOW_RESTING_ANGLE = 0;
public static final double SHOULDER_MAX_ANGLE = 135;
public static final double ELBOW_MAX_ANGLE = 180;
public static final double ELBOW_GB_RATIO = 1.d;
public static final double SHOULDER_GB_RATIO = 1.d;
public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869;
public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0;
public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717;
public static final double ELBOW_REVERSE_SOFT_LIMIT = 0;
// PID Constants
public static final int SHOULDER_POSITION_SLOT_IDX = 0;
public static final int SHOULDER_VELOCITY_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 0;
public static final int ELBOW_POSITION_SLOT_IDX = 0;
public static final int ELBOW_VELOCITY_SLOT_IDX = 0;
public static final int ELBOW_PID_LOOP_IDX = 0;
public static final Gains SHOULDER_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2);
public static final Gains ELBOW_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2);
public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(0.2, 0.0, 5.0, 0.0, 0, 0.2);
public static final Gains ELBOW_VELOCITY_GAINS = new Gains(0.3, 0.005, 5.0, 0.0, 0, 1.0);
public static final int CLIMBER_TIMEOUT_MS = 50;
public static final double THRESHOLD = 0;
// TODO: Update Constants
// Robot Angle
public static final double ROBOT_ANGLE_ID = 0;
}
public static final class ClawConstants {
public static final int LEFT_CLAW_ID = 44;
public static final int RIGHT_CLAW_ID = 45;
public static final double OPEN_POSITION = 0; // TODO: find actual position
public static final double CLOSE_POSITION = 1; // TODO: find actual position
public static final double THRESHOLD = .1; // TODO: find actual threshold
public static final double CALIBRATION_SPEED = 0;
public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit;
public static final int TOP_LIMIT = 1800;
public static final int BOTTOM_LIMIT = 1200;
}
/**
* The OIConstants class contains the ID for the XBox controllers
*/
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final int BUTTON_BOX_ID = 2;
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 {
@@ -159,36 +263,50 @@ public final class Constants {
public static final int SHOOTER_TIMEOUT_MS = 32;
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(
true, 60, 40, 0.5);
public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER = new SupplyCurrentLimitConfiguration(
true, 10, 0, 0);
public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_SHOOTER = new StatorCurrentLimitConfiguration(
true, 27, 0, 0);
public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21;
public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22;
public static final double TURRET_SPEED_MULTIPLIER = 0.4d;
public static final int DEGREES_PER_ROT = 0;
public static final double TURRET_SPEED_MULTIPLIER = 0.4;
public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5;
public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844;
public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;
public static final double ENCODER_TICKS_PER_REV = 2048;
public static final double TURRET_CLIMBING_POS = -3.76;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains TURRET_SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SWERVE_SHOOT_GAINS = new Gains(6.0, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
public static final int TURRET_MOTOR_CAN_ID = 19;
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double TURRET_FORWARD_LIMIT = 61.7; // TODO: find
public static final double TURRET_REVERSE_LIMIT = -42.3; // TODO: find
//Gains for turret
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.3/*TURRET_SPEED_MULTIPLIER*/);
public static final double SHOOTER_TURRET_MIN = -0.3;//-TURRET_SPEED_MULTIPLIER;
//Gains for hood
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7);
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values
public static final double TURRET_FORWARD_HARD_LIMIT = 18.429;
public static final double TURRET_REVERSE_HARD_LIMIT = -117.8;
public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 5;
public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2;
public static final double TURRET_SOFT_LIMIT_TOLERANCE = 20.0;
//Shooter gains for actual Drum
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0);
/* Hood Constants */
public static final int SHOOTER_ANGLE_ADJUST_ID = 20;
public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find
public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find
public static final double HOOD_FORWARD_LIMIT = 48.69; // TODO: find
public static final double HOOD_REVERSE_LIMIT = -100; // TODO: find
public static final double HOOD_FORWARD_SOFT_LIMIT = 0.0; // TODO: find
public static final double HOOD_REVERSE_SOFT_LIMIT = -150; // TODO: find
public static final double HOOD_SOFT_LIMIT_TOLERANCE = 20.0;
}
@@ -199,21 +317,25 @@ public final class Constants {
// public static final double TARGET_HEIGHT = 67.5;
// public static final double FOV = 29.8; //Field of view limelight
public static final double LIME_ANGLE = 24.7;
public static final double LIME_ANGLE = 56.4;
public static final String NAME = "photonCamera";
public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?)
public static final double LIME_HEIGHT = 26;
public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?)
public static final double H_FOV = 59.6;
public static final double V_FOV = 49.7;
public static final double LIME_VIXELS = 960;
public static final double LIME_HIXELS = 720;
public static final double V_FOV = 45.7;
public static final double LIME_HIXELS = 920;
public static final double LIME_VIXELS = 720;
public static final double TURRET_kP = 0.1;
public static final double POINTS_THRESHOLD = 400;
public static final double RANGE = 10;
public static final double LIMELIGHT_RADIUS = 1.d;
public static final double EDGE_TO_CENTER = 20;
public static final double LIMELIGHT_RADIUS = 8;
public static final double SHOOTER_CORRECTION = 1.d;
}
}
src/main/java/frc4388/robot/Robot.java
+39 -54
View File
@@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.Vector2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
@@ -27,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.utility.RobotTime;
import frc4388.utility.VelocityCorrection;
import frc4388.utility.desmos.DesmosServer;
/**
* The VM is configured to automatically run this class, and to call the
@@ -41,10 +43,12 @@ public class Robot extends TimedRobot {
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
private double current;
private SendableChooser<Pose2d> odoChooser = new SendableChooser<Pose2d>();
private HashMap<String, Pose2d> odoChoices = new HashMap<>();
private Pose2d selectedOdo;
private static DesmosServer desmosServer = new DesmosServer(8000);
public static Alliance alliance;
/**
* This function is run when the robot is first started up and should be
@@ -116,6 +120,10 @@ public class Robot extends TimedRobot {
return "Not Running";
}
});
desmosServer.start();
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
// DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
}
/**
@@ -130,8 +138,19 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition());
SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition());
// current =
// m_robotContainer.m_robotBoomBoom.getCurrent() +
// m_robotContainer.m_robotClimber.getCurrent(); //+
// m_robotContainer.m_robotHood.getCurrent() +
// m_robotContainer.m_robotIntake.getCurrent() +
// m_robotContainer.m_robotExtender.getCurrent() +
// m_robotContainer.m_robotSerializer.getCurrent() +
// m_robotContainer.m_robotStorage.getCurrent() +
// m_robotContainer.m_robotSwerveDrive.getCurrent();
// m_robotContainer.m_robotTurret.getCurrent();
// SmartDashboard.putNumber("Total Robot Current Draw", current);
// SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
// SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent());
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled
// commands, running already-scheduled commands, removing finished or
@@ -141,44 +160,8 @@ public class Robot extends TimedRobot {
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
// velocity correction tests
VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom);
//SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
//SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
// System.out.println("Position: (" + vc.position.x + ", " + vc.position.y + ")");
// System.out.println("Velocity: (" + vc.cartesianVelocity.x + ", " + vc.cartesianVelocity.y + ")");
// System.out.println("Tangential Velocity: (" + vc.tangentialVelocity.x + ", " + vc.tangentialVelocity.y + ")");
// System.out.println("Radial Velocity: (" + vc.radialVelocity.x + ", " + vc.radialVelocity.y + ")");
SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
// odo chooser stuff
addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)),
new Pose2d(1, 2, new Rotation2d(Math.PI/3)),
new Pose2d(1, 3, new Rotation2d(Math.PI/4)));
updateOdoChooser();
SmartDashboard.putData("Odometry Chooser", odoChooser);
// print odometry data to smart dashboard for debugging (if causing timeout
// errors, you can comment it)
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
}
public void updateOdoChooser() {
for (Map.Entry<String,Pose2d> entry : odoChoices.entrySet()) {
odoChooser.addOption(entry.getKey(), entry.getValue());
}
}
public void addOdoChoices(Pose2d... points) {
for (Pose2d point : points) {
String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + "°)";
odoChoices.put(key, point);
}
}
/**
@@ -190,6 +173,18 @@ public class Robot extends TimedRobot {
public void disabledInit() {
LOGGER.fine("disabledInit()");
m_robotTime.endMatchTime();
if (isTest()) {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
.resolve("recording." + System.currentTimeMillis() + ".path").toFile();
if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// m_robotContainer.createPath(null, null, false).write(outputFile);
LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
}
@Override
@@ -204,11 +199,7 @@ public class Robot extends TimedRobot {
public void autonomousInit() {
LOGGER.fine("autonomousInit()");
selectedOdo = odoChooser.getSelected();
if (selectedOdo == null) {
selectedOdo = m_robotContainer.getOdometry();
}
m_robotContainer.resetOdometry(selectedOdo);
Robot.alliance = DriverStation.getAlliance();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -229,13 +220,8 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
LOGGER.fine("teleopInit()");
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
selectedOdo = odoChooser.getSelected();
if (selectedOdo == null) {
selectedOdo = m_robotContainer.getOdometry();
}
m_robotContainer.resetOdometry(selectedOdo);
Robot.alliance = DriverStation.getAlliance();
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
@@ -252,8 +238,7 @@ public class Robot extends TimedRobot {
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
public void teleopPeriodic() {}
@Override
public void testInit() {
src/main/java/frc4388/robot/RobotContainer.java
+231 -69
View File
@@ -4,8 +4,12 @@
package frc4388.robot;
import java.util.ArrayList;
import java.util.Objects;
import java.util.logging.Logger;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
@@ -15,7 +19,6 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
@@ -26,13 +29,22 @@ import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.PathRecorder;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.robot.commands.ShooterCommands.TrackTarget;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Claws;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Extender;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Serializer;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -44,26 +56,45 @@ import frc4388.utility.controller.DeadbandedXboxController;
*/
public class RobotContainer {
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
/* RobotMap */
// RobotMap
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
public final Climber m_robotClimber = new Climber(m_robotMap.elbow);
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
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);*/
/* Controllers */
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
/* Autonomous */
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
public static boolean softLimits = true;
// control mode switching
public static enum ControlMode { SHOOTER, CLIMBER };
public static ControlMode currentControlMode = ControlMode.SHOOTER;
// turret mode switching
private enum TurretMode { MANUAL, AUTONOMOUS };
private TurretMode currentTurretMode = TurretMode.MANUAL;
// climber mode switching
private enum ClimberMode { MANUAL, AUTONOMOUS };
private ClimberMode currentClimberMode = ClimberMode.MANUAL;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -71,36 +102,60 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
// Swerve Drive with Input
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive));
// Intake with Triggers
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
m_robotSwerveDrive.driveWithInput( 0,
0,
0,
0,
true);
}}
, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
m_robotIntake.setDefaultCommand(
new RunCommand(() -> m_robotIntake.runWithTriggers(
getOperatorController().getLeftTriggerAxis(),
getOperatorController().getRightTriggerAxis()),
m_robotIntake));
// Storage Management
// m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.manageStorage(), m_robotStorage).withName("Storage manageStorage defaultCommand"));
// Serializer Management
// m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// Turret Manual
m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret));
m_robotHood.setDefaultCommand(new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY() * 0.1), m_robotHood));
// m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
// continually sends updates to the Blinkin LED controller to keep the lights on
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
// Serializer Manual
m_robotSerializer.setDefaultCommand(
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// Creates a button on the SmartDashboard that will record the path of the robot.
SmartDashboard.putData("Path Recording", m_pathChooser);
// Turret Manual
m_robotTurret.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
}, m_robotTurret));
// Hood Manual
m_robotHood.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
}, m_robotHood));
//Climber Manual
m_robotClimber.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
}, m_robotClimber));
// autoInit();
// recordInit();
}
/**
@@ -117,7 +172,7 @@ public class RobotContainer {
if (binding == XboxController.Button.kLeftBumper)
/* Left Bumper > Shift Down */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
else if (binding == XboxController.Button.kRightBumper)
/* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
/* Right Bumper > Shift Up */ button.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
else if (binding == XboxController.Button.kLeftStick)
/* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kRightStick)
@@ -127,45 +182,133 @@ public class RobotContainer {
else if (binding == XboxController.Button.kB)
/* B > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kX)
/* X > TEMP */ button.whenPressed(() -> {
m_robotMap.leftFront.reset();
m_robotMap.rightFront.reset();
m_robotMap.leftBack.reset();
m_robotMap.rightBack.reset();
});
/* X > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kY)
/* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kBack)
/* Back > Reset Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
/* Back > Calibrate Odometry */ button.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
else if (binding == XboxController.Button.kStart)
/* Start > Reset Gyro */ button.whenPressed(m_robotSwerveDrive::resetGyro);
/* Start > Calibrate Odometry */ button.whenPressed(m_robotSwerveDrive::resetGyro);
/* ------------------------------------ Operator ------------------------------------ */
button = new JoystickButton(getDriverController(), binding.value);
if (binding == XboxController.Button.kLeftBumper)
/* Left Bumper > Storage Out */ button.whileHeld(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED), m_robotStorage)
.whenReleased(() -> m_robotStorage.runStorage(0.0), m_robotStorage);
/* Left Bumper > Storage In */
button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
else if (binding == XboxController.Button.kRightBumper)
/* Right Bumper > Storage In */ button.whileHeld(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
.whenReleased(() -> m_robotStorage.runStorage(0.0), m_robotStorage);
/* Right Bumper > Storage Out */
button.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
else if (binding == XboxController.Button.kLeftStick)
/* Left Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kRightStick)
/* Right Stick > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kA)
/* A > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
/* A > Spit Out Ball */
button.whileHeld(new RunCommand(m_robotTurret::gotoMidpoint, m_robotTurret))
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
else if (binding == XboxController.Button.kB)
/* B > Reset Hood*/ button.whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0)));
/* B > Toggle Claws */ button.whenPressed(new InstantCommand(m_robotClaws::toggleClaws, m_robotClaws));
else if (binding == XboxController.Button.kX)
/* X > Run Shooter */ button.whileHeld(() -> m_robotBoomBoom.runDrumShooter(0.3))
.whenReleased(() -> m_robotBoomBoom.runDrumShooter(0.0));
/* X > TEST BUTTON */ button.whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
///* X > Toggles extender in and out */ button.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
else if (binding == XboxController.Button.kY)
/* Y > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
/* Y > TEST BUTTON */ button.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
///* Y > Full aim command */ button.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
else if (binding == XboxController.Button.kBack)
/* Back > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == XboxController.Button.kStart)
/* Start > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
}
// Iterate over all button box buttons.
for (ButtonBox.Button binding : ButtonBox.Button.values()) {
/* ---------------------------------- Button Box ---------------------------------- */
JoystickButton button = new JoystickButton(getButtonBox(), binding.value);
if (binding == ButtonBox.Button.kRightSwitch)
/* Right Switch > Unbound */ button.whenPressed(new PrintCommand("Unbound"));
else if (binding == ButtonBox.Button.kMiddleSwitch)
/* Middle Switch > Climber and Shooter mode switching */
button.whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
else if (binding == ButtonBox.Button.kLeftSwitch)
/* Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) */
button.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
else if (binding == ButtonBox.Button.kRightButton)
/* Right Button > Extender Out */
button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
else if (binding == ButtonBox.Button.kLeftButton)
/* Left Button > Extender In */
button.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
}
/**
* Generate autonomous
* @param maxVel max velocity for the path (null to override default value of 5.0)
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
* @param inputs strings (paths) or commands you want to run (in order)
* @return array of commands
*/
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
ArrayList<Command> commands = new ArrayList<Command>();
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// parse input
for (int i=0; i<inputs.length; i++) {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
}
/**
@@ -174,29 +317,40 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
if (m_pathChooser.getPath() != null) {
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
thetaController.enableContinuousInput(-Math.PI, Math.PI);
// if (loadedPathTrajectory != null) {
// PIDController xController = SwerveDriveConstants.X_CONTROLLER;
// PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
// ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
PathPlannerState initialState = m_pathChooser.getPath().getInitialState();
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
return new SequentialCommandGroup(
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
new PPSwerveControllerCommand(m_pathChooser.getPath(), m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
} else {
LOGGER.severe("No auto selected.");
return new RunCommand(() -> {
}).withName("No Autonomous Path");
}
// PathPlannerState initialState = loadedPathTrajectory.getInitialState();
// Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
// return new SequentialCommandGroup(
// new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
// new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
// m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
// m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
// new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
// } else {
// LOGGER.severe("No auto selected.");
// return new RunCommand(() -> {
// }).withName("No Autonomous Path");
// }
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
// ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths.
// * return new ParallelCommandGroup(buildAuto(null,
// * null,
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
}
public XboxController getDriverController() {
public static XboxController getDriverController() {
return m_driverXbox;
}
@@ -204,6 +358,14 @@ public class RobotContainer {
return m_operatorXbox;
}
public ButtonBox getButtonBox() {
return m_buttonBox;
}
public static void setSoftLimits(boolean set) {
softLimits = set;
}
/**
* Get odometry.
*
src/main/java/frc4388/robot/RobotMap.java
+185 -117
View File
@@ -4,20 +4,29 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.CAN;
import edu.wpi.first.networktables.NetworkTableType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.ClimberConstants;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.ShooterConstants;
@@ -36,6 +45,11 @@ public class RobotMap {
// configureLEDMotorControllers();
configureSwerveMotorControllers();
configureShooterMotorControllers();
configureIntakeMotors();
configureExtenderMotors();
configureSerializerMotors();
configureStorageMotors();
configureClimberMotors();
}
/* LED Subsystem */
@@ -43,30 +57,30 @@ public class RobotMap {
// void configureLEDMotorControllers() {}
/* Swerve Subsystem */
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
/* Swerve Subsystem */
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
public SwerveModule leftFront;
public SwerveModule leftBack;
public SwerveModule rightFront;
public SwerveModule rightBack;
void configureSwerveMotorControllers() {
public SwerveModule leftFront;
public SwerveModule leftBack;
public SwerveModule rightFront;
public SwerveModule rightBack;
void configureSwerveMotorControllers() {
leftFrontSteerMotor.configFactoryDefault();
leftFrontWheelMotor.configFactoryDefault();
rightFrontSteerMotor.configFactoryDefault();
@@ -75,106 +89,139 @@ public class RobotMap {
leftBackWheelMotor.configFactoryDefault();
rightBackSteerMotor.configFactoryDefault();
rightBackWheelMotor.configFactoryDefault();
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
NeutralMode mode = NeutralMode.Coast;
leftFrontSteerMotor.setNeutralMode(mode);
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
leftFrontWheelMotor.setNeutralMode(mode);// Coast
rightFrontSteerMotor.setNeutralMode(mode);
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
rightFrontWheelMotor.setNeutralMode(mode);// Coast
leftBackSteerMotor.setNeutralMode(mode);
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
leftBackWheelMotor.setNeutralMode(mode);// Coast
rightBackSteerMotor.setNeutralMode(mode);
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
rightBackWheelMotor.setNeutralMode(mode);// Coast
// current limits
leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder,
SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder,
SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
// config cancoder as remote encoder for swerve steer motors
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
// Shooter Config
/* Boom Boom Subsystem */
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
/* Climb Subsystem */
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
// turret subsystem
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
private void configureClimberMotors() {
elbow.configFactoryDefault();
elbow.setNeutralMode(NeutralMode.Brake);
}
/* Hooks Subsystem */
public final Servo leftClaw = new Servo(1);
public final Servo rightClaw = new Servo(2);
// hood subsystem
public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
// Shooter Config
/* Boom Boom Subsystem */
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
// Create motor CANSparkMax
void configureShooterMotorControllers() {
// turret subsystem
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
// hood subsystem
public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
// Create motor CANSparkMax
void configureShooterMotorControllers() {
// LEFT FALCON
shooterFalconLeft.configFactoryDefault();
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
@@ -183,55 +230,76 @@ public class RobotMap {
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
ShooterConstants.SHOOTER_TIMEOUT_MS);
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
ShooterConstants.SHOOTER_TIMEOUT_MS);
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
ShooterConstants.SHOOTER_TIMEOUT_MS);
// RIGHT FALCON
shooterFalconRight.setInverted(false);
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
shooterFalconRight.configFactoryDefault();
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
shooterFalconRight.setInverted(false);
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
// m_shooterFalconRight.configPeakOutputForward(0,
// ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
ShooterConstants.SHOOTER_TIMEOUT_MS);
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
ShooterConstants.SHOOTER_TIMEOUT_MS);
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.follow(shooterFalconLeft);
// /* Turret Subsytem */
// shooterFalconRight.configStatorCurrentLimit(new
// StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers
// out of our ass anymore
// shooterFalconLeft.configSupplyCurrentLimit(new
// SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull
// numbers out of our ass anymore
// turret
shooterTurret.restoreFactoryDefaults();
shooterTurret.setIdleMode(IdleMode.kBrake);
shooterTurret.setInverted(true);
// hood subsystem
angleAdjusterMotor.restoreFactoryDefaults();
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
}
/* Serializer Subsystem */
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
// public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless);
public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
/* Intake Subsytem */
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
/* Storage Subsystem */
public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
// public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER);
// public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE);
angleAdjusterMotor.setInverted(true);
}
/* Serializer Subsystem */
public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
/* Intake Subsystem */
public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
void configureIntakeMotors() {
intakeMotor.configFactoryDefault();
intakeMotor.setInverted(false);
intakeMotor.setNeutralMode(NeutralMode.Coast);
intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
}
void configureExtenderMotors() {
extenderMotor.restoreFactoryDefaults();
extenderMotor.setInverted(true);
extenderMotor.setIdleMode(IdleMode.kBrake);
}
void configureSerializerMotors() {
serializerBelt.restoreFactoryDefaults();
}
/* Storage Subsystem */
public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
void configureStorageMotors() {
storageMotor.restoreFactoryDefaults();
}
}
src/main/java/frc4388/robot/commands/AimToCenter.java
-85
View File
@@ -1,85 +0,0 @@
// 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.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
public class AimToCenter extends CommandBase {
/** Creates a new AimWithOdometry. */
Turret m_turret;
SwerveDrive m_drive;
VisionOdometry m_visionOdometry;
// use odometry to find x and y later
double x;
double y;
double m_targetAngle;
// public static Gains m_aimGains;
public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) {
// Use addRequirements() here to declare subsystem dependencies.
m_turret = turret;
m_drive = drive;
m_visionOdometry = visionOdometry;
addRequirements(m_turret, m_drive, m_visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = 0;
y = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
m_turret.runshooterRotatePID(m_targetAngle);
// Check if limelight is within range (comment out to disable vision odo)
if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
m_visionOdometry.updateOdometryWithVision();
m_visionOdometry.setLEDs(true);
}
else{
m_visionOdometry.setLEDs(false);
}
}
public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return angle;
}
/**
* Checks if in deadzone.
* @param angle Angle to check.
* @return True if in deadzone.
*/
public static boolean isDeadzone(double angle) {
if (angle == Double.NaN) {
return false;
}
return !((ShooterConstants.TURRET_REVERSE_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_LIMIT));
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java
+61
View File
@@ -0,0 +1,61 @@
// 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.commands.ButtonBoxCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
public class RunMiddleSwitch extends CommandBase {
private static boolean manual = false;
private boolean newManual = false;
private boolean changes = false;
/** Creates a new RunMiddleSwitch. */
public RunMiddleSwitch() {
// Use addRequirements() here to declare subsystem dependencies.
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
changes = (newManual == manual);
if (manual) {
printManual();
} else {
printNormal();
}
newManual = manual;
}
public void printNormal(){
System.out.println("Normal");
}
public void printManual(){
System.out.println("Manual");
}
public static void setManual(boolean set)
{
manual = set;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return changes;
}
}
src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java
+62
View File
@@ -0,0 +1,62 @@
// 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.commands.ButtonBoxCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.Turret;
public class TurretManual extends CommandBase {
// subsystems
private Turret turret;
// booleans
private static boolean manual = false;
private boolean newManual = false;
private boolean changes = false;
/** Creates a new TurretManual. */
public TurretManual(Turret turret) {
// Use addRequirements() here to declare subsystem dependencies.
this.turret = turret;
addRequirements(this.turret);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
changes = (newManual != manual);
if (manual) {
System.out.println("Manual Turret"); // TODO: turret manual controls
} else {
System.out.println("Auto Turret"); // TODO: turret auto controls;
}
newManual = manual;
}
public static void setManual(boolean set)
{
manual = set;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
manual = !manual;
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return changes;
}
}
src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java
+53
View File
@@ -0,0 +1,53 @@
// 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.commands.ClimberCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.subsystems.Claws;
public class RunClaw extends CommandBase {
// parameters
public Claws m_claws;
public Claws.ClawType clawType;
public boolean open;
/**
* Creates a new RunClaw, which runs a claw.
* @param sClaws Claws subsystem.
* @param which Which claw to run.
* @param open Whether to open or close the claw.
*/
public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) {
// Use addRequirements() here to declare subsystem dependencies.
m_claws = sClaws;
clawType = which;
this.open = open;
addRequirements(m_claws);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// m_claws.runClaw(clawType, open);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// return m_claws.checkSwitchAndCurrent(clawType);
return false; // TODO: real return
}
}
src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java
+74
View File
@@ -0,0 +1,74 @@
// 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.commands.ClimberCommands;
import org.opencv.core.Point;
import edu.wpi.first.wpilibj.drive.Vector2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ClimberConstants;
import frc4388.robot.subsystems.Claws;
import frc4388.robot.subsystems.Climber;
import frc4388.utility.Vector2D;
public class RunClimberPath extends CommandBase {
Climber climber;
Claws claws;
Point[] path;
int nextIndex;
boolean endPath;
/** Creates a new RunClimberPath. */
public RunClimberPath(Climber _climber, Claws _claws, Point[] _path) {
path = _path;
endPath = false;
climber = _climber;
claws = _claws;
addRequirements(climber, claws);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
claws.setOpen(true);
nextIndex = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// if(!claws.fullyOpen())
return;
// Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
// Vector2D dir = new Vector2D(path[nextIndex]);
// dir.subtract(new Vector2D(climberPos));
// if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
// nextIndex++;
// else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
// endPath = true;
// claws.setOpen(false);
// } else if(!endPath) {
// dir = dir.unit();
// climber.controlWithInput(dir.x, dir.y);
// }
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
// return endPath && claws.fullyClosed();
return false;
}
}
src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java
+34
View File
@@ -0,0 +1,34 @@
// 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.commands.ExtenderIntakeCommands;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import frc4388.robot.subsystems.Extender;
import frc4388.robot.subsystems.Intake;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ExtenderIntakeGroup extends ParallelRaceGroup {
public static int direction;
/** Creates a new RunExtenderAndIntake. */
public ExtenderIntakeGroup(Intake intake, Extender extender) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely
addCommands(new RunIntakeConditionally(intake), new RunExtender(extender));
}
public static void setDirectionToOut() {
ExtenderIntakeGroup.direction = 1;
}
public static void changeDirection() { // Never implemented?
ExtenderIntakeGroup.direction *= -1;
}
}
src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java
+66
View File
@@ -0,0 +1,66 @@
// 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.commands.ExtenderIntakeCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ExtenderConstants;
import frc4388.robot.subsystems.Extender;
import frc4388.robot.subsystems.Intake;
public class RunExtender extends CommandBase {
private Extender extender;
private double error;
private double tolerance;
/** Creates a new RunExtender. */
public RunExtender(Extender extender) {
// Use addRequirements() here to declare subsystem dependencies.
this.extender = extender;
updateError();
tolerance = 5.0;
addRequirements(this.extender);
}
public void updateError() {
if (ExtenderIntakeGroup.direction > 0) {
this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT);
} else {
this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_REVERSE_LIMIT);
}
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println("RunExtender is working");
this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0);
updateError();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
ExtenderIntakeGroup.changeDirection();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (error < tolerance) {
System.out.println("RunExtender finished");
this.extender.runExtender(0.0);
return true;
}
return false;
}
}
src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java
+47
View File
@@ -0,0 +1,47 @@
// 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.commands.ExtenderIntakeCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.Intake;
public class RunIntakeConditionally extends CommandBase {
private Intake intake;
/** Creates a new RunIntakeConditionally. */
public RunIntakeConditionally(Intake intake) {
// Use addRequirements() here to declare subsystem dependencies.
this.intake = intake;
addRequirements(this.intake);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (ExtenderIntakeGroup.direction > 0) {
this.intake.runAtOutput(-1);
} else {
this.intake.runAtOutput(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/Shoot.java
-195
View File
@@ -1,195 +0,0 @@
// 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.commands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.utility.DummySensor;
import frc4388.utility.Gains;
public class Shoot extends CommandBase {
// subsystems
public SwerveDrive m_swerve;
public BoomBoom m_boomBoom;
public Turret m_turret;
public Hood m_hood;
// given
public double m_gyroAngle;
public double m_odoX;
public double m_odoY;
public double m_distance;
// targets
public double m_targetVel;
public double m_targetHood;
public double m_targetAngle;
public Pose2d m_targetPoint;
// pid
public double error;
public double prevError;
public Gains gains = ShooterConstants.SHOOT_GAINS;
public double kP, kI, kD;
public double proportional, integral, derivative;
public double time;
public double output;
public double normOutput;
public double tolerance;
public boolean isAimedInTolerance;
public int inverted;
// testing
public boolean simMode = true;
public DummySensor driveDummy;
public DummySensor turretDummy;
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
* TODO: Velocity Correction
* @param sDrive Drive Train
* @param sShooter Shooter Drum
* @param sTurret Shooter Turret
* @param sHood Shooter Hood
*/
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
// Use addRequirements() here to declare subsystem dependencies.
m_swerve = sDrive;
m_boomBoom = sShooter;
m_turret = sTurret;
m_hood = sHood;
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
kP = gains.kP;
kI = gains.kI;
kD = gains.kD;
proportional = 0;
integral = 0;
derivative = 0;
time = 0.02;
tolerance = 5.0;
isAimedInTolerance = false;
if (simMode) {
driveDummy = new DummySensor(180);
turretDummy = new DummySensor(180);
DummySensor.resetAll();
}
}
/**
* Updates error for custom PID.
*/
public void updateError() {
m_targetPoint = SwerveDriveConstants.HUB_POSE;
m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
error = (m_targetAngle - turretDummy.get() + 360) % 360;
// error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
isAimedInTolerance = (Math.abs(error) <= tolerance);
if (simMode) {
SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
System.out.println("Target Angle: " + m_targetAngle);
System.out.println("Error: " + error);
}
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_odoX = 0;//m_swerve.getOdometry().getX();
m_odoY = -1;//m_swerve.getOdometry().getY();
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
// get targets (shooter tables)
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
// deadzone processing
if (AimToCenter.isDeadzone(m_targetAngle)) {}
// initial error
updateError();
System.out.println("Error: " + error);
prevError = error;
}
/**
* Run custom PID.
*/
public void runPID() {
if (error > 180){
error = 360 - error;
inverted = -1;
}
else{
inverted = 1;
}
prevError = error;
updateError();
proportional = error;
integral = integral + error * time;
derivative = (error - prevError) / time;
output = kP * proportional + kI * integral + kD * derivative;
normOutput = output/360 * inverted;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (simMode) {
System.out.println("Normalized Output: " + normOutput);
}
// custom pid
runPID();
if (simMode) {
driveDummy.apply(normOutput);
System.out.println("Drive Dummy: " + driveDummy.get());
}
m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the
// entire swerve drive or its the line below
// m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
m_hood.runAngleAdjustPID(m_targetHood);
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
if (simMode) {
turretDummy.apply(normOutput);
System.out.println("Turret Dummy: " + turretDummy.get());
}
m_turret.m_boomBoomRotateMotor.set(normOutput);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (simMode) {
return isAimedInTolerance;
}
return false;
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+118
View File
@@ -0,0 +1,118 @@
// 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.commands.ShooterCommands;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
public class AimToCenter extends CommandBase {
/** Creates a new AimWithOdometry. */
Turret m_turret;
VisionOdometry m_visionOdometry;
Supplier<Pose2d> supplier;
Pose2d odo;
// use odometry to find x and y later
double x;
double y;
double m_targetAngle;
// public static Gains m_aimGains;
public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier<Pose2d> supplier) {
// Use addRequirements() here to declare subsystem dependencies.
m_turret = turret;
m_visionOdometry = visionOdometry;
this.supplier = supplier;
addRequirements(m_turret, m_visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
odo = this.supplier.get();
// ! Yes I realize this stupid, yes it works I promise, coordinate system is funky
x = -odo.getY();
y = -odo.getX();
SmartDashboard.putNumber("trans x", x);
SmartDashboard.putNumber("trans y", y);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
odo = this.supplier.get(); // * update odometry using really cool supplier -aarav
m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360;
SmartDashboard.putNumber("Target Angle", m_targetAngle);
m_turret.runShooterRotatePID(m_targetAngle);
// Check if limelight is within range (comment out to disable vision odo)
if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
// m_visionOdometry.updateOdometryWithVision();
// m_visionOdometry.setLEDs(true);
}
else{
// m_visionOdometry.setLEDs(false);
}
}
public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
// double angle = Math.toDegrees(Math.atan2(y, -x) - gyro);
return (angle - 360);
}
public static double aaravAngleToCenter(double x, double y, double gyro) {
double actualGyro = -gyro + 90;
double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro;
if (-x > 0) { return (180 + exp); }
if (-x < 0) { return (360 + exp); }
if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated
if (x == 0 && y < 0) { return (90 - actualGyro); }
System.err.println("Invalid case.");
return 0;
}
/**
* Checks if in deadzone.
* @param angle Angle to check.
* @return True if in deadzone.
*/
public static boolean isDeadzone(double angle) {
if (angle == Double.NaN) {
return false;
}
return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT));
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+29
View File
@@ -0,0 +1,29 @@
// 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.commands.ShooterCommands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class Seek extends SequentialCommandGroup {
/** Seeks.
* Seeking -> Sought
* @author Aarav Shah
* @blame Aarav Shah (thomas did this)
*/
public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry));
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+217
View File
@@ -0,0 +1,217 @@
// 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.commands.ShooterCommands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants;
import frc4388.robot.Robot;
import frc4388.robot.RobotContainer;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.DummySensor;
import frc4388.utility.Gains;
public class Shoot extends CommandBase {
// subsystems
private SwerveDrive swerve;
private Turret turret;
private BoomBoom drum;
private Hood hood;
private VisionOdometry visionOdometry;
private boolean toShoot;
// given
private double odoX, odoY;
private double distance;
private double gyroAngle;
// targets
private double targetAngle, targetVel, targetHood;
// pid
private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS;
private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS;
private double error;
private double prevError;
private double kP, kI, kD;
private double proportional, integral, derivative;
private double output, normOutput;
private double tolerance;
boolean timerStarted;
long startTime;
private double timeTolerance;
private boolean isAimedInTolerance;
private int inverted;
private double initialSwerveRotation;
private boolean endsWithLimelight;
/**
* Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
*
* @param swerve Drive Train
* @param drum Shooter Drum
* @param turret Shooter Turret
* @param hood Shooter Hood
*
* @author Aarav Shah
*/
public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, boolean toShoot, boolean endsWithLimelight) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.drum = drum;
this.turret = turret;
this.hood = hood;
this.visionOdometry = visionOdometry;
this.toShoot = toShoot;
this.endsWithLimelight = endsWithLimelight;
kP = turretGains.kP;
kI = turretGains.kI;
kD = turretGains.kD;
proportional = 0;
integral = 0;
derivative = 0;
tolerance = 10.0;
timeTolerance = 500;
isAimedInTolerance = false;
this.inverted = 1;
addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry);
}
// public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) {
// this(swerve, drum, turret, hood, false);
// }
/**
* Updates error for custom PID.
*/
public void updateError() {
targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360;
// if (error > 180) {
// error = error - 360;
// this.inverted = -1;
// } else {
// this.inverted = 1;
// }
if (error > 180) { error = error - 360; }
isAimedInTolerance = (Math.abs(error) <= tolerance);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
timerStarted = false;
startTime = 0;
this.odoX = -this.swerve.getOdometry().getY(); // 3.2766
this.odoY = -this.swerve.getOdometry().getX(); // -0.9398
this.distance = Math.hypot(odoX, odoY);
this.gyroAngle = this.swerve.getRegGyro().getDegrees();
this.initialSwerveRotation = gyroAngle;
// get targets (shooter tables)
this.targetVel = drum.getVelocity(distance);
this.targetHood = drum.getHood(distance);
this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
// initial error
updateError();
prevError = error;
}
/**
* Run custom PID.
*/
public void runPID() {
prevError = error;
updateError();
this.proportional = error;
this.integral = integral + (error * Constants.LOOP_TIME);
this.derivative = (error - prevError) / Constants.LOOP_TIME;
this.output = kP * proportional + kI * integral + kD * derivative;
this.normOutput = (output / 360); // inverted;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
runPID();
SmartDashboard.putNumber("Error", this.error);
SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle);
SmartDashboard.putNumber("Normalized Output", this.normOutput);
this.turret.runTurretWithCustomPID(normOutput);
this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative
if (this.toShoot) {
this.hood.runAngleAdjustPID(this.targetHood);
this.drum.runDrumShooterVelocityPID(this.targetVel);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase());
// ? return to initial swerve rotation
// swerve.driveWithInput(0, 0, initialSwerveRotation, true);
// this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true);
// this.turret.m_boomBoomRotateMotor.set(0.0);
// ? should stop the turret and the swerve
////this.swerve.stopModules();
this.turret.runTurretWithInput(0.0);
if (this.toShoot) {
this.hood.runHood(0.0);
this.drum.runDrumShooter(0.0);
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (!endsWithLimelight) {
if (isAimedInTolerance && !timerStarted) {
timerStarted = true;
startTime = System.currentTimeMillis();
}
SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
} else {
return this.visionOdometry.m_camera.getLatestResult().hasTargets();
}
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+164
View File
@@ -0,0 +1,164 @@
// 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.commands.ShooterCommands;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.stream.Collector;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.Vector2D;
import frc4388.utility.desmos.DesmosServer;
public class TrackTarget extends CommandBase {
/** Creates a new TrackTarget. */
SwerveDrive m_swerve;
Turret m_turret;
VisionOdometry m_visionOdometry;
BoomBoom m_boomBoom;
Hood m_hood;
static double velocity;
static double hoodPosition;
ArrayList<Point> points = new ArrayList<>();
private boolean targetLocked = false;
private double velocityTolerance = 100.0;
private double hoodTolerance = 5.0;
boolean isExecuted = false;
public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
m_swerve = swerve;
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
velocity = 0;
hoodPosition = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
try {
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2;
m_turret.runTurretWithInput(output);
double position = m_turret.m_boomBoomRotateEncoder.getPosition();
if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
else
m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
true);
double regressedDistance = getDistance(average.y);
// ! add 30 to the distance to get in target. May need to be adjusted
velocity = m_boomBoom.getVelocity(regressedDistance + 30);
hoodPosition = m_boomBoom.getHood(regressedDistance + 30);
m_boomBoom.runDrumShooterVelocityPID(velocity);
m_hood.runAngleAdjustPID(hoodPosition);
double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
double currentHood = this.m_hood.getEncoderPosition();
targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance);
SmartDashboard.putNumber("Regressed Distance", regressedDistance);
// SmartDashboard.putNumber("Distance", distance);
SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition);
SmartDashboard.putNumber("Vel Target Track", velocity);
SmartDashboard.putBoolean("Target Locked", targetLocked);
} catch (Exception e){
e.printStackTrace();
}
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
Point average = VisionOdometry.averagePoint(points);
HashMap<Point, Double> pointDistances = new HashMap<>();
double distanceSum = 0;
for(Point point : points) {
Vector2D difference = new Vector2D(point);
difference.subtract(new Vector2D(average));
double mag = difference.magnitude();
distanceSum += mag;
pointDistances.put(point, mag);
}
final double averageDist = distanceSum / points.size();
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
}
public final double getDistance(double averageY) {
double y_rot = averageY / VisionConstants.LIME_VIXELS;
y_rot *= Math.toRadians(VisionConstants.V_FOV);
y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2;
y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
double regressedDistance = distanceRegression(distance);
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
return regressedDistance;
}
public final double distanceRegression(double distance) {
return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
// return isExecuted && Math.abs(output) < .1;
}
}
src/main/java/frc4388/robot/commands/StorageCommands/Drum.java
View File
src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java
+76
View File
@@ -0,0 +1,76 @@
// 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.commands.StorageCommands;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.ScheduleCommand;
import frc4388.robot.Robot;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Turret;
public class ManageStorage extends CommandBase {
// subsystems
private Storage storage;
private BoomBoom drum;
private Turret turret;
private Alliance alliance;
private boolean rightColor;
/** Creates a new ManageStorage. */
public ManageStorage(Storage storage, BoomBoom drum, Turret turret) {
// Use addRequirements() here to declare subsystem dependencies.
this.storage = storage;
this.drum = drum;
this.turret = turret;
rightColor = true;
addRequirements(this.storage, this.drum, this.turret);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
checkColor();
if (rightColor) {
this.storage.manageStorage();
} else {
// * CommandScheduler.getInstance().schedule(new ExampleCommand());
// * new ExampleCommand().schedule();
// * new ExampleCommand().execute(); (accompanied by initialize and onFinished)
new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command
}
}
private void checkColor() {
// this.alliance = this.storage.getColor();
// rightColor = this.alliance.equals(Robot.alliance);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java
+72
View File
@@ -0,0 +1,72 @@
// 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.commands.StorageCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Storage;
import frc4388.robot.subsystems.Turret;
public class SpitOutWrongColor extends CommandBase {
// subsystems
private Storage storage;
private BoomBoom drum;
private Turret turret;
// time (in milliseconds)
private long initialTime;
private long elapsedTime;
private long threshold;
private double initialTurret;
private int spitVelocity;
/** Creates a new SpitOutWrongColor. */
public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) {
// Use addRequirements() here to declare subsystem dependencies.
this.storage = storage;
this.drum = drum;
this.turret = turret;
addRequirements(this.storage, this.drum, this.turret);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
initialTime = System.currentTimeMillis();
elapsedTime = 0;
threshold = 2000;
initialTurret = this.turret.getEncoderPosition();
spitVelocity = 2000;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
elapsedTime = System.currentTimeMillis() - initialTime;
this.storage.runStorage(0.9);
this.turret.gotoMidpoint();
this.drum.runDrumShooterVelocityPID(spitVelocity);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
this.storage.runStorage(0.0);
this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (elapsedTime >= threshold);
}
}
src/main/java/frc4388/robot/commands/TrackTarget.java
-97
View File
@@ -1,97 +0,0 @@
// 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.commands;
import java.util.ArrayList;
import org.opencv.core.Point;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.VisionOdometry;
public class TrackTarget extends CommandBase {
/** Creates a new TrackTarget. */
Turret m_turret;
SwerveDrive m_drive;
VisionOdometry m_visionOdometry;
BoomBoom m_boomBoom;
Hood m_hood;
// use odometry to find x and y later
double x;
double y;
double distance;
double vel;
double hood;
double average;
double output;
Pose2d pos = new Pose2d();
ArrayList<Point> points = new ArrayList<>();
// public static Gains m_aimGains;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) {
// Use addRequirements() here to declare subsystem dependencies.
m_turret = turret;
m_drive = drive;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
addRequirements(m_turret, m_drive, m_visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
x = 0;
y = 0;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
double pointTotal = 0;
for(Point point : points)
{
pointTotal = pointTotal + point.x;
}
average = pointTotal/points.size();
output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP;
m_turret.runTurretWithInput(output);
try{
pos = m_visionOdometry.getVisionOdometry();
distance = Math.hypot(pos.getX(), pos.getY());
}
catch (Exception e){
}
vel = m_boomBoom.getVelocity(distance); // has been changed for robot reveal night so this variable is percent output
hood = m_boomBoom.getHood(distance);
m_boomBoom.runDrumShooter(vel);
// m_boomBoom.runDrumShooterVelocityPID(vel);
m_hood.runAngleAdjustPID(hood);
//m_turret.runshooterRotatePID(m_targetAngle);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+48 -14
View File
@@ -20,8 +20,10 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.diffplug.common.base.Errors;
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;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.CSV;
@@ -34,9 +36,11 @@ public class BoomBoom extends SubsystemBase {
public WPI_TalonFX m_shooterFalconRight;
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
public static BoomBoom m_boomBoom;
double speed2;
double velP;
double input;
public double pidOffset = 0;
public boolean m_isDrumReady = false;
public double m_fireVel;
@@ -52,48 +56,64 @@ public class BoomBoom extends SubsystemBase {
private ShooterTableEntry[] m_shooterTable;
/*
* Creates new BoomBoom subsystem, has drum shooter and angle adjuster
*/
/** Creates a new BoomBoom. */
/** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
m_shooterFalconLeft = shooterFalconLeft;
m_shooterFalconRight = shooterFalconRight;
setShooterGains();
m_shooterTable = readShooterTable();
// Run a helper method that logs the contents of the table on a new thread.
new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
}
public Double getVelocity(Double distance) {
// This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
// linear interpolation of the two values (drumVelocity) at the two closest points in the table
// (m_shooterTable) to the given value (distance).
/**
* This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
* linear interpolation of the two values (drumVelocity) at the two closest points in the table
* (m_shooterTable) to the given value (distance).
* @param distance Distance in shooter table
* @return Drum Velocity in units per 100 ms
*/
public Double getVelocity(final Double distance) {
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
public Double getHood(Double distance) {
// This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
// interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
// to the given value (distance).
/**
* This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
* interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
* to the given value (distance).
* @param distance Distance in shooter table
* @return Hood extension in units
*/
public Double getHood(final Double distance) {
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
SmartDashboard.putNumber("Shooter Current", getCurrent());
SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage());
SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
m_hoodSubsystem = subsystem0;
m_turretSubsystem = subsystem1;
}
}
/**
* Runs the Drum motor at a given speed
* @param speed percent output form -1.0 to 1.0
*/
public void runDrumShooter(double speed) {
m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
// m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
m_shooterFalconLeft.set(speed);
SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
}
@@ -106,6 +126,7 @@ public class BoomBoom extends SubsystemBase {
}
public void runDrumShooterVelocityPID(double targetVel) {
SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset);
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
// New BoomBoom controller stuff
@@ -151,4 +172,17 @@ public class BoomBoom extends SubsystemBase {
return new ShooterTableEntry[] { dummyEntry };
}
}
public void updateOffset(double change) {
pidOffset = pidOffset + change;
}
public void increaseSpeed(double amount)
{
speed2 = speed2 + amount;
}
public double getCurrent(){
return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent();
}
}
src/main/java/frc4388/robot/subsystems/Claws.java
+66
View File
@@ -0,0 +1,66 @@
package frc4388.robot.subsystems;
import java.nio.file.ClosedWatchServiceException;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.ClimberConstants;
public class Claws extends SubsystemBase {
public Servo m_leftClaw;
public Servo m_rightClaw;
// public CANSparkMax m_leftClaw;
// public CANSparkMax m_rightClaw;
// private SparkMaxLimitSwitch m_leftLimitSwitchF;
// private SparkMaxLimitSwitch m_rightLimitSwitchF;
// private SparkMaxLimitSwitch m_leftLimitSwitchR;
// private SparkMaxLimitSwitch m_rightLimitSwitchR;
private double m_leftOffset;
private double m_rightOffset;
private boolean m_open;
public static enum ClawType {LEFT, RIGHT}
public Claws(Servo leftClaw, Servo rightClaw) {
m_leftClaw = leftClaw;
m_rightClaw = rightClaw;
m_open = false;
}
/**
* Sets the state of both hooks
* @param open The state of the hooks
*/
public void setOpen(boolean open) {
if(open) {
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);
SmartDashboard.putBoolean("Claws Closed", false);
} else {
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400);
SmartDashboard.putBoolean("Claws Closed", true);
}
}
public void toggleClaws() {
m_open = !m_open;
setOpen(m_open);
}
@Override
public void periodic() {
}
}
src/main/java/frc4388/robot/subsystems/Climber.java
+23
View File
@@ -0,0 +1,23 @@
// 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 edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
private WPI_TalonFX elbow;
/** Creates a new Climber */
public Climber(WPI_TalonFX elbow) { this.elbow = elbow; }
public void setEncoders(double value) { this.elbow.setSelectedSensorPosition(value); }
public double getCurrent() { return this.elbow.getSupplyCurrent(); }
public void setMotors(double elbowOutput) { this.elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); }
@Override
public void periodic() {}
}
src/main/java/frc4388/robot/subsystems/Extender.java
+69
View File
@@ -0,0 +1,69 @@
// 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.led.ColorFlowAnimation.Direction;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ExtenderConstants;
public class Extender extends SubsystemBase {
private CANSparkMax m_extenderMotor;
// private SparkMaxLimitSwitch m_inLimit;
// private SparkMaxLimitSwitch m_outLimit;
/** Creates a new Extender. */
public Extender(CANSparkMax extenderMotor) {
m_extenderMotor = extenderMotor;
// m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_inLimit.enableLimitSwitch(false);
// m_outLimit.enableLimitSwitch(false);
m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT);
m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT);
setExtenderSoftLimits(true);
}
/**
* Set status of extender motor soft limits.
* @param set Boolean to set soft limits to.
*/
public void setExtenderSoftLimits(boolean set) {
m_extenderMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition());
}
public void runExtender(double input) {
// if (!m_serializer.getBeam() && input < 0.) return;
m_extenderMotor.set(input);
}
public double getPosition() {
return m_extenderMotor.getEncoder().getPosition();
}
public void setEncoder(double position) {
m_extenderMotor.getEncoder().setPosition(position);
}
public double getCurrent() {
return m_extenderMotor.getOutputCurrent();
}
}
src/main/java/frc4388/robot/subsystems/Hood.java
+62 -14
View File
@@ -10,7 +10,9 @@ import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.SparkMaxLimitSwitch;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.SparkMaxRelativeEncoder.Type;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
@@ -20,8 +22,8 @@ public class Hood extends SubsystemBase {
public BoomBoom m_shooterSubsystem;
public CANSparkMax m_angleAdjusterMotor;
public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
// public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
// public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
public static Gains m_angleAdjusterGains;
public RelativeEncoder m_angleEncoder;
@@ -30,8 +32,10 @@ public class Hood extends SubsystemBase {
public boolean m_isHoodReady = false;
public double m_fireAngle;
public double m_fireAngle;
public double speedLimiter;
public double calibrationSpeed = 1.0;
/** Creates a new Hood. */
public Hood(CANSparkMax angleAdjusterMotor) {
@@ -41,20 +45,50 @@ public double m_fireAngle;
m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_hoodUpLimitSwitch.enableLimitSwitch(true);
m_hoodDownLimitSwitch.enableLimitSwitch(true);
// m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_hoodUpLimitSwitch.enableLimitSwitch(true);
// m_hoodDownLimitSwitch.enableLimitSwitch(true);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
setHoodSoftLimits(true);
this.speedLimiter = 1.0;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition());
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
runVelocityRamping();
}
public void runVelocityRamping() {
if (areSoftLimitsEnabled()) {
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
}
}
public boolean areSoftLimitsEnabled() {
return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
}
/**
@@ -79,20 +113,34 @@ public double m_fireAngle;
m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
}
/**
* Runs the hood with the given input
* @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle)
*/
public void runHood(double input) {
m_angleAdjusterMotor.set(input);
m_angleAdjusterMotor.set(input * this.speedLimiter * this.calibrationSpeed);
}
public void resetGyroAngleAdj(){
/**
* Run a PID to go to the zero position.
*/
public void gotoZero() {
runAngleAdjustPID(0);
}
public void resetGyroAngleAdj() {
m_angleEncoder.setPosition(0);
}
public double getAnglePosition(){
return 0.0;//m_angleEncoder.getPosition();
public double getEncoderPosition(){
return m_angleEncoder.getPosition();
}
public double getAnglePositionDegrees(){
return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
}
public double getCurrent(){
return m_angleAdjusterMotor.getOutputCurrent();
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+21 -47
View File
@@ -4,74 +4,48 @@
package frc4388.robot.subsystems;
//Imported Limit switch ONLY
import com.revrobotics.SparkMaxLimitSwitch;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.CANSparkMax;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
public class Intake extends SubsystemBase {
private WPI_TalonFX m_intakeMotor;
private CANSparkMax m_extenderMotor;
private Serializer m_serializer;
private SparkMaxLimitSwitch m_inLimit;
private SparkMaxLimitSwitch m_outLimit;
public boolean toggle;
/** Creates a new Intake. */
public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) {
public Intake(WPI_TalonFX intakeMotor) {
m_intakeMotor = intakeMotor;
m_extenderMotor = extenderMotor;
m_serializer = serializer;
m_extenderMotor.restoreFactoryDefaults();
m_intakeMotor.setNeutralMode(NeutralMode.Brake);
m_intakeMotor.setInverted(false);
m_extenderMotor.setInverted(true);
m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_inLimit.enableLimitSwitch(true);
m_outLimit.enableLimitSwitch(true);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Intake Percent Output", m_intakeMotor.get());
SmartDashboard.putNumber("Extender Direction", ExtenderIntakeGroup.direction);
}
/**
* Runs The Intake With Triggers.
* @param leftTrigger Left Trigger to Run -
* @param rightTrigger Right Trigger to Run +
* Runs The Intake With Triggers as input
* @param leftTrigger Left Trigger to Run Inward
* @param rightTrigger Right Trigger to Run Outward
*/
public void runWithTriggers(double leftTrigger, double rightTrigger) {
m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3);
}
/**
* Runs The Extender-
* @param extended Wether the Extender Is Extended
*/
public void runExtender(boolean extended) {
if (!m_serializer.getBeam() && !extended) return;
double extenderMotorSpeed = extended ? 0.25d : -0.25d;
m_extenderMotor.set(extenderMotorSpeed);
m_intakeMotor.set((rightTrigger - leftTrigger) * IntakeConstants.INTAKE_SPEED_MULTIPLIER);
SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent());
SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent());
}
public void runExtender(double input) {
if (!m_serializer.getBeam() && input < 0.) return;
m_extenderMotor.set(input);
public void runAtOutput(double output, double multiplier) {
m_intakeMotor.set(output * multiplier);
}
/**
* Toggles The Extender
*/
public void toggleExtender() {
toggle = !toggle;
runExtender(toggle);
public void runAtOutput(double output) {
m_intakeMotor.set(output * IntakeConstants.INTAKE_SPEED_MULTIPLIER);
}
public double getCurrent() {
return m_intakeMotor.getSupplyCurrent();
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+1
View File
@@ -57,4 +57,5 @@ public class LED extends SubsystemBase {
public LEDPatterns getPattern() {
return m_currentPattern;
}
}
src/main/java/frc4388/robot/subsystems/Serializer.java
+7 -1
View File
@@ -19,7 +19,10 @@ public class Serializer extends SubsystemBase{
// m_serializerShooterBelt.set(0);
}
/**
*
* @param input from -1.0 to 1.0, positive is inward
*/
public void setSerializer(double input){
m_serializerBelt.set(input);
}
@@ -48,4 +51,7 @@ public class Serializer extends SubsystemBase{
public boolean getSerializerState() {
return serializerState;
}
public double getCurrent(){
return m_serializerBelt.getOutputCurrent();
}
}
src/main/java/frc4388/robot/subsystems/Storage.java
+33 -20
View File
@@ -6,57 +6,67 @@ package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.revrobotics.ColorSensorV3;
import edu.wpi.first.wpilibj.util.Color;
public class Storage extends SubsystemBase {
public CANSparkMax m_storageMotor;
private DigitalInput m_beamShooter;
private DigitalInput m_beamIntake;
public ColorSensorV3 m_colorSensor;
/** Creates a new Storage. */
public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) {
public Storage(CANSparkMax storageMotor, ColorSensorV3 colorSensor) {
m_storageMotor = storageMotor;
m_beamShooter = beamShooter;
m_beamIntake = beamIntake;
m_colorSensor = colorSensor;
}
public Storage(CANSparkMax storageMotor) {
m_storageMotor = storageMotor;
m_beamShooter = null;
m_beamIntake = null;
m_colorSensor = null;
}
/**
* If The Beam Is Broken, Run Storage
* If Else, Stop Running Storage
*/
public void manageStorage() {
if (getBeamIntake()) runStorage(0.d);
else runStorage(1.d);
if (getColorBroken()) runStorage(0.d);
else runStorage(0.9);
}
/**
* Runs The Storage at a Specifyed Speed
* @param input The Specifyed Speed
* @param input The value frm -1.0 to 1.0, positive is inwards (towards the shooter)
*/
public void runStorage(double input) {
m_storageMotor.set(input);
}
/**
* Gets The Beam State On The Shooter
* Gets the state of the colorsensor as a beam break
* @return The State Of The Beam on the Shooter
*/
public boolean getBeamShooter(){
return m_beamShooter.get();//True if open
public boolean getColorBroken(){
return (getRed() || getBlue());
}
/**
* Gets The Beam State Of The Intake
* @return The Beam State Of The Intake
*/
public boolean getBeamIntake(){
return m_beamIntake.get(); //True if open
public boolean getRed(){
// return (m_colorSensor.getRed() >= 200 && m_colorSensor.getBlue() < 100 && m_colorSensor.getGreen() < 100);
return (m_colorSensor.getColor() == Color.kRed);
}
public boolean getBlue(){
// return (m_colorSensor.getBlue() >= 200 && m_colorSensor.getRed() < 100 && m_colorSensor.getGreen() < 100);
return (m_colorSensor.getColor() == Color.kBlue);
}
public Alliance getColor() {
return (getRed() ? Alliance.Red : Alliance.Blue);
}
@Override
/**
@@ -65,4 +75,7 @@ public class Storage extends SubsystemBase {
public void periodic() {
//manageStorage();
}
public double getCurrent(){
return m_storageMotor.getOutputCurrent();
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+62 -50
View File
@@ -4,19 +4,15 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import com.ctre.phoenix.sensors.PigeonIMUConfiguration;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
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.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -38,29 +34,19 @@ 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(halfWidth), Units.inchesToMeters(-halfHeight));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(halfHeight));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(-halfHeight));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(halfHeight));
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public WPI_PigeonIMU m_gyro;
protected FusionStatus fstatus = new FusionStatus();
public WPI_Pigeon2 m_gyro;
/*
* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
* The numbers used
* below are robot specific, and should be tuned.
*/
public SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDriveOdometry m_odometry;
// public SwerveDriveOdometry m_odometry;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
@@ -70,7 +56,7 @@ public class SwerveDrive extends SubsystemBase {
private final Field2d m_field = new Field2d();
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
WPI_PigeonIMU gyro) {
WPI_Pigeon2 gyro) {
m_leftFront = leftFront;
m_leftBack = leftBack;
@@ -78,26 +64,25 @@ public class SwerveDrive extends SubsystemBase {
m_rightBack = rightBack;
m_gyro = gyro;
modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack };
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
m_poseEstimator = new SwerveDrivePoseEstimator(
m_gyro.getRotation2d(),
getRegGyro(),//m_gyro.getRotation2d(),
new Pose2d(),
m_kinematics,
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
VecBuilder.fill(Units.degreesToRadians(1)),
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
// m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
}
// https://github.com/ZachOrr/MK3-Swerve-Example
/**
* Method to drive the robot using joystick info.
*
* @link https://github.com/ZachOrr/MK3-Swerve-Example
* @param speeds[0] Speed of the robot in the x direction (forward).
* @param speeds[1] Speed of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
@@ -105,41 +90,53 @@ public class SwerveDrive extends SubsystemBase {
* field.
*/
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
if (speedX == 0 && speedY == 0 && rot == 0)
ignoreAngles = true;
else
ignoreAngles = false;
Translation2d speed = new Translation2d(-speedX, speedY);
ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0);
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, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED);
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
// new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
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).minus(new Rotation2d(0, 1));
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
if (ignoreAngles) {
rot = 0;
}
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds);
// if (ignoreAngles) {
// SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
// for (int i = 0; i < states.length; i ++) {
// SwerveModuleState state = states[i];
// lockedStates[i]= new SwerveModuleState(0, state.angle);
// }
// setModuleStates(lockedStates);
// }
setModuleStates(states);
// SmartDashboard.putNumber("rot", rot);
// SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
}
/**
@@ -150,6 +147,7 @@ public class SwerveDrive extends SubsystemBase {
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
// int i = 2; {
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
@@ -164,9 +162,12 @@ public class SwerveDrive extends SubsystemBase {
updateOdometry();
updateSmartDash();
SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
@@ -174,7 +175,7 @@ public class SwerveDrive extends SubsystemBase {
// odometry
SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
// chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
@@ -206,9 +207,12 @@ public class SwerveDrive extends SubsystemBase {
*
* @return Rotation2d object holding current gyro in radians
*/
public Rotation2d getRegGyro() {
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
return new Rotation2d(regCur * Math.PI / 180);
public Rotation2d getRegGyro() {
// * test chassis regression
// double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
// * new robot regression
double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743;
return new Rotation2d(Math.toRadians(regCur));
}
/**
@@ -260,4 +264,12 @@ public class SwerveDrive extends SubsystemBase {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
public double getCurrent(){
return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
}
public double getVoltage(){
return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+17 -3
View File
@@ -38,6 +38,9 @@ public class SwerveModule extends SubsystemBase {
public double m_currentPos;
public double m_lastPos;
public SwerveModuleState lastState = new SwerveModuleState();
public SwerveModuleState currentState;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
this.driveMotor = driveMotor;
@@ -79,6 +82,7 @@ public class SwerveModule extends SubsystemBase {
// driveMotor.configAllSettings(driveTalonFXConfiguration);
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.sensorCoefficient = 0.087890625;
canCoderConfiguration.magnetOffsetDegrees = offset;
canCoderConfiguration.sensorDirection = true;
canCoder.configAllSettings(canCoderConfiguration);
@@ -90,9 +94,7 @@ public class SwerveModule extends SubsystemBase {
}
private Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback
// coefficient
// and the sensor value reports degrees.
// ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}
@@ -120,6 +122,7 @@ public class SwerveModule extends SubsystemBase {
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}
@@ -181,15 +184,26 @@ public class SwerveModule extends SubsystemBase {
@Override
public void periodic() {
currentState = this.getState();
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
lastState = currentState;
}
public void reset() {
canCoder.setPositionToAbsolute();
// canCoder.configSensorInitializationStrategy(initializationStrategy)
}
public double getCurrent(){
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
}
public double getVoltage(){
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
}
src/main/java/frc4388/robot/subsystems/Turret.java
+172 -27
View File
@@ -18,6 +18,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry;
import frc4388.utility.Gains;
public class Turret extends SubsystemBase {
@@ -29,48 +31,149 @@ public class Turret extends SubsystemBase {
public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID,
// MotorType.kBrushless);
public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS;
// SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit;
public Gyro m_turretGyro;
public double m_targetDistance = 0;
public SparkMaxPIDController m_boomBoomRotatePIDController;
public RelativeEncoder m_boomBoomRotateEncoder;
public boolean m_isAimReady = false;
SparkMaxLimitSwitch m_boomBoomLeftLimit;
SparkMaxLimitSwitch m_boomBoomRightLimit;
SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController();
public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder();
boolean leftState;
boolean recentlyPressed;
boolean leftPrevState = false;
boolean hasLeftSwitchChanged = false;
// Variables
public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument
long leftCurrentTime;
long leftElapsedTime;
public double speedLimiter;
public double calibrationSpeed = 1.0;
public Turret(CANSparkMax boomBoomRotateMotor) {
m_boomBoomRotateMotor = boomBoomRotateMotor;
m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake);
// m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_boomBoomRightLimit.enableLimitSwitch(true);
// m_boomBoomLeftLimit.enableLimitSwitch(true);
// SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
// SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
// setTurretSoftLimits(true);
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true);
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit.enableLimitSwitch(true);
m_boomBoomLeftLimit.enableLimitSwitch(true);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT);
setTurretSoftLimits(true);
this.speedLimiter = 1.0;
setTurretPIDGains();
}
m_boomBoomRotateMotor.setInverted(true);
// public void toggleLeftLimitSwitch() {
// TODO: find better way to do this, but im in a hurry
// if (leftSwitch.isLimitSwitchEnabled()) {
// leftSwitch.enableLimitSwitch(false);
// } else {
// leftSwitch.enableLimitSwitch(true);
// }
// }
// public void turnOnLeftLimitSwitch() {
// SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// System.out.println("Left Switch ENABLED");
// leftSwitch.enableLimitSwitch(true);
// }
// public void turnOffLeftLimitSwitch() {
// SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// System.out.println("Left Switch DISABLED");
// leftSwitch.enableLimitSwitch(false);
// }
/**
* Set gains for turret PIDs.
*/
public void setTurretPIDGains() {
m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
// TODO: add 0.1
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
// SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition());
SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed());
SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed());
// limit switch annoying time thing but actually worked first try wtf
leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed).
hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state.
if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time.
leftCurrentTime = System.currentTimeMillis();
leftElapsedTime = 0;
}
if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false;
if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){
recentlyPressed = true;
m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/);
}
SmartDashboard.putBoolean("Recently Pressed", recentlyPressed);
if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time.
leftElapsedTime = System.currentTimeMillis() - leftCurrentTime;
}
if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position.
m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);
}
leftPrevState = leftState; // * Update the state of the left limit switch.
// * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output).
runVelocityRamping();
}
public void runVelocityRamping() {
if (areSoftLimitsEnabled()) {
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE));
}
if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
}
}
public boolean areSoftLimitsEnabled() {
return this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse);
}
/**
@@ -82,17 +185,42 @@ public class Turret extends SubsystemBase {
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
}
/**
* Set status of turret limit switches.
* @param set Boolean to set limit switches to.
*/
public void setTurretLimitSwitches(boolean set) {
m_boomBoomRightLimit.enableLimitSwitch(set);
m_boomBoomLeftLimit.enableLimitSwitch(set);
}
public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) {
m_boomBoomSubsystem = subsystem0;
m_sDriveSubsystem = subsystem1;
}
/**
* Move the turret with an input
* @param input from -1.0 to 1.0, positive is clockwise
*/
public void runTurretWithInput(double input) {
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed);
}
public void runshooterRotatePID(double targetAngle) {
targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
public void runTurretWithCustomPID(double input) {
m_boomBoomRotateMotor.set(input * this.speedLimiter);
}
public void runShooterRotatePID(double targetAngle) {
double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2;
targetAngle = (targetAngle % 360);
if (targetAngle > softMid) {
targetAngle = targetAngle - 360;
}
targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT;
m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
@@ -100,13 +228,30 @@ public class Turret extends SubsystemBase {
m_boomBoomRotateEncoder.setPosition(0);
}
public double getboomBoomRotatePosition() {
/**
* Run a PID to go to the zero position.
*/
public void gotoZero() {
runShooterRotatePID(0);
}
/**
* Run a PID to go to the midpoint position, between the two soft limits.
*/
public void gotoMidpoint() {
runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT);
}
public double getEncoderPosition() {
return m_boomBoomRotateEncoder.getPosition();
}
public double getBoomBoomAngleDegrees() {
return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
/ ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT);
}
public double getCurrent(){
return m_boomBoomRotateMotor.getOutputCurrent();
}
}
src/main/java/frc4388/robot/subsystems/Vision.java
+2 -2
View File
@@ -86,10 +86,10 @@ public void track(){
public void checkFinished(){
if (xAngle < 0.5 && xAngle > -0.5 && target == 1){
m_turret.m_isAimReady = true;
// m_turret.m_isAimReady = true;
}
else{
m_turret.m_isAimReady = false;
// m_turret.m_isAimReady = false;
}
}
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+28 -19
View File
@@ -31,7 +31,7 @@ import frc4388.utility.VisionObscuredException;
*/
public class VisionOdometry extends SubsystemBase {
// roborio ip & port: 10.43.88.2:1735
private PhotonCamera m_camera;
public PhotonCamera m_camera;
private SwerveDrive m_drive;
private Turret m_shooter;
@@ -54,13 +54,16 @@ public class VisionOdometry extends SubsystemBase {
* Breaks down targets into 4 corners and uses the top 2 points
*
* @return Vision points on the rim of the target in screen space
* @throws VisionObscuredException
*/
public ArrayList<Point> getTargetPoints() {
public ArrayList<Point> getTargetPoints() throws VisionObscuredException {
PhotonPipelineResult result = m_camera.getLatestResult();
latency = result.getLatencyMillis();
System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
if(!result.hasTargets())
return new ArrayList<Point>();
throw new VisionObscuredException();
ArrayList<Point> points = new ArrayList<>();
@@ -90,16 +93,11 @@ public class VisionOdometry extends SubsystemBase {
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
/** Gets estimated odometry based on limelight data
*
* @return The estimated odometry pose, including gyro rotation
* @throws VisionObscuredException
*/
public Pose2d getVisionOdometry() throws VisionObscuredException {
public Point getTargetOffset() throws VisionObscuredException {
ArrayList<Point> screenPoints = getTargetPoints();
if(screenPoints.size() < 3)
throw new VisionObscuredException("Not enough vision points available");
throw new VisionObscuredException("Less than 3 vision points available");
ArrayList<Point3> points3d = get3dPoints(screenPoints);
ArrayList<Point> points = topView(points3d);
@@ -110,14 +108,25 @@ public class VisionOdometry extends SubsystemBase {
guess = iterateGuess(guess, points);
}
guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees());
guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
return guess;
}
SmartDashboard.putNumber("Vision ODO x: ", guess.x);
SmartDashboard.putNumber("Vision ODO y: ", guess.y);
/** Gets estimated odometry based on limelight data
*
* @return The estimated odometry pose, including gyro rotation
* @throws VisionObscuredException
*/
public Pose2d getVisionOdometry() throws VisionObscuredException {
Point targetOffset = getTargetOffset();
targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees());
targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees());
SmartDashboard.putNumber("Vision ODO x: ", targetOffset.x);
SmartDashboard.putNumber("Vision ODO y: ", targetOffset.y);
Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees()));
Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation);
Pose2d odometryPose = new Pose2d(targetOffset.x, targetOffset.y, rotation);
return odometryPose;
}
@@ -125,9 +134,9 @@ public class VisionOdometry extends SubsystemBase {
public double getLatency() {
return latency;
}
public boolean getLEDs() {
if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false;
return true;
return m_camera.getLEDMode() != VisionLEDMode.kOff;
}
public void updateOdometryWithVision(){
@@ -139,6 +148,7 @@ public class VisionOdometry extends SubsystemBase {
err.printStackTrace();
}
}
/** Reverse 3d projects target points from screen coordinates to 3d space
* <p>
* Uses the known height of the target to project points
@@ -284,9 +294,8 @@ public class VisionOdometry extends SubsystemBase {
* @return The angle corrected for the quadrent
*/
public static final double correctQuadrent(double angle, Point guess, Point circlePoint) {
if(circlePoint.x - guess.x < 0) {
if(circlePoint.x - guess.x < 0)
return angle - Math.PI;
}
return angle;
}