mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Aarav Shah
@@ -91,6 +91,9 @@
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
// "guid": "78696e70757401000000000000000000"
|
||||
// },
|
||||
// {
|
||||
"guid": "78696e70757401000000000000000000",
|
||||
"useGamepad": true
|
||||
}
|
||||
|
||||
+10
@@ -1,16 +1,19 @@
|
||||
{
|
||||
"HALProvider": {
|
||||
"Other Devices": {
|
||||
"SPARK MAX [10]": {
|
||||
"Talon FX[2]": {
|
||||
"header": {
|
||||
"open": true
|
||||
}
|
||||
},
|
||||
"SPARK MAX [30]": {
|
||||
"Talon FX[3]": {
|
||||
"header": {
|
||||
"open": true
|
||||
}
|
||||
},
|
||||
"SPARK MAX [31]": {
|
||||
"Talon FX[4]": {
|
||||
"header": {
|
||||
"open": true
|
||||
@@ -21,10 +24,14 @@
|
||||
"NTProvider": {
|
||||
"types": {
|
||||
"/FMSInfo": "FMSInfo",
|
||||
"/LiveWindow/BoomBoom": "Subsystem",
|
||||
"/LiveWindow/Hood": "Subsystem",
|
||||
"/LiveWindow/LED": "Subsystem",
|
||||
"/LiveWindow/SwerveDrive": "Subsystem",
|
||||
"/LiveWindow/SwerveModule": "Subsystem",
|
||||
"/LiveWindow/Turret": "Subsystem",
|
||||
"/LiveWindow/Ungrouped/Scheduler": "Scheduler",
|
||||
"/LiveWindow/Vision": "Subsystem"
|
||||
"/SmartDashboard/Driver Controller": "Mechanism2d",
|
||||
"/SmartDashboard/Field": "Field2d"
|
||||
},
|
||||
@@ -38,6 +45,9 @@
|
||||
},
|
||||
"NetworkTables": {
|
||||
"SmartDashboard": {
|
||||
"Recording": {
|
||||
"open": true
|
||||
},
|
||||
"open": true
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
|
||||
0 ,16 ,12000
|
||||
999 ,28.8 ,12000
|
||||
|
@@ -4,12 +4,15 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
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 frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
@@ -52,9 +55,8 @@ public final class Constants {
|
||||
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
public static final int GYRO_ID = 14;
|
||||
|
||||
// ofsets are in degrees
|
||||
|
||||
//NATHAN if you truncate or round or simplify these i will cry
|
||||
// offsets are in degrees
|
||||
// NATHAN if you truncate or round or simplify these i will cry
|
||||
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
|
||||
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
|
||||
public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
|
||||
@@ -120,4 +122,62 @@ public final class Constants {
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
public static final double RIGHT_AXIS_DEADBAND = 0.6;
|
||||
}
|
||||
|
||||
public static final class ShooterConstants {
|
||||
/* PID Constants Shooter */
|
||||
public static final int CLOSED_LOOP_TIME_MS = 1;
|
||||
|
||||
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
|
||||
public static final double TURRET_SPEED_MULTIPLIER = 0.1d;
|
||||
public static final int DEGREES_PER_ROT = 0;
|
||||
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;
|
||||
|
||||
|
||||
|
||||
/* 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
|
||||
|
||||
// deadzones
|
||||
public static final double HARD_DEADZONE_LEFT = 0.0;
|
||||
public static final double HARD_DEADZONE_RIGHT = 340.0;
|
||||
|
||||
public static final double DIG_DEADZONE_LEFT = 40.0;
|
||||
public static final double DIG_DEADZONE_RIGHT = 60.0;
|
||||
|
||||
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
|
||||
|
||||
/* Hood Constants */
|
||||
public static final int SHOOTER_ANGLE_ADJUST_ID = 32;
|
||||
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 class VisionConstants {
|
||||
public static final double TURN_P_VALUE = 0.8;
|
||||
public static final double X_ANGLE_ERROR = 0.5;
|
||||
public static final double GRAV = 385.83;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,6 +4,13 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Objects;
|
||||
|
||||
@@ -27,8 +34,13 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.AimToCenter;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
@@ -50,7 +62,22 @@ public class RobotContainer {
|
||||
private final TalonFX m_testMotor = new TalonFX(23);
|
||||
|
||||
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();
|
||||
private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||
private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor,
|
||||
m_robotMap.leftFrontWheelMotor,
|
||||
m_robotMap.rightFrontSteerMotor,
|
||||
m_robotMap.rightFrontWheelMotor,
|
||||
m_robotMap.leftBackSteerMotor,
|
||||
m_robotMap.leftBackWheelMotor,
|
||||
m_robotMap.rightBackSteerMotor,
|
||||
m_robotMap.rightBackWheelMotor,
|
||||
m_robotMap.leftFrontEncoder,
|
||||
m_robotMap.rightFrontEncoder,
|
||||
m_robotMap.leftBackEncoder,
|
||||
m_robotMap.rightBackEncoder);
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
@@ -61,7 +88,14 @@ public class RobotContainer {
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
/* Default Commands */
|
||||
// drives the swerve drive with a two-axis input from the driver controller
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
|
||||
//Turret default command
|
||||
|
||||
m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
getDriverController().getLeftX(),
|
||||
@@ -73,8 +107,6 @@ public class RobotContainer {
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
|
||||
m_testMotor.set(TalonFXControlMode.PercentOutput, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -105,10 +137,23 @@ public class RobotContainer {
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
// activates "BoomBoom"
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
|
||||
.whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
|
||||
/* Driver Buttons */
|
||||
// activates intake
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON);
|
||||
// .whenPressed() -> m_robot
|
||||
/* operator button */
|
||||
// activates hood
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(() -> m_robotHood.runHood(0.5d))
|
||||
.whenReleased(() -> m_robotHood.runHood(0.d));
|
||||
// new JoystickButton(getOperatorJoystick());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -5,13 +5,20 @@
|
||||
package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
|
||||
@@ -24,16 +31,18 @@ public class RobotMap {
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
configureSwerveMotorControllers();
|
||||
configureShooterMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* Swerve Subsystem */
|
||||
|
||||
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
@@ -46,6 +55,7 @@ public class RobotMap {
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
@@ -107,10 +117,56 @@ public class RobotMap {
|
||||
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
//leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
// Shooter Config
|
||||
/* Boom Boom Subsystem */
|
||||
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
|
||||
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
|
||||
|
||||
public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
// Create motor CANSparkMax
|
||||
void configureShooterMotorControllers() {
|
||||
|
||||
// LEFT FALCON
|
||||
shooterFalconLeft.configFactoryDefault();
|
||||
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconLeft.setInverted(true);
|
||||
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
// RIGHT FALCON
|
||||
shooterFalconRight.setInverted(false);
|
||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconRight.configFactoryDefault();
|
||||
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
// m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
|
||||
shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, 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
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,78 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
|
||||
public class AimToCenter extends CommandBase {
|
||||
/** Creates a new AimWithOdometry. */
|
||||
Turret m_turret;
|
||||
SwerveDrive m_drive;
|
||||
|
||||
// use odometry to find x and y later
|
||||
double x;
|
||||
double y;
|
||||
double m_targetAngle;
|
||||
|
||||
// public static Gains m_aimGains;
|
||||
|
||||
public AimToCenter(Turret turret, SwerveDrive drive) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_turret = turret;
|
||||
m_drive = drive;
|
||||
addRequirements(m_turret, m_drive);
|
||||
}
|
||||
|
||||
// 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.gyro.getAngle());
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,152 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
import frc4388.robot.subsystems.Hood;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
|
||||
public class Shoot extends CommandBase {
|
||||
|
||||
// subsystems
|
||||
public SwerveDrive m_swerve;
|
||||
public BoomBoom m_boomBoom;
|
||||
public Turret m_turret;
|
||||
public Hood m_hood;
|
||||
|
||||
// given
|
||||
public double m_gyroAngle;
|
||||
public double m_odoX;
|
||||
public double m_odoY;
|
||||
public double m_distance;
|
||||
|
||||
// targets
|
||||
public double m_targetVel;
|
||||
public double m_targetHood;
|
||||
public double m_targetAngle;
|
||||
public double m_driveTargetAngle;
|
||||
|
||||
// pid
|
||||
public double error;
|
||||
public double prevError;
|
||||
public double kP, kI, kD;
|
||||
public double integral, derivative;
|
||||
public double time;
|
||||
public double output;
|
||||
public double tolerance = 5.0;
|
||||
|
||||
// // dummy motor
|
||||
// public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420);
|
||||
// public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration();
|
||||
|
||||
/** Creates a new Shoot. */
|
||||
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
m_swerve = sDrive;
|
||||
m_boomBoom = sShooter;
|
||||
m_turret = sTurret;
|
||||
m_hood = sHood;
|
||||
|
||||
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
|
||||
|
||||
kP = 0.1;
|
||||
kI = 0.0;
|
||||
kD = 0.0;
|
||||
|
||||
integral = 0;
|
||||
derivative = 0;
|
||||
time = 0.02;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates error for custom PID.
|
||||
*/
|
||||
public void updateError() {
|
||||
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
m_odoX = 0; //TODO: get this value using odometry
|
||||
m_odoY = 0; //TODO: get this value using odometry
|
||||
m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
|
||||
|
||||
// get targets (shooter tables)
|
||||
m_targetVel = m_boomBoom.getVelocity(m_distance);
|
||||
m_targetHood = m_boomBoom.getHood(m_distance);
|
||||
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
|
||||
m_driveTargetAngle = m_targetAngle + m_turret.getBoomBoomAngleDegrees();
|
||||
|
||||
// // normal (i think) PID stuff
|
||||
// dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice();
|
||||
// dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID();
|
||||
// dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
|
||||
|
||||
// dummyConfiguration.slot0.kP = 0.1;
|
||||
// dummyConfiguration.slot0.kI = 0;
|
||||
// dummyConfiguration.slot0.kD = 0;
|
||||
// dummyConfiguration.slot0.kF = 0;
|
||||
|
||||
// // weird PID stuff
|
||||
// dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice();
|
||||
// dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID;
|
||||
// dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
|
||||
// // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0;
|
||||
|
||||
// dummyConfiguration.slot1.kP = 0.1;
|
||||
// dummyConfiguration.slot1.kI = 0;
|
||||
// dummyConfiguration.slot1.kD = 0;
|
||||
// dummyConfiguration.slot1.kF = 0;
|
||||
|
||||
// dummy.configAllSettings(dummyConfiguration);
|
||||
|
||||
// initial error
|
||||
updateError();
|
||||
prevError = error;
|
||||
}
|
||||
|
||||
/**
|
||||
* Run custom PID.
|
||||
*/
|
||||
public void runPID() {
|
||||
prevError = error;
|
||||
updateError();
|
||||
integral = integral + error * time;
|
||||
derivative = (error - prevError) / time;
|
||||
output = kP * error + kI * integral + kD * derivative;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
// dummy.selectProfileSlot(0, 0);
|
||||
// dummy.selectProfileSlot(1, 1);
|
||||
// dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle);
|
||||
// m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true);
|
||||
// m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch
|
||||
|
||||
// custom pid
|
||||
runPID();
|
||||
m_swerve.driveWithInput(0, 0, output, true);
|
||||
|
||||
m_hood.runAngleAdjustPID(m_targetHood);
|
||||
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
|
||||
}
|
||||
|
||||
// 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() {
|
||||
updateError();
|
||||
return Math.abs(error) <= tolerance;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
// 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 java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.util.Comparator;
|
||||
import java.util.Map;
|
||||
import java.util.Optional;
|
||||
import java.util.function.Function;
|
||||
import java.util.regex.Pattern;
|
||||
import java.util.stream.IntStream;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.CSV;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class BoomBoom extends SubsystemBase {
|
||||
public WPI_TalonFX m_shooterFalconLeft;
|
||||
public WPI_TalonFX m_shooterFalconRight;
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static BoomBoom m_boomBoom;
|
||||
public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020
|
||||
// BangBangController m_controller = new BangBangController();
|
||||
|
||||
double velP;
|
||||
double input;
|
||||
|
||||
public boolean m_isDrumReady = false;
|
||||
public double m_fireVel;
|
||||
|
||||
public Hood m_hoodSubsystem;
|
||||
public Turret m_turretSubsystem;
|
||||
|
||||
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
|
||||
|
||||
public static class ShooterTableEntry {
|
||||
public Double distance, hoodExt, drumVelocity;
|
||||
}
|
||||
|
||||
private ShooterTableEntry[] m_shooterTable;
|
||||
|
||||
/*
|
||||
* Creates new BoomBoom subsystem, has drum shooter and angle adjuster
|
||||
*/
|
||||
/** Creates a new BoomBoom. */
|
||||
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
|
||||
m_shooterFalconLeft = shooterFalconLeft;
|
||||
m_shooterFalconRight = shooterFalconRight;
|
||||
|
||||
|
||||
try {
|
||||
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
|
||||
private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
|
||||
|
||||
@Override
|
||||
protected String headerSanitizer(final String header) {
|
||||
return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
|
||||
}
|
||||
|
||||
};
|
||||
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath());
|
||||
new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
|
||||
} catch (final IOException e) {
|
||||
e.printStackTrace();
|
||||
// throw new RuntimeException(e);
|
||||
}
|
||||
}
|
||||
|
||||
public Double getVelocity(final Double distance) {
|
||||
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
|
||||
}
|
||||
|
||||
public Double getHood(final Double distance) {
|
||||
return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
|
||||
}
|
||||
|
||||
private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
|
||||
final E closestRecord = closestEntry.getValue();
|
||||
final int closestRecordIndex = closestEntry.getKey();
|
||||
final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
|
||||
return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
|
||||
}
|
||||
|
||||
private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
|
||||
return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
|
||||
}
|
||||
|
||||
private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
|
||||
final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
|
||||
return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
public void setShooterGains() {
|
||||
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
|
||||
m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
public void runDrumShooterVelocityPID(double targetVel) {
|
||||
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
|
||||
//Shrinks the feedforward slightly to avoid over speeding the shooter
|
||||
|
||||
|
||||
// m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel));
|
||||
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
// 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.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 edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class Hood extends SubsystemBase {
|
||||
public BoomBoom m_shooterSubsystem;
|
||||
|
||||
public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
|
||||
public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
|
||||
public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
|
||||
public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
|
||||
public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
|
||||
|
||||
public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
|
||||
|
||||
|
||||
//public boolean m_isHoodReady = false;
|
||||
|
||||
public double m_fireAngle;
|
||||
|
||||
|
||||
/** Creates a new Hood. */
|
||||
public Hood() {
|
||||
m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
|
||||
|
||||
// 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);
|
||||
setHoodSoftLimits(true);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
|
||||
/**
|
||||
* Set status of hood motor soft limits.
|
||||
* @param set Boolean to set soft limits to.
|
||||
*/
|
||||
public void setHoodSoftLimits(boolean set) {
|
||||
m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
|
||||
m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
|
||||
}
|
||||
|
||||
public void runAngleAdjustPID(double targetAngle)
|
||||
{
|
||||
//Set PID Coefficients
|
||||
m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
|
||||
m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
|
||||
m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
|
||||
m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
|
||||
m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
|
||||
m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput);
|
||||
|
||||
m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
|
||||
public void runHood(double input) {
|
||||
// m_angleAdjusterMotor.set(input);
|
||||
}
|
||||
|
||||
public void resetGyroAngleAdj(){
|
||||
// m_angleEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getAnglePosition(){
|
||||
return 0.0;//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;
|
||||
}
|
||||
}
|
||||
@@ -19,9 +19,11 @@ 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.wpilibj2.command.SubsystemBase;
|
||||
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;
|
||||
@@ -61,31 +63,13 @@ public class SwerveDrive extends SubsystemBase {
|
||||
private final Field2d m_field = new Field2d();
|
||||
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
|
||||
// m_leftFrontSteerMotor = leftFrontSteerMotor;
|
||||
// m_leftFrontWheelMotor = leftFrontWheelMotor;
|
||||
// m_rightFrontSteerMotor = rightFrontSteerMotor;
|
||||
// m_rightFrontWheelMotor = rightFrontWheelMotor;
|
||||
// m_leftBackSteerMotor = leftBackSteerMotor;
|
||||
// m_leftBackWheelMotor = leftBackWheelMotor;
|
||||
// m_rightBackSteerMotor = rightBackSteerMotor;
|
||||
// m_rightBackWheelMotor = rightBackWheelMotor;
|
||||
// m_leftFrontEncoder = leftFrontEncoder;
|
||||
// m_rightFrontEncoder = rightFrontEncoder;
|
||||
// m_leftBackEncoder = leftBackEncoder;
|
||||
// m_rightBackEncoder = rightBackEncoder;
|
||||
|
||||
m_leftFront = leftFront;
|
||||
m_leftBack = leftBack;
|
||||
m_rightFront = rightFront;
|
||||
m_rightBack = rightBack;
|
||||
m_gyro = gyro;
|
||||
|
||||
// modules = new SwerveModule[] {
|
||||
// new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
|
||||
// new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
|
||||
// new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
|
||||
// new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
|
||||
// };
|
||||
|
||||
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
|
||||
|
||||
m_poseEstimator =
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
// 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.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;
|
||||
import com.revrobotics.SparkMaxPIDController;
|
||||
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
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.utility.Gains;
|
||||
|
||||
public class Turret extends SubsystemBase {
|
||||
|
||||
/** Creates a new Turret. */
|
||||
public BoomBoom m_boomBoomSubsystem;
|
||||
public SwerveDrive m_sDriveSubsystem;
|
||||
|
||||
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 boolean m_isAimReady = false;
|
||||
|
||||
SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController();
|
||||
public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder();
|
||||
|
||||
// Variables
|
||||
public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument
|
||||
|
||||
m_boomBoomRotateMotor = boomBoomRotateMotor;
|
||||
m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController();
|
||||
m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder();
|
||||
m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake);
|
||||
|
||||
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
|
||||
m_boomBoomRightLimit.enableLimitSwitch(true);
|
||||
m_boomBoomLeftLimit.enableLimitSwitch(true);
|
||||
SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
|
||||
SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
|
||||
|
||||
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT);
|
||||
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT);
|
||||
setTurretSoftLimits(true);
|
||||
|
||||
m_boomBoomRotateMotor.setInverted(false);
|
||||
|
||||
m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
|
||||
m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
|
||||
m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
|
||||
m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
|
||||
m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
|
||||
m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
}
|
||||
|
||||
/**
|
||||
* Set status of turret motor soft limits.
|
||||
* @param set Boolean to set soft limits to.
|
||||
*/
|
||||
public void setTurretSoftLimits(boolean set) {
|
||||
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
|
||||
m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
|
||||
}
|
||||
|
||||
public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) {
|
||||
m_boomBoomSubsystem = subsystem0;
|
||||
m_sDriveSubsystem = subsystem1;
|
||||
}
|
||||
|
||||
public void runTurretWithInput(double input) {
|
||||
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
|
||||
}
|
||||
|
||||
public void runshooterRotatePID(double targetAngle) {
|
||||
targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT;
|
||||
m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition);
|
||||
}
|
||||
|
||||
public void resetGyroShooterRotate() {
|
||||
m_boomBoomRotateEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getboomBoomRotatePosition() {
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
// 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 edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class Vision extends SubsystemBase {
|
||||
//setup
|
||||
Turret m_turret;
|
||||
BoomBoom m_boomBoom;
|
||||
Hood m_hood;
|
||||
|
||||
NetworkTableEntry xEntry;
|
||||
IHandController m_driverController;
|
||||
//Aiming
|
||||
double turnAmount = 0;
|
||||
double xAngle = 0;
|
||||
double yAngle = 0;
|
||||
double target = 0;
|
||||
public double distance;
|
||||
public double realDistance;
|
||||
public static double fireVel;
|
||||
public static double fireAngle;
|
||||
|
||||
public double m_hoodTrim;
|
||||
public double m_turretTrim;
|
||||
public double m_fireAngle;
|
||||
|
||||
|
||||
public Vision(Turret aimSubsystem, BoomBoom boomBoom) {
|
||||
m_turret = aimSubsystem;
|
||||
m_boomBoom = boomBoom;
|
||||
m_hood = m_boomBoom.m_hoodSubsystem;
|
||||
//addRequirements(m_turret);
|
||||
limeOff();
|
||||
changePipeline(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3);
|
||||
}
|
||||
|
||||
public void track(){
|
||||
target = getV();
|
||||
xAngle = getX();
|
||||
yAngle = getY();
|
||||
|
||||
//find distance
|
||||
distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
|
||||
realDistance = (1.09 * distance) - 12.8;
|
||||
|
||||
// if (target == 1.0) { //checks if target is in view
|
||||
// //aims left and right
|
||||
// turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
|
||||
// if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
|
||||
// turnAmount = 0;
|
||||
// }
|
||||
// else if (turnAmount > 0 && turnAmount < 0.1){
|
||||
// turnAmount = 0.1;
|
||||
// }
|
||||
// else if (turnAmount < 0 && turnAmount > -0.1){
|
||||
// turnAmount = -0.1;
|
||||
// }
|
||||
// }
|
||||
|
||||
SmartDashboard.putNumber("Distance to Target", realDistance);
|
||||
|
||||
|
||||
// //start CSV
|
||||
|
||||
// fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance);
|
||||
// fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance);
|
||||
// //fire angle unknown so far
|
||||
// //end of CSV
|
||||
|
||||
// m_boomBoom.m_fireVel = fireVel;
|
||||
// m_hood.m_fireAngle = fireAngle;
|
||||
// m_turret.m_targetDistance = distance;
|
||||
|
||||
// checkFinished();
|
||||
}
|
||||
|
||||
public void checkFinished(){
|
||||
if (xAngle < 0.5 && xAngle > -0.5 && target == 1){
|
||||
m_turret.m_isAimReady = true;
|
||||
}
|
||||
else{
|
||||
m_turret.m_isAimReady = false;
|
||||
}
|
||||
}
|
||||
|
||||
public void limeOff(){
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
|
||||
}
|
||||
|
||||
public void limeOn(){
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
|
||||
}
|
||||
|
||||
public void changePipeline(int pipelineId)
|
||||
{
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);
|
||||
}
|
||||
|
||||
public double getV()
|
||||
{
|
||||
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
|
||||
}
|
||||
|
||||
public double getX()
|
||||
{
|
||||
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
|
||||
}
|
||||
|
||||
public double getY()
|
||||
{
|
||||
return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
|
||||
}
|
||||
@Override
|
||||
public void periodic(){
|
||||
//called once per scheduler run
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,240 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
import java.awt.Color;
|
||||
import java.io.BufferedReader;
|
||||
import java.io.IOException;
|
||||
import java.lang.invoke.MethodHandleProxies;
|
||||
import java.lang.invoke.MethodHandles;
|
||||
import java.lang.invoke.MethodType;
|
||||
import java.lang.reflect.Array;
|
||||
import java.lang.reflect.Field;
|
||||
import java.lang.reflect.Modifier;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.text.MessageFormat;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
import java.util.Objects;
|
||||
import java.util.function.BiConsumer;
|
||||
import java.util.function.Function;
|
||||
import java.util.function.IntFunction;
|
||||
import java.util.function.Predicate;
|
||||
import java.util.function.Supplier;
|
||||
import java.util.regex.Pattern;
|
||||
import java.util.stream.Collectors;
|
||||
import java.util.stream.IntStream;
|
||||
import java.util.stream.Stream;
|
||||
|
||||
public class CSV<R> {
|
||||
private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]");
|
||||
|
||||
private final Supplier<R> generator;
|
||||
private final IntFunction<R[]> arrayGenerator;
|
||||
private final Map<String, BiConsumer<R, String>> setters;
|
||||
|
||||
/**
|
||||
* A binary string operator to be applied to the entire header of the CSV.
|
||||
*/
|
||||
protected String headerSanitizer(final String header) {
|
||||
return SANITIZER.matcher(header).replaceAll("");
|
||||
}
|
||||
|
||||
/**
|
||||
* A binary string operator to be applied to each name in the header of the CSV.
|
||||
*/
|
||||
protected String nameProcessor(final String name) {
|
||||
return Character.toLowerCase(name.charAt(0)) + name.substring(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new {@code CSV} instance and prepares for populating the fields of objects created by
|
||||
* the given generator. Private fields and fields of primitive types are not supported.
|
||||
* @param generator a parameterless supplier which produces a new object with any number of fields
|
||||
* corresponding to header names from a CSV file. The first character of the names
|
||||
* from the header in the CSV file will be made lowercase and invalid characters
|
||||
* will be removed to match Java naming conventions.
|
||||
* @see #read(Path)
|
||||
*/
|
||||
@SuppressWarnings("unchecked")
|
||||
public CSV(final Supplier<R> generator) {
|
||||
final Class<?> clazz = generator.get().getClass();
|
||||
final Map<Class<?>, Function<String, ?>> fieldParsers = new HashMap<>();
|
||||
this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size);
|
||||
this.generator = generator;
|
||||
this.setters = new HashMap<>();
|
||||
for (final Field field : clazz.getFields()) {
|
||||
final Function<String, ?> parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser);
|
||||
if (parser != null)
|
||||
this.setters.put(field.getName(), (final R obj, final String rawValue) -> {
|
||||
try {
|
||||
field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue));
|
||||
} catch (final IllegalAccessException e) {
|
||||
throw new RuntimeException(e);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Reads and parses the contents of the given CSV file, and returns an array filled with populated
|
||||
* objects created with the previously given generator. Cells are parsed using their corresponding
|
||||
* field's {@code valueOf(String)} function.
|
||||
* @param path the path to a CSV file
|
||||
* @return the parsed data from the CSV file
|
||||
* @throws IOException if an I/O error occurs opening the file
|
||||
*/
|
||||
@SuppressWarnings("unchecked")
|
||||
public R[] read(final Path path) throws IOException {
|
||||
try (final BufferedReader reader = Files.newBufferedReader(path)) {
|
||||
final BiConsumer<R, String>[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new);
|
||||
final Stream<String> lines = reader.lines();
|
||||
return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator);
|
||||
}
|
||||
}
|
||||
|
||||
@SuppressWarnings("unchecked")
|
||||
private static Function<String, ?> getTypeParser(final Class<?> type) {
|
||||
try {
|
||||
return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class)));
|
||||
} catch (final NoSuchMethodException | IllegalAccessException e) {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private static <R> R deserializeRecordString(final String recordString, final BiConsumer<R, String>[] fieldParseSetters, final R object) {
|
||||
final int recordStringLength = recordString.length();
|
||||
int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0;
|
||||
while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) {
|
||||
final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex);
|
||||
String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip();
|
||||
if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) {
|
||||
final int fieldLength = field.length();
|
||||
if (countTrailing(field, '"') % 2 == 0) {
|
||||
tryFieldEndFromIndex = tryFieldEndIndex + 1;
|
||||
continue;
|
||||
} else
|
||||
field = field.substring(1, fieldLength - 1).replace("\"\"", "\"");
|
||||
}
|
||||
final BiConsumer<R, String> setter = fieldParseSetters[i];
|
||||
if (setter != null)
|
||||
setter.accept(object, field);
|
||||
tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1;
|
||||
i++;
|
||||
}
|
||||
return object;
|
||||
}
|
||||
|
||||
private static int countTrailing(final String str, final char c) {
|
||||
final int l = str.length();
|
||||
int count = 0;
|
||||
while (str.charAt(l - count - 1) == c && count < l)
|
||||
count++;
|
||||
return count;
|
||||
}
|
||||
|
||||
public static final class ReflectionTable {
|
||||
public static <T> String create(final T[] objects, boolean colored) {
|
||||
final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new);
|
||||
final List<List<ReflectionTable>> rows = new ArrayList<>();
|
||||
rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList()));
|
||||
rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList()));
|
||||
final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray());
|
||||
if (colored)
|
||||
IntStream.range(0, fields.length).forEach(i -> {
|
||||
final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics();
|
||||
rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax()));
|
||||
});
|
||||
MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s ");
|
||||
return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF));
|
||||
}
|
||||
|
||||
private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00);
|
||||
private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66);
|
||||
private static final String CONTROL = "\033";
|
||||
private static final String CSI = "[";
|
||||
private static final String LF = "\n";
|
||||
private static final String RESET = "0";
|
||||
private static final String BOLD = "1";
|
||||
private static final String ITALIC = "3";
|
||||
private static final String UNDERLINE = "4";
|
||||
private static final String BACKGROUND_RED = "41";
|
||||
private static final String FOREGROUND = "38";
|
||||
private static final String BACKGROUND = "48";
|
||||
private static final String TRUECOLOR = "2";
|
||||
private static final String SEPARATOR = ";";
|
||||
private static final String SGR = "m";
|
||||
private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR;
|
||||
private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR;
|
||||
private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR;
|
||||
private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR;
|
||||
private Object value;
|
||||
private String string;
|
||||
private boolean padRight;
|
||||
private String escape;
|
||||
|
||||
private ReflectionTable(final Object obj, final Field field) {
|
||||
try {
|
||||
value = field.get(obj);
|
||||
string = Objects.toString(value);
|
||||
padRight = !Number.class.isAssignableFrom(field.getType());
|
||||
escape = Objects.isNull(value) ? NULL_STYLE : "";
|
||||
} catch (final IllegalAccessException | IllegalArgumentException e) {
|
||||
value = null;
|
||||
string = e.getClass().getSimpleName();
|
||||
padRight = false;
|
||||
escape = ERROR_STYLE;
|
||||
}
|
||||
}
|
||||
|
||||
private ReflectionTable(final Field field) {
|
||||
value = null;
|
||||
string = field.getName();
|
||||
padRight = true;
|
||||
escape = HEADER_STYLE;
|
||||
}
|
||||
|
||||
private Number getValue() {
|
||||
return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0);
|
||||
}
|
||||
|
||||
private void colorByValue(final Number min, final Number max) {
|
||||
if (Objects.nonNull(value)) {
|
||||
final double range = max.doubleValue() - min.doubleValue();
|
||||
final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range;
|
||||
final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue()));
|
||||
escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true);
|
||||
}
|
||||
}
|
||||
|
||||
private static String colorTo24BitSGR(final Color color, final boolean background) {
|
||||
return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR;
|
||||
}
|
||||
|
||||
private static int range(final double normal, final int min, final int max) {
|
||||
return (int) (normal * (max - min) + min);
|
||||
}
|
||||
|
||||
/* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */
|
||||
private static float contrastRatio(final Color lighter, final Color darker) {
|
||||
return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f);
|
||||
}
|
||||
|
||||
/* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */
|
||||
private static float relativeLuminance(final Color color) {
|
||||
final float[] components = color.getRGBComponents(null);
|
||||
final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f);
|
||||
final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f);
|
||||
final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f);
|
||||
return 0.2126f * r + 0.7152f * g + 0.0722f * b;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,113 @@
|
||||
package frc4388.commands;
|
||||
|
||||
import org.junit.Test;
|
||||
|
||||
import frc4388.robot.commands.AimToCenter;
|
||||
import org.junit.Assert;
|
||||
|
||||
public class AimToCenterTest {
|
||||
|
||||
private static final double DELTA = 1e-15;
|
||||
|
||||
@Test
|
||||
public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
|
||||
boolean output;
|
||||
|
||||
//20 deg
|
||||
output = AimToCenter.isHardwareDeadzone(20.);
|
||||
Assert.assertFalse(output);
|
||||
|
||||
//-10 deg
|
||||
output = AimToCenter.isHardwareDeadzone(-10.);
|
||||
Assert.assertTrue(output);
|
||||
|
||||
//-1 deg
|
||||
output = AimToCenter.isHardwareDeadzone(-1.);
|
||||
Assert.assertTrue(output);
|
||||
|
||||
//341 deg
|
||||
output = AimToCenter.isHardwareDeadzone(341.);
|
||||
Assert.assertTrue(output);
|
||||
|
||||
//340 deg
|
||||
output = AimToCenter.isHardwareDeadzone(340.);
|
||||
Assert.assertFalse(output);
|
||||
|
||||
//0 deg
|
||||
output = AimToCenter.isHardwareDeadzone(0.);
|
||||
Assert.assertFalse(output);
|
||||
|
||||
//200 deg
|
||||
output = AimToCenter.isHardwareDeadzone(200.);
|
||||
Assert.assertFalse(output);
|
||||
|
||||
//2000000 deg
|
||||
output = AimToCenter.isHardwareDeadzone(2000000.);
|
||||
Assert.assertTrue(output);
|
||||
|
||||
//NaN deg
|
||||
output = AimToCenter.isHardwareDeadzone(Double.NaN);
|
||||
Assert.assertFalse(output);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() {
|
||||
double actual;
|
||||
double expected;
|
||||
|
||||
//(5,5) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(5., 5., 0.);
|
||||
expected = 180. + 45.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(-5,5) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(-5.0, 5., 0.);
|
||||
expected = 180. + 90. + 45.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(-5,-5) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(-5.0, -5., 0.);
|
||||
expected = 45.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(5,-5) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(5., -5., 0.);
|
||||
expected = 90. + 45.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(0,0) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(0., 0., 0.);
|
||||
Assert.assertNotNull(actual);
|
||||
|
||||
//(5,5) Gyro = 180 deg
|
||||
actual = AimToCenter.angleToCenter(5., 5., 180.);
|
||||
expected = 45.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(100,100) Gyro = 90 deg
|
||||
actual = AimToCenter.angleToCenter(100., 100., 90.);
|
||||
expected = 90. + 45.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(null,5) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.);
|
||||
expected = Double.NaN;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(null,null) Gyro = 0 deg
|
||||
actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.);
|
||||
expected = Double.NaN;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(null,null) Gyro = null deg
|
||||
actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN);
|
||||
expected = Double.NaN;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
//(5,5) Gyro = -20 deg
|
||||
actual = AimToCenter.angleToCenter(5., -5., -45.);
|
||||
expected = 180.;
|
||||
Assert.assertEquals(expected, actual, DELTA);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -17,6 +17,9 @@ import frc4388.utility.LEDPatterns;
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class LEDSubsystemTest {
|
||||
// Arrange
|
||||
Spark ledController = mock(Spark.class);
|
||||
LED led = new LED(ledController);
|
||||
@Test
|
||||
public void testConstructor() {
|
||||
// Arrange
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
{
|
||||
"fileName": "REVLib.json",
|
||||
"name": "REVLib",
|
||||
"version": "2022.1.1",
|
||||
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
|
||||
"mavenUrls": [
|
||||
"https://maven.revrobotics.com/"
|
||||
],
|
||||
"jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-java",
|
||||
"version": "2022.1.1"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2022.1.1",
|
||||
"skipInvalidPlatforms": true,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxaarch64bionic",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"osxx86-64"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-cpp",
|
||||
"version": "2022.1.1",
|
||||
"libName": "REVLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxaarch64bionic",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"osxx86-64"
|
||||
]
|
||||
},
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2022.1.1",
|
||||
"libName": "REVLibDriver",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxaarch64bionic",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"osxx86-64"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user