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 branch 'testRoboReveal' into Climber

Browse Source
This commit is contained in:
Ryan Manley
2022-03-18 13:04:20 -06:00
committed by GitHub
parent 9844b11b44 fbd74f72ce
commit 1f357082b9
42 changed files with 2527 additions and 1029 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/ShooterData.csv
+11 -2
View File
@@ -1,3 +1,12 @@
Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
0 ,16 ,0
999 ,28.8 ,1200
0 ,-29.5 ,8000
78.5 ,-29.5 ,8000
99 ,-39.62 ,8500
127.25 ,-49.12 ,9500
150 ,-66.22 ,10000
164.5 ,-75.52 ,10000
186 ,-76.24 ,10000
207 ,-104.07 ,11000
227 ,-105.32 ,11500
255.5 ,-105.8 ,13500
999 ,-105.8 ,13500
1 Distance (in) Hood Ext. (u) Drum Velocity (u/ds)
2 0 16 -29.5 0 8000
3 999 78.5 28.8 -29.5 1200 8000
4 99 -39.62 8500
5 127.25 -49.12 9500
6 150 -66.22 10000
7 164.5 -75.52 10000
8 186 -76.24 10000
9 207 -104.07 11000
10 227 -105.32 11500
11 255.5 -105.8 13500
12 999 -105.8 13500
src/main/deploy/VelocityCorrectionData.csv
+12
View File
@@ -0,0 +1,12 @@
Distance (in) ,Duration (s)
0 ,0
78.5 ,0
99 ,0
127.25 ,0
150 ,0
164.5 ,0
186 ,0
207 ,0
227 ,0
255.5 ,0
999 ,0
1 Distance (in) Duration (s)
2 0 0
3 78.5 0
4 99 0
5 127.25 0
6 150 0
7 164.5 0
8 186 0
9 207 0
10 227 0
11 255.5 0
12 999 0
src/main/java/frc4388/robot/Constants.java
+92 -62
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;
@@ -12,7 +15,7 @@ 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.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
@@ -33,28 +36,27 @@ public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048;
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4;
public static final double WHEEL_SPEED = 0.1;
public static final double WIDTH = 15.25;
public static final double HEIGHT = 15.25;
public static final double ROTATION_SPEED = 4.0;
public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant?
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
@@ -68,10 +70,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 - 180 - 90) % 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;
@@ -107,7 +109,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;
@@ -115,9 +116,20 @@ 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;
// TODO: put in real numbers for the hub
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
}
@@ -125,17 +137,28 @@ public final class Constants {
public static final double SERIALIZER_BELT_SPEED = 0.1d;
// CAN IDs
public static final int SERIALIZER_BELT = 16;
public static final int SERIALIZER_BELT = 17;
public static final int SERIALIZER_BELT_BEAM = 27; // TODO
}
public static final class IntakeConstants {
// CAN IDs
public static final int INTAKE_MOTOR = 14;
public static final int EXTENDER_MOTOR = 15;
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 = 17;
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.3;
@@ -217,8 +240,10 @@ public final class Constants {
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 {
@@ -228,47 +253,48 @@ 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 int SHOOTER_FALCON_LEFT_CAN_ID = 23;
public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24;
public static final int SHOOTER_ROTATE_ID = 31; // TODO: find this value
public static final double TURRET_SPEED_MULTIPLIER = 0.1d;
public static final int DEGREES_PER_ROT = 0;
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.8;
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;
// Shoot Command Constants
public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0);
/* Turret Constants */
// ID
public static final int TURRET_MOTOR_CAN_ID = 30;
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 float TURRET_FORWARD_LIMIT = 0; // TODO: find
public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find
public static final int TURRET_MOTOR_CAN_ID = 19;
//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);
// deadzones
public static final double HARD_DEADZONE_LEFT = 0.0;
public static final double HARD_DEADZONE_RIGHT = 340.0;
public static final double TURRET_FORWARD_HARD_LIMIT = 18.429;
public static final double TURRET_REVERSE_HARD_LIMIT = -106.454;
public static final double DIG_DEADZONE_LEFT = 40.0;
public static final double DIG_DEADZONE_RIGHT = 60.0;
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 int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find
public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; // "//
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values
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 = 32;
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 float HOOD_FORWARD_LIMIT = 0; // TODO: find
public static final float HOOD_REVERSE_LIMIT = 0; // 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;
}
@@ -279,21 +305,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; // Convert to metric
public static final double TARGET_RADIUS = 4*12; // Convert to metric
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
+79 -4
View File
@@ -7,20 +7,27 @@ package frc4388.robot;
import java.io.File;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.stream.IntStream;
import com.diffplug.common.base.Errors;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
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;
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
@@ -36,6 +43,15 @@ public class Robot extends TimedRobot {
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
private SendableChooser<Pose2d> odoChooser = new SendableChooser<Pose2d>();
private HashMap<String, Pose2d> odoChoices = new HashMap<>();
private Pose2d selectedOdo;
private double current;
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
* used for any initialization code.
@@ -106,6 +122,9 @@ public class Robot extends TimedRobot {
return "Not Running";
}
});
desmosServer.start();
// DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
}
/**
@@ -120,6 +139,21 @@ 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
@@ -134,6 +168,25 @@ public class Robot extends TimedRobot {
// 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());
// 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);
}
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);
}
}
/**
@@ -155,6 +208,8 @@ public class Robot extends TimedRobot {
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
m_robotContainer.m_robotVisionOdometry.setLEDs(false);
}
@Override
@@ -168,6 +223,15 @@ public class Robot extends TimedRobot {
@Override
public void autonomousInit() {
LOGGER.fine("autonomousInit()");
Robot.alliance = DriverStation.getAlliance();
selectedOdo = odoChooser.getSelected();
if (selectedOdo == null) {
selectedOdo = m_robotContainer.getOdometry();
}
m_robotContainer.resetOdometry(selectedOdo);
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
@@ -187,7 +251,19 @@ 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());
Robot.alliance = DriverStation.getAlliance();
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);
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
@@ -203,8 +279,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
+324 -251
View File
@@ -29,12 +29,16 @@ import java.util.regex.Matcher;
import java.util.regex.Pattern;
import java.util.stream.Collectors;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.diffplug.common.base.Errors;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
import org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -51,6 +55,7 @@ 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.NotifierCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -62,7 +67,20 @@ import frc4388.robot.subsystems.Claws.ClawType;
import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.Shoot;
import frc4388.robot.commands.TrackTarget;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.AimToCenter;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.robot.commands.ShooterCommands.TrackTarget;
import frc4388.robot.commands.StorageCommands.ManageStorage;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Extender;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.LED;
@@ -78,6 +96,7 @@ import frc4388.utility.ListeningSendableChooser;
import frc4388.utility.PathPlannerUtil;
import frc4388.utility.Vector2D;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -89,32 +108,38 @@ import frc4388.utility.controller.DeadbandedXboxController;
*/
public class RobotContainer {
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
// RobotMap
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
private 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);
// Subsystems
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);
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);
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
// private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
// private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer);
// private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter);
// //private final LED m_robotLED = new LED(m_robotMap.LEDController);
// private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
// private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
// private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
// private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
/* Controllers */
private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31);
//public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor);
// Controllers
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
/* Autonomous */
// Autonomous
private PathPlannerTrajectory loadedPathTrajectory = null;
// private final ListeningSendableChooser<File> autoChooser = new ListeningSendableChooser<>(this::loadPath);
private final List<Waypoint> pathPoints = new ArrayList<>();
@@ -128,6 +153,8 @@ public class RobotContainer {
// Function that removes the ".path" from the end of a string.
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern
.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
public static boolean softLimits = true;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -147,39 +174,52 @@ public class RobotContainer {
// new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(),
// getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Turret default command%
//m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
// m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret));
// //Swerve Drive
// m_robotSwerveDrive.setDefaultCommand(
// new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
// getDriverController().getLeftX(),
// getDriverController().getLeftY(),
// getDriverController().getRightX(),
// getDriverController().getRightY(),
// true),
// m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// //Intake with Triggers
// m_robotIntake.setDefaultCommand(
// new RunCommand(() -> m_robotIntake.runWithTriggers(
// getOperatorController().getLeftTriggerAxis(),
// getOperatorController().getRightTriggerAxis()),
// m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
// //Storage Management
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
getDriverController().getLeftX(),
getDriverController().getLeftY(),
//getDriverController().getRightX(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true),
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
m_robotIntake.setDefaultCommand(
new RunCommand(() -> m_robotIntake.runWithTriggers(
getOperatorController().getLeftTriggerAxis(),
getOperatorController().getRightTriggerAxis()),
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
// m_robotStorage.setDefaultCommand(
// new RunCommand(() -> m_robotStorage.manageStorage(),
// m_robotStorage).withName("Storage manageStorage defaultCommand"));
// //Serializer Management
// m_robotSerializer.setDefaultCommand(
// new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(),
// m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// new ManageStorage(m_robotStorage,
// m_robotBoomBoom,
// m_robotTurret).withName("Storage ManageStorage defaultCommand"));
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber)
// );
// 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(getOperatorController().getLeftTriggerAxis() * 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).withName("Turret runTurretWithInput defaultCommand"));
m_robotHood.setDefaultCommand(
new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
// 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"));
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
// autoInit();
// recordInit();
}
@@ -193,88 +233,112 @@ public class RobotContainer {
private void configureButtonBindings() {
/* Driver Buttons */
// "XboxController.Button.kBack" was undefined yet, 7 works just fine
// new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
// .whenPressed(m_robotSwerveDrive::resetGyro);
// new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// // new XboxControllerRawButton(m_driverXbox,
// // XboxControllerRaw.LEFT_BUMPER_BUTTON)
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
// Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
// Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
.whenPressed(m_robotSwerveDrive::resetGyro);
// Left Bumper > Shift Down
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
// Right Bumper > Shift Up
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
// new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
// // new XboxControllerRawButton(m_driverXbox,
// // XboxControllerRaw.RIGHT_BUMPER_BUTTON)
// .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
// new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp
// .whenPressed(() -> m_robotMap.leftFront.reset())
// .whenPressed(() -> m_robotMap.rightFront.reset())
// .whenPressed(() -> m_robotMap.leftBack.reset())
// .whenPressed(() -> m_robotMap.rightBack.reset());
/* Operator Buttons */
// run claws
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2)))
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2)))
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2)))
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2)))
// .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
//Toggles extender in and out
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
.whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
// Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// // Left Bumper > Storage Out (note: neccessary?)
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// A > Shoot with Odo
/*new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret));
//B > Shoot with Lime
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
.whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
// .whileHeld%
/* Button Box Buttons */
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), 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));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
// .whileHeld(new TurretManual(m_robotTurret));
// control turret manual mode
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
// .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
// .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
/*
* // activates "BoomBoom"
* new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
* .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
* m_robotHood));
*/
//Extender
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whenPressed(() -> m_robotIntake.runExtender(true));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(() -> m_robotIntake.runExtender(false));
// //Storage
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))
// .whenReleased(() -> m_robotStorage.runStorage(0.0));
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
// .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
// .whenReleased(() -> m_robotStorage.runStorage(0.0));
// //Shooter
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
// }
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
@@ -308,6 +372,18 @@ public class RobotContainer {
return m_driverXbox;
}
public XboxController getOperatorController() {
return m_operatorXbox;
}
public ButtonBox getButtonBox() {
return m_buttonBox;
}
public static void setSoftLimits(boolean set) {
softLimits = set;
}
/**
* Get odometry.
*
@@ -326,10 +402,6 @@ public class RobotContainer {
// m_robotSwerveDrive.resetOdometry(pose);
// }
public XboxController getOperatorController() {
return m_operatorXbox;
}
/**
* Creates a WatchKey for the path planner directory and registers it with the
* WatchService.
@@ -362,144 +434,145 @@ public class RobotContainer {
* Creates a button on the SmartDashboard that will record the path of the
* robot.
*/
// public void recordInit() {
// SmartDashboard.putData("Recording",
// new RunCommand(this::recordPeriodic) {
// @Override
// public void end(boolean interupted) {
// new InstantCommand(RobotContainer.this::saveRecording) {
// @Override
// public boolean runsWhenDisabled() {
// return true;
// }
// }.withName("Save Recording").schedule();
// }
// }.withName("Record Path (Cancel to Save)"));
// }
// /**
// * Called when a file is created, modified, or deleted.
// * Adds newly created .path files to the SendableChooser.
// * Reloads the path if the currently selected file is modified.
// *
// * @param watchKey The WatchKey that is being observed.
// */
// private void updateAutoChooser(WatchKey watchKey) {
// List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
// if (!watchEvents.isEmpty()) {
// List<WatchEvent<?>> pathWatchEvents = watchEvents.stream()
// .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
// for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
// Path watchEventPath = (Path) pathWatchEvent.context();
// File watchEventFile = watchEventPath.toFile();
// String watchEventFileName = watchEventFile.getName();
// if (watchEventFileName.endsWith(".path")) {
// if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
// LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
// watchEventFileName);
// autoChooser.addOption(watchEventFile.getName(), watchEventFile);
// } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
// LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
// if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
// LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
// loadPath(watchEventFileName);
// }
// } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
// LOGGER.log(Level.SEVERE,
// "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
// watchEventFileName);
// }
// }
// }
// }
// if (!watchKey.reset())
// LOGGER.severe("File watch key invalid.");
// }
public void recordInit() {
SmartDashboard.putData("Recording",
new RunCommand(this::recordPeriodic) {
@Override
public void end(boolean interupted) {
new InstantCommand(RobotContainer.this::saveRecording) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Save Recording").schedule();
}
}.withName("Record Path (Cancel to Save)"));
}
// private void loadPath(String pathName) {
// LOGGER.warning("Loading path " + pathName);
// loadedPathTrajectory = null;
// loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")),
// SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
// LOGGER.info("Done loading");
// }
/**
* Called when a file is created, modified, or deleted.
* Adds newly created .path files to the SendableChooser.
* Reloads the path if the currently selected file is modified.
*
* @param watchKey The WatchKey that is being observed.
*/
private void updateAutoChooser(WatchKey watchKey) {
List<WatchEvent<?>> watchEvents = watchKey.pollEvents();
if (!watchEvents.isEmpty()) {
List<WatchEvent<?>> pathWatchEvents = watchEvents.stream()
.filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
for (WatchEvent<?> pathWatchEvent : pathWatchEvents) {
Path watchEventPath = (Path) pathWatchEvent.context();
File watchEventFile = watchEventPath.toFile();
String watchEventFileName = watchEventFile.getName();
if (watchEventFileName.endsWith(".path")) {
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
watchEventFileName);
autoChooser.addOption(watchEventFile.getName(), watchEventFile);
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
if (watchEventFileName.equals(autoChooser.getSelected().getName())) {
LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
loadPath(watchEventFileName);
}
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
LOGGER.log(Level.SEVERE,
"PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
watchEventFileName);
}
}
}
}
if (!watchKey.reset())
LOGGER.severe("File watch key invalid.");
}
// private void saveRecording() {
// // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
// File outputFile = PATHPLANNER_DIRECTORY
// .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
// LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
// if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// // TODO: Change to use measured maximum velocity and acceleration.
// var path = createPath(null, null, false);
// if (RobotBase.isReal())
// path.write(outputFile);
// StringWriter writer = new StringWriter();
// path.write(writer);
// recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
// autoChooser.setDefaultOption(outputFile.getName(), outputFile);
// LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
// } else
// LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
// }
private void loadPath(String pathName) {
LOGGER.warning("Loading path " + pathName);
loadedPathTrajectory = null;
loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")),
SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
LOGGER.info("Done loading");
}
// // public void recordPeriodic() {
// // Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
// // Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// // // FIXME: Chassis speeds are created from joystick inputs and do not reflect
// // // actual robot velocity.
// // Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
// // m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
// // Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
// // SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
// // pathPoints.add(waypoint);
// // }
private void saveRecording() {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = PATHPLANNER_DIRECTORY
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// TODO: Change to use measured maximum velocity and acceleration.
var path = createPath(null, null, false);
if (RobotBase.isReal())
path.write(outputFile);
StringWriter writer = new StringWriter();
path.write(writer);
recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
autoChooser.setDefaultOption(outputFile.getName(), outputFile);
LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
} else
LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
}
// public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
// // Remove points whose angles to neighboring points are less than 10 degrees
// // apart.
// int j = 0;
// for (int i = 1; i < pathPoints.size() - 1; i++) {
// var prev = pathPoints.get(j).anchorPoint.orElseThrow();
// var current = pathPoints.get(i).anchorPoint.orElseThrow();
// var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
// var fromPrevious = current.minus(prev);
// var toNext = next.minus(current);
// var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
// var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
// if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE
// || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE
// && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
// pathPoints.set(i, null);
// else
// j = i;
// }
// pathPoints.removeIf(Objects::isNull);
// // Make control points
// pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(),
// pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
// for (int i = 1; i < pathPoints.size() - 1; i++) {
// var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(),
// pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
// pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
// pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
// }
// pathPoints.get(pathPoints.size() - 1).prevControl = Optional
// .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(),
// pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
// // Create the path
// PathPlannerUtil.Path path = new PathPlannerUtil.Path();
// path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
// path.maxVelocity = Optional.ofNullable(maxVelocity);
// path.maxAcceleration = Optional.ofNullable(maxAcceleration);
// path.isReversed = Optional.ofNullable(isReversed);
// pathPoints.clear();
// return path;
// }
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
// actual robot velocity.
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
m_robotSwerveDrive.getChassisSpeeds()[1]);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
pathPoints.add(waypoint);
}
// private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current,
// Translation2d next) {
// var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
// return Pair.of(current.minus(line), current.plus(line));
// }
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
// Remove points whose angles to neighboring points are less than 10 degrees
// apart.
int j = 0;
for (int i = 1; i < pathPoints.size() - 1; i++) {
var prev = pathPoints.get(j).anchorPoint.orElseThrow();
var current = pathPoints.get(i).anchorPoint.orElseThrow();
var next = pathPoints.get(i + 1).anchorPoint.orElseThrow();
var fromPrevious = current.minus(prev);
var toNext = next.minus(current);
var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE
|| (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE
&& pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
pathPoints.set(i, null);
else
j = i;
}
pathPoints.removeIf(Objects::isNull);
// Make control points
pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(),
pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
for (int i = 1; i < pathPoints.size() - 1; i++) {
var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(),
pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
}
pathPoints.get(pathPoints.size() - 1).prevControl = Optional
.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(),
pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
// Create the path
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
path.maxVelocity = Optional.ofNullable(maxVelocity);
path.maxAcceleration = Optional.ofNullable(maxAcceleration);
path.isReversed = Optional.ofNullable(isReversed);
pathPoints.clear();
return path;
}
private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current,
Translation2d next) {
var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
return Pair.of(current.minus(line), current.plus(line));
}
}
src/main/java/frc4388/robot/RobotMap.java
+76 -24
View File
@@ -10,16 +10,18 @@ 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_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;
@@ -40,15 +42,19 @@ import frc4388.robot.subsystems.SwerveModule;
public class RobotMap {
public RobotMap() {
configureLEDMotorControllers();
// configureLEDMotorControllers();
configureSwerveMotorControllers();
configureShooterMotorControllers();
configureIntakeMotors();
configureExtenderMotors();
configureSerializerMotors();
configureStorageMotors();
}
/* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() {}
// void configureLEDMotorControllers() {}
/* Swerve Subsystem */
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
@@ -143,6 +149,28 @@ public class RobotMap {
rightBackSteerMotor.setNeutralMode(mode);
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);
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
@@ -165,7 +193,7 @@ public class RobotMap {
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
}
/* Climb Subsystem */
public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
@@ -186,7 +214,7 @@ public class RobotMap {
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
// hood subsystem
public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
// Create motor CANSparkMax
void configureShooterMotorControllers() {
@@ -202,13 +230,15 @@ public class RobotMap {
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
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,
@@ -217,34 +247,56 @@ public class RobotMap {
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
ShooterConstants.SHOOTER_TIMEOUT_MS);
// /* 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
shooterFalconRight.follow(shooterFalconLeft);
// turret
shooterTurret.restoreFactoryDefaults();
shooterTurret.setIdleMode(IdleMode.kBrake);
shooterTurret.setInverted(true);
// hood subsystem
angleAdjusterMotor.restoreFactoryDefaults();
angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
angleAdjusterMotor.setInverted(true);
}
/* 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 Subsystem */
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);
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/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/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/AimToCenter.java → src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+9 -15
View File
@@ -2,7 +2,7 @@
// 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;
package frc4388.robot.commands.ShooterCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ShooterConstants;
@@ -43,7 +43,7 @@ public class AimToCenter extends CommandBase {
@Override
public void execute() {
m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
m_turret.runshooterRotatePID(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){
@@ -61,21 +61,15 @@ public class AimToCenter extends CommandBase {
}
/**
* Checks if in hardware deadzone (due to mechanical limitations).
* Checks if in deadzone.
* @param angle Angle to check.
* @return True if in hardware deadzone.
* @return True if in deadzone.
*/
public static boolean isHardwareDeadzone(double angle) {
return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
}
/**
* Checks if in digital deadzone (due to climber).
* @param angle Angle to check.
* @return True if in digital deadzone.
*/
public static boolean isDigitalDeadzone(double angle) {
return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
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.
src/main/java/frc4388/robot/commands/Shoot.java → src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+64 -35
View File
@@ -2,14 +2,14 @@
// 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;
package frc4388.robot.commands.ShooterCommands;
import edu.wpi.first.hal.simulation.SimulatorJNI;
import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;
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;
@@ -35,21 +35,25 @@ public class Shoot extends CommandBase {
public double m_targetVel;
public double m_targetHood;
public double m_targetAngle;
public double m_driveTargetAngle;
public Pose2d m_targetPoint;
// pid
public double error;
public double prevError;
public Gains shootGains = ShooterConstants.SHOOT_GAINS;
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 DummySensor dummy = new DummySensor(0);
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
@@ -68,9 +72,9 @@ public class Shoot extends CommandBase {
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
kP = shootGains.kP;
kI = shootGains.kI;
kD = shootGains.kD;
kP = gains.kP;
kI = gains.kI;
kD = gains.kD;
proportional = 0;
integral = 0;
@@ -80,24 +84,37 @@ public class Shoot extends CommandBase {
tolerance = 5.0;
isAimedInTolerance = false;
DummySensor.resetAll();
if (simMode) {
driveDummy = new DummySensor(180);
turretDummy = new DummySensor(180);
DummySensor.resetAll();
}
}
/**
* Updates error for custom PID.
*/
public void updateError() {
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
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);
SmartDashboard.putBoolean("isAimed?", isAimedInTolerance);
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 = m_swerve.getOdometry().getX();
m_odoY = m_swerve.getOdometry().getY();
m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
m_odoX = 0;//m_swerve.getOdometry().getX();
m_odoY = -1;//m_swerve.getOdometry().getY();
m_gyroAngle = m_swerve.getRegGyro().getDegrees();
@@ -105,22 +122,10 @@ public class Shoot extends CommandBase {
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
// target angle tests
m_gyroAngle = 0;
m_odoX = -1;
m_odoY = 1;
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
// deadzone processing
if (AimToCenter.isHardwareDeadzone(m_targetAngle)) {
m_targetAngle = m_targetAngle + 20;
}
if (AimToCenter.isDigitalDeadzone(m_targetAngle)) {
// this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work
m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true);
}
if (AimToCenter.isDeadzone(m_targetAngle)) {}
// initial error
updateError();
@@ -131,6 +136,13 @@ public class Shoot extends CommandBase {
* Run custom PID.
*/
public void runPID() {
if (error > 180){
error = 360 - error;
inverted = -1;
}
else{
inverted = 1;
}
prevError = error;
updateError();
@@ -138,20 +150,34 @@ public class Shoot extends CommandBase {
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() {
// custom pid
if (simMode) {
System.out.println("Normalized Output: " + normOutput);
}
// custom pid
runPID();
// m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
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_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.
@@ -161,6 +187,9 @@ public class Shoot extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
// if (simMode) {
return isAimedInTolerance;
// }
// return false;
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+168
View File
@@ -0,0 +1,168 @@
// 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 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.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. */
Turret m_turret;
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<>();
private boolean targetLocked = false;
private double velocityTolerance = 100.0;
private double hoodTolerance = 5.0;
double m=0;
double b=0;
boolean isExecuted = false;
// public static Gains m_aimGains;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
// Use addRequirements() here to declare subsystem dependencies.
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() {
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());
SmartDashboard.putBoolean("Target Locked", this.targetLocked);
try {
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
Point average = VisionOdometry.averagePoint(points);
for(Point point : points) {
Vector2D difference = new Vector2D(point);
difference.subtract(new Vector2D(average));
if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
points.remove(point);
}
average = VisionOdometry.averagePoint(points);
DesmosServer.putPoint("average", average);
for(int i = 0; i < points.size(); i++) {
DesmosServer.putPoint("Point" + i, points.get(i));
}
output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 4;
// output *= 0.5;
DesmosServer.putDouble("output", output);
m_turret.runTurretWithInput(output);
double y_rot = average.y / 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);
DesmosServer.putDouble("distance", distance);
updateRegressionDesmos();
double regressedDistance = distanceRegression(distance);
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
DesmosServer.putDouble("distanceReg", regressedDistance);
//Vision odometry circle fit based pose estimate
// Point targetOffset = m_visionOdometry.getTargetOffset();
// DesmosServer.putPoint("targetOff", targetOffset);
vel = m_boomBoom.getVelocity(regressedDistance + 30);
hood = m_boomBoom.getHood(regressedDistance + 30);
// m_boomBoom.runDrumShooter(vel);
m_boomBoom.runDrumShooterVelocityPID(vel);
m_hood.runAngleAdjustPID(hood);
double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
double currentHood = this.m_hood.getEncoderPosition();
this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance);
SmartDashboard.putNumber("Regressed Distance", regressedDistance);
SmartDashboard.putNumber("Distance", distance);
SmartDashboard.putNumber("Hood Target Angle Track", hood);
SmartDashboard.putNumber("Vel Target Track", vel);
// isExecuted = true;
}
catch (Exception e){
e.printStackTrace();
// System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java");
}
// m_turret.runshooterRotatePID(m_targetAngle);
}
public final double distanceRegression(double distance) {
return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
}
public void updateRegressionDesmos() {
m = DesmosServer.readDouble("m");
b = DesmosServer.readDouble("b");
DesmosServer.putArray("MB", m, b);
}
// 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
-121
View File
@@ -1,121 +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.io.IOException;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.Point;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
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.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);
hood = m_boomBoom.getHood(distance);
m_boomBoom.runDrumShooterVelocityPID(vel);
m_hood.runAngleAdjustPID(hood);
//m_turret.runshooterRotatePID(m_targetAngle);
}
/* 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 hardware deadzone (due to mechanical limitations).
* @param angle Angle to check.
* @return True if in hardware deadzone.
*/
public static boolean isHardwareDeadzone(double angle) {
return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT));
}
/**
* Checks if in digital deadzone (due to climber).
* @param angle Angle to check.
* @return True if in digital deadzone.
*/
public static boolean isDigitalDeadzone(double angle) {
return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT));
}
// 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
+44 -13
View File
@@ -18,8 +18,10 @@ import java.util.stream.IntStream;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.CSV;
@@ -31,9 +33,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;
@@ -49,13 +53,11 @@ 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();
try {
// This is a helper class that allows us to read a CSV file into a Java array.
@@ -92,17 +94,25 @@ public class BoomBoom extends SubsystemBase {
}
}
/**
* 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) {
// 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).
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
/**
* 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) {
// 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).
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
@@ -163,19 +173,26 @@ public class BoomBoom extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
// speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
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);
SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
}
@@ -188,8 +205,9 @@ public class BoomBoom extends SubsystemBase {
}
public void runDrumShooterVelocityPID(double targetVel) {
SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset);
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
m_shooterFalconRight.follow(m_shooterFalconLeft);
// New BoomBoom controller stuff
// Controls a motor with the output of the BangBang controller
// Controls a motor with the output of the BangBang conroller and a feedforward
@@ -199,4 +217,17 @@ public class BoomBoom extends SubsystemBase {
// feedforward.calculate(targetVel));
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
}
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/Climber.java
+38 -267
View File
@@ -1,279 +1,50 @@
// // 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.
// 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.
// /*
// Safety
// Hooks
// Add
// */
package frc4388.robot.subsystems;
// package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.revrobotics.CANSparkMax;
// import com.ctre.phoenix.motorcontrol.NeutralMode;
// import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
// import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
// import org.opencv.core.Point;
public class Climber extends SubsystemBase {
// import edu.wpi.first.math.Matrix;
// import edu.wpi.first.math.geometry.Rotation2d;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj2.command.SubsystemBase;
// import frc4388.robot.Constants;
// import frc4388.robot.Constants.ClimberConstants;
WPI_TalonFX m_climberShoulder;
WPI_TalonFX m_climberElbow;
// public class Climber extends SubsystemBase {
// public WPI_TalonFX m_shoulder;
// public WPI_TalonFX m_elbow;
/** Creates a new Climber. */
public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) {
m_climberShoulder = climberShoulder;
m_climberElbow = climberElbow;
// private double m_shoulderOffset;
// private double m_elbowOffset;
m_climberShoulder.configFactoryDefault();
m_climberElbow.configFactoryDefault();
m_climberShoulder.setNeutralMode(NeutralMode.Brake);
m_climberElbow.setNeutralMode(NeutralMode.Brake);
}
// private WPI_PigeonIMU m_gyro;
// private boolean m_groundRelative;
// private double m_robotAngle;
// private double m_robotPosition;
// private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d);
// public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) {
// m_shoulder = shoulder;
// m_shoulder.configFactoryDefault();
// m_shoulder.setNeutralMode(NeutralMode.Brake);
public void runShoulderWithInput(double input) {
m_climberShoulder.set(input);
}
// m_elbow = elbow;
// m_elbow.configFactoryDefault();
// m_elbow.setNeutralMode(NeutralMode.Brake);
public void runElbowWithInput(double input){
m_climberElbow.set(input);
}
// setClimberGains();
public void runBothMotorsWithInputs(double shoulderInput, double elbowInput) {
m_climberShoulder.set(shoulderInput);
m_climberElbow.set(elbowInput);
}
// m_shoulderOffset = m_shoulder.getSelectedSensorPosition();
// m_elbowOffset = m_elbow.getSelectedSensorPosition();
@Override
public void periodic() {
// This method will be called once per scheduler run
}
// m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
// m_elbow.configForwardSoftLimitEnable(false);
// m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
// m_elbow.configReverseSoftLimitEnable(false);
// m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
// m_shoulder.configForwardSoftLimitEnable(false);
// m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
// m_shoulder.configReverseSoftLimitEnable(false);
// if(groundRelative)
// m_gyro = gyro;
// m_groundRelative = groundRelative;
// }
// /* getJointAngles
// * Gets joint angles for climber arm
// * targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the
// * rotation of the robot. Both parameters should be in cm.
// * returns [shoulderAngle, elbowAngle] in radians
// * Does not set the motors automatically
// *
// * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */
// public static double[] getJointAngles(Point targetPoint, double tiltAngle) {
// double [] angles = new double[2];
// double mag = Math.hypot(targetPoint.x, targetPoint.y);
// if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
// mag = ClimberConstants.MAX_ARM_LENGTH;
// } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
// targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
// targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
// mag = ClimberConstants.MIN_ARM_LENGTH;
// } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
// targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
// targetPoint.y = 0.d;
// mag = ClimberConstants.MIN_ARM_LENGTH;
// }
// // The angle to the target point
// double theta;
// if(targetPoint.x == 0.d) {
// theta = Math.PI/2.d; // TODO rename variable
// } else {
// theta = Math.atan(targetPoint.y / targetPoint.x);
// }
// theta += tiltAngle;
// if(targetPoint.x < 0.d)
// theta += Math.PI;
// // Correct target position for tilt
// targetPoint.x = Math.cos(theta) * mag;
// targetPoint.y = Math.sin(theta) * mag;
// // Law and Order: Cosines edition
// double shoulderAngle;
// if(mag == 0) {
// shoulderAngle = 0;
// } else {
// shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
// (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
// }
// shoulderAngle = theta - shoulderAngle;
// double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
// (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
// //elbowAngle = Math.PI - elbowAngle;
// // System.out.println("sa1: " + shoulderAngle);
// // System.out.println("ea1: " + elbowAngle);
// // If the climber is resting on the robot base, rotate the upper arm in the direction of the target
// if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
// shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
// double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
// // System.out.println("xDiff: " + xDiff);
// elbowAngle = Math.atan(-targetPoint.y / xDiff);
// // System.out.println("ea2: " + elbowAngle);
// if(elbowAngle < 0) {
// elbowAngle = Math.PI - Math.abs(elbowAngle);
// }
// if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
// elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
// // System.out.println("ea3: " + elbowAngle);
// // Deal with negative wraparound
// // if(xDiff >= 0.d)
// // elbowAngle += Math.PI;
// // System.out.println("ea4: " + elbowAngle);
// }
// angles[0] = shoulderAngle;
// angles[1] = elbowAngle;
// return angles;
// }
// public void setMotors(double shoulderOutput, double elbowOutput) {
// m_shoulder.set(shoulderOutput);
// m_elbow.set(elbowOutput);
// }
// /* Rotation Matrix
// R = [cos(0) -sin(0) \n sin(0) cos(0)]
// Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)]
// Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix
// Rotation Matrix video: https://youtu.be/Ta8cKqltPfU
// */
// public double getRobotTilt() {
// double[] ypr = new double[3];
// m_gyro.getYawPitchRoll(ypr);
// double theta = 0;
// // double sin;
// // double cos;
// // xsin = 0; //placeholder for sin, cos
// // ysin = 0;
// // xcos = 0;
// // ycos = 0;
// double[][] rotMax = {
// {Math.cos(theta) - Math.sin(theta), 0 },
// {Math.sin(theta) + Math.cos(theta), 0},
// {0, 0, 1}
// };
// if (m_robotPosition != m_robotAngle){
// setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition);
// }
// return Math.toRadians(ypr[1]); // Pitch
// // multiply by pie and then divide by 180
// }
// public void setClimberGains() {
// // shoulder PIDs
// m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
// m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
// // elbow PIDs
// m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
// m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
// }
// public void setJointAngles(double[] angles) {
// System.out.println(angles);
// setJointAngles(angles[0], angles[1]);
// }
// public void setJointAngles(double shoulderAngle, double elbowAngle) {
// shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
// elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
// shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
// elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
// // Convert radians to ticks
// System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
// double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
// double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
// shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO;
// elbowAngle *= ClimberConstants.GEAR_BOX_RATIO;
// shoulderPosition += m_shoulderOffset;
// elbowPosition += m_elbowOffset;
// m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
// m_elbow.set(TalonFXControlMode.Position, elbowPosition);
// }
// public void controlWithInput(double xInput, double yInput) {
// m_position.x += xInput * ClimberConstants.MOVE_SPEED;
// m_position.y += yInput * ClimberConstants.MOVE_SPEED;
// System.out.println("x: " + m_position.x + " y: " + m_position.y);
// double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d;
// // double[] testAngles = getJointAngles(0, 0, 0);
// // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]);
// // double[] testAngles2 = getJointAngles(5000, 5000, 0);
// // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]);
// // double[] testAngles3 = getJointAngles(0, 75, 0);
// // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]);
// // double[] testAngles4 = getJointAngles(75, 0, 0);
// // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]);
// // double[] testAngles5 = getJointAngles(-75, 0, 0);
// // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]);
// // double[] testAngles6 = getJointAngles(60, 25, 0);
// // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]);
// double[] jointAngles = getJointAngles(m_position, tiltAngle);
// setJointAngles(jointAngles);
// }
// public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) {
// m_robotPosition = robotPosition;
// m_robotAngle = robotAngle;
// m_robotAngle = 45; //45 is placeholder
// }
// @Override
// public void periodic(){
// SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
// }
// }
public double getCurrent() {
return m_climberElbow.getSupplyCurrent();
}
}
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
+54 -19
View File
@@ -4,20 +4,17 @@
package frc4388.robot.subsystems;
import javax.sql.rowset.WebRowSet;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.SparkMaxLimitSwitch;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.SparkMaxRelativeEncoder.Type;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.utility.Gains;
@@ -25,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;
@@ -35,8 +32,9 @@ public class Hood extends SubsystemBase {
public boolean m_isHoodReady = false;
public double m_fireAngle;
public double m_fireAngle;
public double speedLimiter;
/** Creates a new Hood. */
public Hood(CANSparkMax angleAdjusterMotor) {
@@ -46,20 +44,43 @@ 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, ShooterConstants.HOOD_FORWARD_LIMIT);
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, 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).
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 * 0.05);
}
if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
}
if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent();
}
/**
@@ -84,20 +105,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);
}
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 -50
View File
@@ -4,77 +4,48 @@
package frc4388.robot.subsystems;
//Imported Limit switch ONLY
import com.revrobotics.SparkMaxLimitSwitch;
import com.revrobotics.SparkMaxLimitSwitch.Type;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
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);
}
/**
* Runs The Extender
* @param extended Wether the Extender Is Extended
*/
public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?)
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 -3
View File
@@ -1,8 +1,6 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Constants.SerializerConstants;
import edu.wpi.first.wpilibj.DigitalInput;
import com.revrobotics.CANSparkMax;
@@ -21,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);
}
@@ -50,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
+38 -21
View File
@@ -6,59 +6,76 @@ package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.wpilibj.DigitalInput;
import frc4388.robot.Constants.StorageConstants;
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_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
/**
* Every Robot Tick Manage The Storage
*/
public void periodic() {
manageStorage();
//manageStorage();
}
public double getCurrent(){
return m_storageMotor.getOutputCurrent();
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+50 -70
View File
@@ -4,8 +4,7 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
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.estimator.SwerveDrivePoseEstimator;
@@ -17,14 +16,12 @@ 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.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
import frc4388.utility.VisionObscuredException;
public class SwerveDrive extends SubsystemBase {
@@ -38,39 +35,29 @@ public class SwerveDrive extends SubsystemBase {
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
Units.inchesToMeters(-halfWidth));
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
m_backLeftLocation, m_backRightLocation);
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 double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
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,15 +65,15 @@ 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(),
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());
@@ -109,37 +96,39 @@ public class SwerveDrive extends SubsystemBase {
ignoreAngles = true;
else
ignoreAngles = false;
Translation2d speed = new Translation2d(-speedX, speedY);
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();
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED));
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(-leftX, leftY);
Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
rotTarget = new Rotation2d(rightX, rightY);
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = -speed.getX();
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
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);
setModuleStates(states);
// SmartDashboard.putNumber("rot", rot);
// SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
}
/**
@@ -150,10 +139,11 @@ 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];
module.setDesiredState(state, false);
module.setDesiredState(state, ignoreAngles);
}
// modules[0].setDesiredState(desiredStates[0], false);
}
@@ -164,7 +154,10 @@ 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());
super.periodic();
@@ -174,42 +167,21 @@ 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
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
// SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
// SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
// SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
* Gets the distance between two given poses.
*
* @param p1 The first pose.
* @param p2 The second pose.
* @return Absolute distance between p1 and p2.
* Gets the current chassis speeds in m/s and rad/s.
* @return Current chassis speeds (vx, vy, ω)
*/
public double distBtwPoses(Pose2d p1, Pose2d p2) {
return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2));
}
/**
* Returns a scalar from your distance to the hub to your target distance.
*
* @param target_dist The target distance.
* @return A scalar that multiplies your distance from the hub to get your
* target distance.
*/
public Pose2d poseGivenDist(double target_dist) {
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
double scalar = target_dist / distBtwPoses(p1, p2);
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
return new_pose;
public double[] getChassisSpeeds() {
return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond};
}
/**
@@ -281,4 +253,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
+7
View File
@@ -120,6 +120,7 @@ public class SwerveModule extends SubsystemBase {
double desiredTicks = currentTicks + deltaTicks;
if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}
@@ -191,5 +192,11 @@ public class SwerveModule extends SubsystemBase {
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
+143 -26
View File
@@ -8,8 +8,6 @@ import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import java.util.concurrent.TimeoutException;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxLimitSwitch;
@@ -20,7 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.commands.Shoot;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.utility.Gains;
public class Turret extends SubsystemBase {
@@ -32,48 +30,138 @@ 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;
double speedLimiter;
public Turret(CANSparkMax boomBoomRotateMotor) {
m_boomBoomRotateMotor = boomBoomRotateMotor;
m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake);
// 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);
SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT);
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT);
setTurretSoftLimits(true);
this.speedLimiter = 1.0;
setTurretPIDGains();
}
m_boomBoomRotateMotor.setInverted(false);
// 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).
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 * 0.05);
}
if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
}
if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {
this.speedLimiter = 1.0;
}
}
/**
@@ -85,17 +173,29 @@ 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);
}
public void runshooterRotatePID(double targetAngle) {
targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
public void runShooterRotatePID(double targetAngle) {
targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT;
m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
}
@@ -103,13 +203,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
+27 -18
View File
@@ -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;
}
src/main/java/frc4388/utility/RobotUnits.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.utility;
import edu.wpi.first.math.util.Units;
/** Add your docs here. */
public class RobotUnits {
// constants
// conversions
public static double falconTicksToRotations(final double ticks) {
double rotations = ticks / 2048;
return rotations;
}
public static double falconRotationsToTicks(final double rotations) {
double ticks = rotations * 2048;
return ticks;
}
}
src/main/java/frc4388/utility/Vector2D.java
+9 -10
View File
@@ -63,8 +63,8 @@ public class Vector2D extends Vector2d {
* @param v Vector to add
*/
public void add(Vector2D v) {
x += v.x;
y += v.x;
this.x += v.x;
this.y += v.x;
}
/**
@@ -82,8 +82,8 @@ public class Vector2D extends Vector2d {
* @param v Vector to subtract
*/
public void subtract(Vector2D v) {
x -= v.x;
y -= v.x;
this.x -= v.x;
this.y -= v.x;
}
/**
@@ -101,8 +101,8 @@ public class Vector2D extends Vector2d {
* @param scalar Scalar to multiply
*/
public void multiply(double scalar) {
x *= scalar;
y *= scalar;
this.x *= scalar;
this.y *= scalar;
}
/**
@@ -120,8 +120,8 @@ public class Vector2D extends Vector2d {
* @param scalar Scalar to divide
*/
public void divide(double scalar) {
x /= scalar;
y /= scalar;
this.x /= scalar;
this.y /= scalar;
}
/**
@@ -153,7 +153,6 @@ public class Vector2D extends Vector2d {
@Override
public String toString() {
return ("(" + this.x + ", " + this.y + ")");
return "<" + this.x + ", " + this.y + ">";
}
}
src/main/java/frc4388/utility/VelocityCorrection.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.utility;
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.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.SwerveDrive;
/** Add your docs here. */
public class VelocityCorrection {
SwerveDrive swerve;
BoomBoom boomBoom;
// vectors (in ft and ft/sec)
public Vector2D position;
public Vector2D cartesianVelocity;
// find
public Vector2D target;
public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) {
this.swerve = swerve;
this.boomBoom = boomBoom;
position = new Vector2D(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY());
cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond);
target = getTargetPoint();
}
private Vector2D getTargetPoint() {
double approxShotTime = 1; // TODO: get shot time from shooter tables
Vector2D targetPoint = Vector2D.multiply(this.cartesianVelocity, -1 * approxShotTime);
return Vector2D.round(targetPoint, 5);
}
}
src/main/java/frc4388/utility/controller/ButtonBox.java
+23
View File
@@ -0,0 +1,23 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj.GenericHID;
public class ButtonBox extends GenericHID {
public ButtonBox(int port) {
super(port);
}
public enum Button {
kRightSwitch(1),
kMiddleSwitch(2),
kLeftSwitch(3),
kRightButton(4),
kLeftButton(5);
@SuppressWarnings("MemberName")
public final int value;
Button(int value) {
this.value = value;
}
}
}
src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java
+2 -1
View File
@@ -3,6 +3,7 @@ package frc4388.utility.controller;
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.Constants.OIConstants;
public class DeadbandedRawXboxController extends RawXboxController {
public DeadbandedRawXboxController(int port) { super(port); }
@@ -13,7 +14,7 @@ public class DeadbandedRawXboxController extends RawXboxController {
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+2 -1
View File
@@ -4,6 +4,7 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import frc4388.robot.Constants.OIConstants;
public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); }
@@ -14,7 +15,7 @@ public class DeadbandedXboxController extends XboxController {
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
src/main/java/frc4388/utility/desmos/DesmosClient.html
+274
View File
@@ -0,0 +1,274 @@
<!DOCTYPE html>
<html>
<head>
<script src="https://www.desmos.com/api/v1.6/calculator.js?apiKey=dcb31709b452b1cf9dc26972add0fda6"></script>
<style>
#status {
position: absolute;
right: 0;
bottom: 0;
width: 60pt;
height: 20pt;
background-color: rgba(0, 0, 0, .8);
font-family: Arial, Helvetica, sans-serif;
text-align: center;
line-height: 20pt;
}
</style>
<title>Desmos Client</title>
</head>
<body>
<div id="calculator" style="width: 100wh; height: 100vh; position: absolute; top: 0; left: 0; right: 0; bottom: 0; overflow: hidden;"></div>
<div id="status" style="color: #00FF00;">Active</div>
<script>
const EXPRESSION_TYPES = {
'expression': ['number', 'point', 'array'],
'note': ['note'],
'table': ['table'],
'clear': ['all', 'except', 'single']
};
let status = document.getElementById('status');
let elt = document.getElementById('calculator');
let calculator = Desmos.GraphingCalculator(elt);
const clearState = calculator.getState();
let helperExpressions = {};
let variables = [];
let url = prompt("Robot URL", "localhost");
let port = 8000;
function requestVariables() {
let http = new XMLHttpRequest();
http.addEventListener("load", setVariables);
http.addEventListener("error", handleError);
http.open("POST", `http://${url}:${port}`);
let json = serverStringify(getVariables());
http.send(json);
}
function handleError() {
status.style = 'color: #FF0000;';
status.innerHTML = 'Offline';
console.log('Request failed');
setTimeout(requestVariables, 0);
}
function setVariables() {
status.style = 'color: #00FF00;';
status.innerHTML = 'Active';
variables = JSON.parse(this.responseText);
for(let variable of variables) {
if(EXPRESSION_TYPES['expression'].includes(variable['type'])) {
variable['lname'] = latexName(variable['name']);
calculator.setExpression({ id: variable['name'], latex: variable['lname'] + '=' + variable['value']});
} else if(EXPRESSION_TYPES['table'].includes(variable['type'])) {
let cols = getColumns(variable['value'].split(' '));
calculator.setExpression({ type: 'table', id: variable['name'], columns: cols});
} else if(EXPRESSION_TYPES['clear'].includes(variable['type'])) {
clear(variable['value'], variable['type']);
} else
console.log('Invalid input type : ' + variable['type']);
}
setTimeout(requestVariables, 0);
}
function getColumns(unparsedCols) {
let cols = [];
for(let col of unparsedCols) {
col = col.split(',');
let latexStr = latexName(col[0]);
let valuesArr = col.slice(1, col.length);
cols.push({ latex: latexStr, values: valuesArr });
}
return cols;
}
function clear(lname, type) {
let state = calculator.getState();
let expressions = state.expressions.list;
if(expressions.length === 0)
return;
let lnames = lname.split(',');
switch(type) {
case 'all':
state = clearState;
break;
case 'except':
for(let i = 0; i < expressions.length; i++) {
if(!expressions[i].latex.includes('='))
return;
let expressionName = regularName(expressions[i].latex.split('=')[0]);
if(!lnames.includes(expressionName)) {
expressions.splice(i, 1);
i--;
}
}
break;
case 'single':
for(let i = 0; i < expressions.length; i++) {
if(!expressions[i].latex.includes('='))
return;
if(lnames.includes(regularName(expressions[i].split('=')[0]))) {
expressions.splice(i, 1);
i--;
}
}
break;
}
calculator.setState(state);
}
function getVariables() {
let vars = [];
let expressions = calculator.getExpressions();
for(expression of expressions) {
if(expression.type === 'expression')
vars = readVariable(expression.latex);
else if(expression.type === 'table')
vars = readTable(expression);
else
console.log('Invalid output type : ' + expression.type);
}
return vars;
}
function findExpressionType(lvalue) {
let raw = lvalue.replace('\\left', '')
.replace('\\right', '');
if(raw.match(/\([a-zA-Z0-9._{}\(\)]*,[a-zA-Z0-9._{}\(\)]*\)/))
return 'point';
else if(raw.match(/\[[a-zA-Z0-9.,_{}\(\)]*\]/))
return 'array';
else
return 'number';
}
function readVariable(latex) {
if(!latex.includes('='))
return [];
let vars = [];
let lname = latex.split('=')[0];
let lvalue = latex.split('=')[1];
if(lname && lname != '' && lvalue && lvalue != '') {
let name = regularName(lname);
let value = regularValue(lvalue);
let type = findExpressionType(lvalue);
let numericValue, numericValueX, numericValueY, listValue;
switch(type) {
case 'number':
numericValue = getNumericValue(name, lname);
break;
case 'point':
numericValueX = getNumericValue(name + 'x', lname + '.x');
numericValueY = getNumericValue(name + 'y', lname + '.y');
break;
case 'array':
listValue = getListValue(name, lname);
break;
}
if(numericValue)
vars.push({"name": name, "type": "number", "value": numericValue});
if(numericValueX && numericValueY)
vars.push({"name": name, "type": "point", "value": numericValueX + ',' + numericValueY});
if(listValue)
vars.push({"name": name, "type": "array", "value": listValue.toString()});
}
return vars;
}
function readTable(expression) {
let vars = [];
for(let column of expression.columns) {
let name = regularName(column.latex);
let listValue = getListValue(name, column.latex);
if(listValue)
vars.push({"name": name, "type": "array", "value": listValue.toString()});
}
return vars;
}
function getNumericValue(key, lname) {
if(!(key in helperExpressions))
helperExpressions[key] = calculator.HelperExpression({ latex: lname });
else
return helperExpressions[key].numericValue;
}
function getListValue(key, lname) {
if(!(key in helperExpressions))
helperExpressions[key] = calculator.HelperExpression({ latex: lname });
else
return helperExpressions[key].listValue;
}
function latexName(name) {
return name.substring(0, 1) + '_{' + name.substring(1, name.length) + '}';
}
function regularName(lname) {
if(lname.length == 1)
return lname;
return lname.substring(0, 1) + lname.substring(3, lname.length - 1);
}
function regularValue(lvalue) {
let value = lvalue.replace('\\left(', '')
.replace('\\right)', '')
.replace('\\left[', '')
.replace('\\right]', '')
.replace('(', '')
.replace(')', '')
.replace('[', '')
.replace(']', '');
return value;
}
function serverStringify(vars) {
let stringified = '';
for(let variable of vars) {
stringified += variable['name'] + '\t'
+ variable['type'] + '\t'
+ variable['value'] + '\n';
}
return stringified;
}
setTimeout(requestVariables, 0);
</script>
</body>
</html>
src/main/java/frc4388/utility/desmos/DesmosServer.java
+352
View File
@@ -0,0 +1,352 @@
package frc4388.utility.desmos;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Set;
import org.opencv.core.Point;
/**
* A http server that allows the robot to communicate with Desmos Graphing Calculator
*
* @author Daniel McGrath
* */
public class DesmosServer extends Thread {
private static HashMap<String, String[]> desmosQueue = new HashMap<>();
private static HashMap<String, String[]> readVariables = new HashMap<>();
private static int clearCount = 0;
private static boolean running = false;
private ServerSocket serverSocket;
private int activePort;
/**
* Creates DesmosServer running on port
* <p>
* Use this for cases when the robot is using the default port
*
* @param port The port the server will run on
* */
public DesmosServer(int port) {
activePort = port;
}
/**
* Creates DesmosServer running on port 5500
* */
public DesmosServer() {
activePort = 8000;
}
@Override
public void run() {
try {
runServer(activePort);
} catch(IOException err) {
err.printStackTrace();
}
}
/**
* Runs server on port
*
* @param port The port the server runs on
* @throws IOException
* */
public void runServer(int port) throws IOException {
System.out.println("Initializing DesmosServer on port " + port + "...");
serverSocket = new ServerSocket(port);
running = true;
System.out.println("DesmosServer is active!");
while(true) {
Socket client = serverSocket.accept();
handleClient(client);
}
}
/**
* Handles client requests
*
* @param client The client connection
* @throws IOException
* */
public void handleClient(Socket client) throws IOException {
InputStreamReader clientStream = new InputStreamReader(client.getInputStream());
BufferedReader bufferedReader = new BufferedReader(clientStream);
ArrayList<String> headers = new ArrayList<>();
String header;
while((header = bufferedReader.readLine()).length() != 0) {
headers.add(header);
}
String body = "";
while(bufferedReader.ready()) {
body += (char) bufferedReader.read();
}
readVariables(body);
sendResponse(client);
}
/**
* Sends JSON response with format
* <p>
* [
* {"int": "24"},
* {"double": "2.4"},
* {"point": "(2,4)"},
* {"list": "[2,4]"}
* ]
*
* @param client The client connection
* @throws IOException
* */
public void sendResponse(Socket client) throws IOException {
OutputStream clientOutput = client.getOutputStream();
// Write Headers
clientOutput.write("HTTP/1.1 200 OK\r\n".getBytes());
clientOutput.write("Access-Control-Allow-Origin: *\r\n".getBytes());
clientOutput.write("Keep-Alive: timeout=2, max=100\r\n".getBytes());
clientOutput.write("Connection: Keep-Alive\r\n".getBytes());
clientOutput.write("Content-Type: application/json\r\n\r\n".getBytes());
clientOutput.write(getJSONOutput().getBytes());
clientOutput.flush();
clientOutput.close();
clearCount = 0;
}
/**
* Produces JSON output containing Desmos output.
*
* @return JSON string to be read by Desmos client
* */
public static String getJSONOutput() {
String json = "[";
if(!desmosQueue.isEmpty()) {
Set<String> keySet = new HashSet<>(desmosQueue.keySet());
for(String key : keySet) {
json += "\n\t{"
+ "\"name\":\"" + key + "\","
+ "\"type\":\"" + desmosQueue.get(key)[0] + "\","
+ "\"value\":\"" + desmosQueue.get(key)[1] + "\""
+ "},";
desmosQueue.remove(key);
}
json = json.substring(0, json.length()-1); // remove comma at the end
}
json += "\n]";
return json;
}
/**
* Interpret client request and update variables
*
* @param requestBody Client request
*/
public static void readVariables(String requestBody) {
for(String variable : requestBody.split("\n")) {
if(variable.equals(""))
break;
String[] readVar = variable.split("\t");
readVariables.put(readVar[0], new String[] {readVar[1], readVar[2]});
}
}
/**
* Checks if the server is running
*
* @return The server status
*/
public static boolean isRunning() {
return running;
}
// ---------------------------------------------------------------------
/**
* Adds integer to desmos queue
*
* @param name Name of desmos variable
* @param value Integer value
* */
public static void putInteger(String name, Integer value) {
desmosQueue.put(name, new String[] {"number", value.toString()});
}
/**
* Adds double to desmos queue
*
* @param name Name of desmos variable
* @param value Double value
* */
public static void putDouble(String name, Double value) {
desmosQueue.put(name, new String[] {"number", value.toString()});
}
/**
* Adds point to desmos queue
*
* @param name Name of desmos variable
* @param value Point value
* */
public static void putPoint(String name, Point point) {
desmosQueue.put(name, new String[] {"point", "(" + point.x + "," + point.y + ")"});
}
public static void putArray(String name, double... arr) {
desmosQueue.put(name, new String[] {"array", Arrays.toString(arr).replace(" ", "")});
}
/**
* Adds table to desmos queue
*
* @param name ID of table
* @param id Column ID
* @param column Double array containing column values
* @param table Repeat id and column in sequence
* */
public static void putTable(String name, Object... table) {
String tableStr = "";
for(int i = 0; i < table.length; i += 2) {
// Check parameters
if(!(table[i] instanceof String)) { return; }
if(!(table[i+1] instanceof double[])) { return; }
tableStr += table[i] + ",";
String values = Arrays.toString((double[]) table[i+1]).replace(" ", "");
tableStr += values.substring(1, values.length() - 1);
tableStr += ' ';
}
tableStr = tableStr.substring(0, tableStr.length()-1); // remove space at the end
desmosQueue.put(name, new String[] {"table", tableStr});
}
/**
* Clears entire expression panel
*/
public static void clearAll() {
desmosQueue.put("clear" + clearCount, new String[] {"all", "n/a"});
clearCount++;
}
/**
* Clears entire expression panel except for expressions in {@code names}
*
* @param names Names which should be preserved in clear
*/
public static void clearExcept(String... names) {
String namesStr = Arrays.toString(names).replace(" ", "");
namesStr = namesStr.substring(1, namesStr.length()-1);
desmosQueue.put("clear" + clearCount, new String[] {"except", namesStr});
clearCount++;
}
/**
* Removes expressions in {@code names}
*
* @param names Names which should be removed
*/
public static void clearSpecific(String... names) {
String namesStr = Arrays.toString(names).replace(" ", "");
namesStr = namesStr.substring(1, namesStr.length()-1);
desmosQueue.put("clear" + clearCount, new String[] {"specific", namesStr});
clearCount++;
}
// ---------------------------------------------------------------------
/**
* Reads desmos integer
*
* @param name Desmos variable name
* @return Numeric value, if variable is an expression it will be evaluated
* <p>if variable is a double it will be cast to int
* */
public static int readInteger(String name) {
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number"))
return 0;
return (int) Double.parseDouble(readVariables.get(name)[1]);
}
/**
* Reads desmos double
*
* @param name Desmos variable name
* @return Numeric value, if variable is an expression it will be evaluated
* */
public static double readDouble(String name) {
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number"))
return 0;
return Double.parseDouble(readVariables.get(name)[1]);
}
/**
* Reads desmos point
*
* @param name Desmos variable name
* @return Point, if variable contains expressions they will be evaluated
* */
public static Point readPoint(String name) {
Point point = new Point();
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("point"))
return point;
String pointStr = readVariables.get(name)[1];
point.x = Double.parseDouble(pointStr.split(",")[0]);
point.y = Double.parseDouble(pointStr.split(",")[1]);
return point;
}
/**
* Reads desmos array, including table columns
*
* @param name Desmos variable name
* @returns Array of numeric values, if array contains expressions they will be evaluated
* */
public static double[] readArray(String name) {
if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("array"))
return new double[0];
String[] unparsed = readVariables.get(name)[1].split(",");
double[] arr = new double[unparsed.length];
for(int i = 0; i < arr.length; i++)
arr[i] = Integer.parseInt(unparsed[i]);
return arr;
}
}
src/test/java/frc4388/commands/AimToCenterTest.java
+15 -14
View File
@@ -2,52 +2,53 @@ package frc4388.commands;
import org.junit.Test;
import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.ShooterCommands.AimToCenter;
import org.junit.Assert;
public class AimToCenterTest {
private static final double DELTA = 1e-15;
@Test
//@Test
public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
boolean output;
//20 deg
output = AimToCenter.isHardwareDeadzone(20.);
output = AimToCenter.isDeadzone(20.);
Assert.assertFalse(output);
//-10 deg
output = AimToCenter.isHardwareDeadzone(-10.);
output = AimToCenter.isDeadzone(-10.);
Assert.assertTrue(output);
//-1 deg
output = AimToCenter.isHardwareDeadzone(-1.);
output = AimToCenter.isDeadzone(-1.);
Assert.assertTrue(output);
//341 deg
output = AimToCenter.isHardwareDeadzone(341.);
output = AimToCenter.isDeadzone(341.);
Assert.assertTrue(output);
//340 deg
output = AimToCenter.isHardwareDeadzone(340.);
Assert.assertFalse(output);
output = AimToCenter.isDeadzone(340.);
Assert.assertTrue(output);
//0 deg
output = AimToCenter.isHardwareDeadzone(0.);
output = AimToCenter.isDeadzone(0.);
Assert.assertFalse(output);
//200 deg
output = AimToCenter.isHardwareDeadzone(200.);
Assert.assertFalse(output);
output = AimToCenter.isDeadzone(200.);
Assert.assertTrue(output);
//2000000 deg
output = AimToCenter.isHardwareDeadzone(2000000.);
output = AimToCenter.isDeadzone(2000000.);
Assert.assertTrue(output);
//NaN deg
output = AimToCenter.isHardwareDeadzone(Double.NaN);
Assert.assertFalse(output);
// output = AimToCenter.isDeadzone(Double.NaN);
// Assert.assertFalse(output);
}
@Test