mirror of
https://github.com/Team4388/2023WayOfTheRobot.git
synced 2026-06-08 16:28:00 -06:00
formatting
This commit is contained in:
Connor Peach
@@ -1,3 +1,3 @@
|
||||
Files placed in this directory will be deployed to the RoboRIO into the
|
||||
'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
|
||||
Files placed in this directory will be deployed to the RoboRIO into the
|
||||
'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
|
||||
to get a proper path relative to the deploy directory.
|
||||
@@ -1,178 +1,178 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.RobotUnits;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be
|
||||
* declared globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class SwerveDriveConstants {
|
||||
|
||||
public static final double MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 0.8;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
public static final double SLOW_SPEED = 0.8;
|
||||
public static final double FAST_SPEED = 1.0;
|
||||
public static final double TURBO_SPEED = 4.0;
|
||||
|
||||
public static final class IDs {
|
||||
public static final int LEFT_FRONT_WHEEL_ID = 2;
|
||||
public static final int LEFT_FRONT_STEER_ID = 3;
|
||||
public static final int LEFT_FRONT_ENCODER_ID = 10;
|
||||
|
||||
public static final int RIGHT_FRONT_WHEEL_ID = 4;
|
||||
public static final int RIGHT_FRONT_STEER_ID = 5;
|
||||
public static final int RIGHT_FRONT_ENCODER_ID = 11;
|
||||
|
||||
public static final int LEFT_BACK_WHEEL_ID = 6;
|
||||
public static final int LEFT_BACK_STEER_ID = 7;
|
||||
public static final int LEFT_BACK_ENCODER_ID = 12;
|
||||
|
||||
public static final int RIGHT_BACK_WHEEL_ID = 8;
|
||||
public static final int RIGHT_BACK_STEER_ID = 9;
|
||||
public static final int RIGHT_BACK_ENCODER_ID = 13;
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
|
||||
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
|
||||
|
||||
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
|
||||
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final class Conversions {
|
||||
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
|
||||
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
|
||||
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
|
||||
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
|
||||
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 3.9;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
|
||||
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;
|
||||
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
|
||||
|
||||
public static final double TICK_TIME_TO_SECONDS = 10;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
|
||||
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
|
||||
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
public static final double HEIGHT = 18.5;
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class GyroConstants {
|
||||
public static final int ID = 14;
|
||||
}
|
||||
|
||||
public static final class ArmConstants {
|
||||
public static final double MIN_ARM_LEN = 1;
|
||||
public static final double MAX_ARM_LEN = 2;
|
||||
public static final double ARM_HEIGHT = 1;
|
||||
public static final double CURVE_POWER = 2;
|
||||
|
||||
public static final double TELE_TICKS_PER_SECOND = (-5);
|
||||
|
||||
public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value
|
||||
public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
|
||||
|
||||
public static final double TELE_FORWARD_SOFT_LIMIT = 100;
|
||||
public static final double TELE_REVERSE_SOFT_LIMIT = 0;
|
||||
|
||||
public static final double kP = 0;
|
||||
public static final double kI = 0;
|
||||
public static final double kD = 0;
|
||||
|
||||
public static final double OFFSET = 0;
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
// public static final int LED_SPARK_ID = 0;
|
||||
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
public static final double RIGHT_AXIS_DEADBAND = 0.6;
|
||||
public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
public static final String NAME = "photonCamera";
|
||||
|
||||
public static final int LIME_HIXELS = 640;
|
||||
public static final int LIME_VIXELS = 480;
|
||||
|
||||
public static final double H_FOV = 59.6;
|
||||
public static final double V_FOV = 45.7;
|
||||
|
||||
public static final double LIME_HEIGHT = 6.0;
|
||||
public static final double LIME_ANGLE = 55.0;
|
||||
|
||||
// public static final double HIGH_TARGET_HEIGHT = 46.0;
|
||||
public static final double HIGH_TAPE_HEIGHT = 44.0;
|
||||
|
||||
// public static final double MID_TARGET_HEIGHT = 34.0;
|
||||
public static final double MID_TAPE_HEIGHT = 24.0;
|
||||
|
||||
public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
|
||||
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.RobotUnits;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be
|
||||
* declared globally (i.e. public static). Do not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class SwerveDriveConstants {
|
||||
|
||||
public static final double MAX_ROT_SPEED = 1.5;
|
||||
public static final double MIN_ROT_SPEED = 0.8;
|
||||
public static double ROTATION_SPEED = MAX_ROT_SPEED;
|
||||
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
|
||||
|
||||
public static final double CORRECTION_MIN = 10;
|
||||
public static final double CORRECTION_MAX = 50;
|
||||
|
||||
public static final double SLOW_SPEED = 0.8;
|
||||
public static final double FAST_SPEED = 1.0;
|
||||
public static final double TURBO_SPEED = 4.0;
|
||||
|
||||
public static final class IDs {
|
||||
public static final int LEFT_FRONT_WHEEL_ID = 2;
|
||||
public static final int LEFT_FRONT_STEER_ID = 3;
|
||||
public static final int LEFT_FRONT_ENCODER_ID = 10;
|
||||
|
||||
public static final int RIGHT_FRONT_WHEEL_ID = 4;
|
||||
public static final int RIGHT_FRONT_STEER_ID = 5;
|
||||
public static final int RIGHT_FRONT_ENCODER_ID = 11;
|
||||
|
||||
public static final int LEFT_BACK_WHEEL_ID = 6;
|
||||
public static final int LEFT_BACK_STEER_ID = 7;
|
||||
public static final int LEFT_BACK_ENCODER_ID = 12;
|
||||
|
||||
public static final int RIGHT_BACK_WHEEL_ID = 8;
|
||||
public static final int RIGHT_BACK_STEER_ID = 9;
|
||||
public static final int RIGHT_BACK_ENCODER_ID = 13;
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
}
|
||||
|
||||
public static final class AutoConstants {
|
||||
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
|
||||
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
|
||||
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
|
||||
|
||||
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
|
||||
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final class Conversions {
|
||||
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
|
||||
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
|
||||
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
|
||||
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
|
||||
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 3.9;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
|
||||
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;
|
||||
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
|
||||
|
||||
public static final double TICK_TIME_TO_SECONDS = 10;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
|
||||
}
|
||||
|
||||
public static final class Configurations {
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
|
||||
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
|
||||
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
|
||||
}
|
||||
|
||||
public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
|
||||
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
|
||||
|
||||
// dimensions
|
||||
public static final double WIDTH = 18.5;
|
||||
public static final double HEIGHT = 18.5;
|
||||
public static final double HALF_WIDTH = WIDTH / 2.d;
|
||||
public static final double HALF_HEIGHT = HEIGHT / 2.d;
|
||||
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
|
||||
public static final class GyroConstants {
|
||||
public static final int ID = 14;
|
||||
}
|
||||
|
||||
public static final class ArmConstants {
|
||||
public static final double MIN_ARM_LEN = 1;
|
||||
public static final double MAX_ARM_LEN = 2;
|
||||
public static final double ARM_HEIGHT = 1;
|
||||
public static final double CURVE_POWER = 2;
|
||||
|
||||
public static final double TELE_TICKS_PER_SECOND = (-5);
|
||||
|
||||
public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value
|
||||
public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
|
||||
|
||||
public static final double TELE_FORWARD_SOFT_LIMIT = 100;
|
||||
public static final double TELE_REVERSE_SOFT_LIMIT = 0;
|
||||
|
||||
public static final double kP = 0;
|
||||
public static final double kI = 0;
|
||||
public static final double kD = 0;
|
||||
|
||||
public static final double OFFSET = 0;
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
// public static final int LED_SPARK_ID = 0;
|
||||
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
public static final double RIGHT_AXIS_DEADBAND = 0.6;
|
||||
public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
|
||||
}
|
||||
|
||||
public static final class VisionConstants {
|
||||
public static final String NAME = "photonCamera";
|
||||
|
||||
public static final int LIME_HIXELS = 640;
|
||||
public static final int LIME_VIXELS = 480;
|
||||
|
||||
public static final double H_FOV = 59.6;
|
||||
public static final double V_FOV = 45.7;
|
||||
|
||||
public static final double LIME_HEIGHT = 6.0;
|
||||
public static final double LIME_ANGLE = 55.0;
|
||||
|
||||
// public static final double HIGH_TARGET_HEIGHT = 46.0;
|
||||
public static final double HIGH_TAPE_HEIGHT = 44.0;
|
||||
|
||||
// public static final double MID_TARGET_HEIGHT = 34.0;
|
||||
public static final double MID_TAPE_HEIGHT = 24.0;
|
||||
|
||||
public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,29 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 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.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 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.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,134 +1,134 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2017-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.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.RobotTime;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
* functions corresponding to each mode, as described in the TimedRobot
|
||||
* documentation. If you change the name of this class or the package after
|
||||
* creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
private RobotContainer m_robotContainer;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
* used for any initialization code.
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every robot packet, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_robotTime.updateTimes();
|
||||
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
* the robot is disabled.
|
||||
*/
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
m_robotTime.endMatchTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void disabledExit() {
|
||||
DeferredBlock.execute();
|
||||
super.disabledExit();
|
||||
}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during autonomous.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
|
||||
// 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
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_robotTime.startMatchTime();
|
||||
|
||||
m_robotContainer.m_robotMap.restart_motor_tests();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
@Override
|
||||
public void testPeriodic() {
|
||||
m_robotContainer.m_robotMap.run_periodic_tests();
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2017-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.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.RobotTime;
|
||||
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
* functions corresponding to each mode, as described in the TimedRobot
|
||||
* documentation. If you change the name of this class or the package after
|
||||
* creating this project, you must also update the build.gradle file in the
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
private RobotContainer m_robotContainer;
|
||||
|
||||
/**
|
||||
* This function is run when the robot is first started up and should be
|
||||
* used for any initialization code.
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
|
||||
SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called every robot packet, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
*/
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_robotTime.updateTimes();
|
||||
|
||||
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
|
||||
// commands, running already-scheduled commands, removing finished or interrupted commands,
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once each time the robot enters Disabled mode.
|
||||
* You can use it to reset any subsystem information you want to clear when
|
||||
* the robot is disabled.
|
||||
*/
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
m_robotTime.endMatchTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void disabledExit() {
|
||||
DeferredBlock.execute();
|
||||
super.disabledExit();
|
||||
}
|
||||
|
||||
/**
|
||||
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during autonomous.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
|
||||
// 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
|
||||
// this line or comment it out.
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotContainer.m_robotSwerveDrive.resetGyro();
|
||||
m_robotTime.startMatchTime();
|
||||
|
||||
m_robotContainer.m_robotMap.restart_motor_tests();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called periodically during operator control.
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
*/
|
||||
@Override
|
||||
public void testPeriodic() {
|
||||
m_robotContainer.m_robotMap.run_periodic_tests();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,350 +1,350 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot;
|
||||
|
||||
import java.util.function.Consumer;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.REVLibError;
|
||||
import com.revrobotics.CANSparkMax.FaultID;
|
||||
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.ConditionalCommand;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.POVButton;
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.subsystems.Arm;
|
||||
import frc4388.robot.subsystems.Claw;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.commands.BooleanCommand;
|
||||
import frc4388.robot.commands.Arm.PivotCommand;
|
||||
import frc4388.robot.commands.Arm.TeleCommand;
|
||||
import frc4388.robot.commands.Autos.AutoBalance;
|
||||
import frc4388.robot.commands.Autos.PlaybackChooser;
|
||||
import frc4388.robot.commands.Placement.AprilRotAlign;
|
||||
import frc4388.robot.commands.Placement.LimeAlign;
|
||||
import frc4388.robot.commands.Swerve.RotateToAngle;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
|
||||
m_robotMap.rightFront,
|
||||
m_robotMap.leftBack,
|
||||
m_robotMap.rightBack,
|
||||
m_robotMap.gyro);
|
||||
|
||||
public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
|
||||
|
||||
public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
|
||||
|
||||
public final Limelight m_robotLimeLight = new Limelight();
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/* Autos */
|
||||
public SendableChooser<Command> chooser = new SendableChooser<>();
|
||||
|
||||
// private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
|
||||
|
||||
// private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
|
||||
// private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
|
||||
// private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
|
||||
|
||||
private PlaybackChooser playbackChooser;
|
||||
|
||||
/* Commands */
|
||||
private Command emptyCommand = new InstantCommand();
|
||||
private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
|
||||
|
||||
private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
|
||||
|
||||
private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
|
||||
|
||||
private boolean readyForPlacement = false;
|
||||
private Boolean isPole = null;
|
||||
|
||||
private SequentialCommandGroup alignToPole =
|
||||
new SequentialCommandGroup(
|
||||
new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
|
||||
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
|
||||
new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
|
||||
// new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
|
||||
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
|
||||
|
||||
// private SequentialCommandGroup alignToShelf =
|
||||
// new SequentialCommandGroup(
|
||||
// new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
// new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
|
||||
// new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
|
||||
// new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
// new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
|
||||
// ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
|
||||
|
||||
private SequentialCommandGroup alignToShelf =
|
||||
new SequentialCommandGroup(
|
||||
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
|
||||
new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
|
||||
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
|
||||
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
|
||||
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
|
||||
|
||||
public SequentialCommandGroup place = null;
|
||||
|
||||
private Consumer<SequentialCommandGroup> queuePlacement = (scg) -> {
|
||||
place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
|
||||
};
|
||||
|
||||
// TODO: find actual values
|
||||
private SequentialCommandGroup placeCubeHigh =
|
||||
new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> System.out.println("Placing cone high")),
|
||||
new PivotCommand(m_robotArm, 64 + 135),
|
||||
new InstantCommand(() -> m_robotArm.setRotVel(0)),
|
||||
new TeleCommand(m_robotArm, 95642),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 70 + 135),
|
||||
new TeleCommand(m_robotArm, 32866),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 0),
|
||||
new TeleCommand(m_robotArm, 0),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 0),
|
||||
new TeleCommand(m_robotArm, 0),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeLow = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 0),
|
||||
new TeleCommand(m_robotArm, 0),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
|
||||
// * Default Commands
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
m_robotArm.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
|
||||
m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
|
||||
}, m_robotArm)
|
||||
.withName("Arm DefaultCommand"));
|
||||
|
||||
// * Auto Commands
|
||||
chooser.setDefaultOption("NoAuto", emptyCommand);
|
||||
chooser.addOption("alignToPole", alignToPole);
|
||||
chooser.addOption("alignToShelf", alignToShelf);
|
||||
|
||||
chooser.addOption("placeConeHigh", placeConeHigh);
|
||||
chooser.addOption("placeConeMid", placeConeMid);
|
||||
chooser.addOption("placeCubeHigh", placeCubeHigh);
|
||||
chooser.addOption("placeCubeMid", placeCubeMid);
|
||||
chooser.addOption("placeLow", placeLow);
|
||||
|
||||
// chooser.addOption("Blue1Path", blue1Path);
|
||||
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
|
||||
|
||||
// chooser.addOption("Red1Path", red1Path);
|
||||
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
|
||||
|
||||
playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
|
||||
.addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
|
||||
.buildDisplay();
|
||||
}
|
||||
|
||||
// here be dragons - enter if you dare
|
||||
private static class TapState {
|
||||
public boolean gearTapped = true;
|
||||
public long gearTime = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
// * Driver Buttons
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
|
||||
|
||||
// because closure reasons - ask Daniel Thomas
|
||||
// final TapState tap = new TapState();
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
|
||||
// .onTrue(new InstantCommand(() -> {
|
||||
// tap.gearTapped = true;
|
||||
// tap.gearTime = System.currentTimeMillis();
|
||||
// }))
|
||||
// .whileTrue(new RunCommand(() -> {
|
||||
// if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
|
||||
// m_robotSwerveDrive.setToTurbo();
|
||||
// tap.gearTapped = false;
|
||||
// }
|
||||
// }))
|
||||
// .onFalse(new InstantCommand(() -> {
|
||||
// if (tap.gearTapped)
|
||||
// m_robotSwerveDrive.setToFast();
|
||||
// else
|
||||
// m_robotSwerveDrive.setToSlow();
|
||||
// }));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(interruptCommand.asProxy()); // final
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
|
||||
// () -> getDeadbandedDriverController().getLeftX(),
|
||||
// () -> getDeadbandedDriverController().getLeftY(),
|
||||
// () -> getDeadbandedDriverController().getRightX(),
|
||||
// () -> getDeadbandedDriverController().getRightY(),
|
||||
// "Blue1Path.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
// * Operator Buttons
|
||||
|
||||
// align (pole)
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
|
||||
.onTrue(alignToPole)
|
||||
.onFalse(interruptCommand.asProxy());
|
||||
|
||||
// align (shelf)
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
|
||||
.onTrue(alignToShelf)
|
||||
.onFalse(interruptCommand.asProxy());
|
||||
|
||||
// toggle claw
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
|
||||
.onTrue(toggleClaw.asProxy());
|
||||
|
||||
// kill soft limits
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
|
||||
|
||||
// SmartDashboard.putBoolean("isn't SparkMAX", );
|
||||
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
|
||||
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
|
||||
|
||||
//Arm to Home
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(armToHome.asProxy());
|
||||
|
||||
//Interupt Button
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(interruptCommand.asProxy());
|
||||
|
||||
|
||||
// place high
|
||||
new POVButton(getDeadbandedOperatorController(), 0)
|
||||
.onTrue(new ConditionalCommand(
|
||||
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
|
||||
emptyCommand.asProxy(),
|
||||
() -> readyForPlacement == true)
|
||||
);
|
||||
|
||||
// place mid
|
||||
new POVButton(getDeadbandedOperatorController(), 270)
|
||||
.onTrue(new ConditionalCommand(
|
||||
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
|
||||
emptyCommand.asProxy(),
|
||||
() -> readyForPlacement == true)
|
||||
);
|
||||
|
||||
// place low
|
||||
new POVButton(getDeadbandedOperatorController(), 180)
|
||||
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
|
||||
|
||||
// confirm
|
||||
new POVButton(getDeadbandedOperatorController(), 90)
|
||||
.onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// return chooser.getSelected();
|
||||
return playbackChooser.getCommand();
|
||||
}
|
||||
|
||||
public DeadbandedXboxController getDeadbandedDriverController() {
|
||||
return this.m_driverXbox;
|
||||
}
|
||||
|
||||
public DeadbandedXboxController getDeadbandedOperatorController() {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
}
|
||||
x/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot;
|
||||
|
||||
import java.util.function.Consumer;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.REVLibError;
|
||||
import com.revrobotics.CANSparkMax.FaultID;
|
||||
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.ConditionalCommand;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.POVButton;
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.subsystems.Arm;
|
||||
import frc4388.robot.subsystems.Claw;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.commands.BooleanCommand;
|
||||
import frc4388.robot.commands.Arm.PivotCommand;
|
||||
import frc4388.robot.commands.Arm.TeleCommand;
|
||||
import frc4388.robot.commands.Autos.AutoBalance;
|
||||
import frc4388.robot.commands.Autos.PlaybackChooser;
|
||||
import frc4388.robot.commands.Placement.AprilRotAlign;
|
||||
import frc4388.robot.commands.Placement.LimeAlign;
|
||||
import frc4388.robot.commands.Swerve.RotateToAngle;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
|
||||
m_robotMap.rightFront,
|
||||
m_robotMap.leftBack,
|
||||
m_robotMap.rightBack,
|
||||
m_robotMap.gyro);
|
||||
|
||||
public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
|
||||
|
||||
public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
|
||||
|
||||
public final Limelight m_robotLimeLight = new Limelight();
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/* Autos */
|
||||
public SendableChooser<Command> chooser = new SendableChooser<>();
|
||||
|
||||
// private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
|
||||
|
||||
// private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
|
||||
// private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
|
||||
// private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
|
||||
|
||||
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
|
||||
|
||||
private PlaybackChooser playbackChooser;
|
||||
|
||||
/* Commands */
|
||||
private Command emptyCommand = new InstantCommand();
|
||||
private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
|
||||
|
||||
private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
|
||||
|
||||
private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
|
||||
|
||||
private boolean readyForPlacement = false;
|
||||
private Boolean isPole = null;
|
||||
|
||||
private SequentialCommandGroup alignToPole =
|
||||
new SequentialCommandGroup(
|
||||
new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
|
||||
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
|
||||
new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
|
||||
// new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
|
||||
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
|
||||
|
||||
// private SequentialCommandGroup alignToShelf =
|
||||
// new SequentialCommandGroup(
|
||||
// new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
// new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
|
||||
// new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
|
||||
// new RotateToAngle(m_robotSwerveDrive, 0.0),
|
||||
// new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
|
||||
// ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
|
||||
|
||||
private SequentialCommandGroup alignToShelf =
|
||||
new SequentialCommandGroup(
|
||||
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
|
||||
new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
|
||||
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
|
||||
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
|
||||
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
|
||||
|
||||
public SequentialCommandGroup place = null;
|
||||
|
||||
private Consumer<SequentialCommandGroup> queuePlacement = (scg) -> {
|
||||
place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
|
||||
};
|
||||
|
||||
// TODO: find actual values
|
||||
private SequentialCommandGroup placeCubeHigh =
|
||||
new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> System.out.println("Placing cone high")),
|
||||
new PivotCommand(m_robotArm, 64 + 135),
|
||||
new InstantCommand(() -> m_robotArm.setRotVel(0)),
|
||||
new TeleCommand(m_robotArm, 95642),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 70 + 135),
|
||||
new TeleCommand(m_robotArm, 32866),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 0),
|
||||
new TeleCommand(m_robotArm, 0),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 0),
|
||||
new TeleCommand(m_robotArm, 0),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
private SequentialCommandGroup placeLow = new SequentialCommandGroup(
|
||||
new PivotCommand(m_robotArm, 0),
|
||||
new TeleCommand(m_robotArm, 0),
|
||||
toggleClaw.asProxy(),
|
||||
armToHome.asProxy()
|
||||
);
|
||||
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
|
||||
// * Default Commands
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
m_robotArm.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
|
||||
m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
|
||||
}, m_robotArm)
|
||||
.withName("Arm DefaultCommand"));
|
||||
|
||||
// * Auto Commands
|
||||
chooser.setDefaultOption("NoAuto", emptyCommand);
|
||||
chooser.addOption("alignToPole", alignToPole);
|
||||
chooser.addOption("alignToShelf", alignToShelf);
|
||||
|
||||
chooser.addOption("placeConeHigh", placeConeHigh);
|
||||
chooser.addOption("placeConeMid", placeConeMid);
|
||||
chooser.addOption("placeCubeHigh", placeCubeHigh);
|
||||
chooser.addOption("placeCubeMid", placeCubeMid);
|
||||
chooser.addOption("placeLow", placeLow);
|
||||
|
||||
// chooser.addOption("Blue1Path", blue1Path);
|
||||
// chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
|
||||
|
||||
// chooser.addOption("Red1Path", red1Path);
|
||||
// chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
|
||||
|
||||
playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
|
||||
.addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
|
||||
.buildDisplay();
|
||||
}
|
||||
|
||||
// here be dragons - enter if you dare
|
||||
private static class TapState {
|
||||
public boolean gearTapped = true;
|
||||
public long gearTime = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
// * Driver Buttons
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
|
||||
|
||||
// because closure reasons - ask Daniel Thomas
|
||||
// final TapState tap = new TapState();
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
|
||||
// .onTrue(new InstantCommand(() -> {
|
||||
// tap.gearTapped = true;
|
||||
// tap.gearTime = System.currentTimeMillis();
|
||||
// }))
|
||||
// .whileTrue(new RunCommand(() -> {
|
||||
// if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
|
||||
// m_robotSwerveDrive.setToTurbo();
|
||||
// tap.gearTapped = false;
|
||||
// }
|
||||
// }))
|
||||
// .onFalse(new InstantCommand(() -> {
|
||||
// if (tap.gearTapped)
|
||||
// m_robotSwerveDrive.setToFast();
|
||||
// else
|
||||
// m_robotSwerveDrive.setToSlow();
|
||||
// }));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(interruptCommand.asProxy()); // final
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
|
||||
// () -> getDeadbandedDriverController().getLeftX(),
|
||||
// () -> getDeadbandedDriverController().getLeftY(),
|
||||
// () -> getDeadbandedDriverController().getRightX(),
|
||||
// () -> getDeadbandedDriverController().getRightY(),
|
||||
// "Blue1Path.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
// * Operator Buttons
|
||||
|
||||
// align (pole)
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
|
||||
.onTrue(alignToPole)
|
||||
.onFalse(interruptCommand.asProxy());
|
||||
|
||||
// align (shelf)
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
|
||||
.onTrue(alignToShelf)
|
||||
.onFalse(interruptCommand.asProxy());
|
||||
|
||||
// toggle claw
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
|
||||
.onTrue(toggleClaw.asProxy());
|
||||
|
||||
// kill soft limits
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
|
||||
|
||||
// SmartDashboard.putBoolean("isn't SparkMAX", );
|
||||
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
|
||||
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
|
||||
|
||||
//Arm to Home
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(armToHome.asProxy());
|
||||
|
||||
//Interupt Button
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(interruptCommand.asProxy());
|
||||
|
||||
|
||||
// place high
|
||||
new POVButton(getDeadbandedOperatorController(), 0)
|
||||
.onTrue(new ConditionalCommand(
|
||||
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
|
||||
emptyCommand.asProxy(),
|
||||
() -> readyForPlacement == true)
|
||||
);
|
||||
|
||||
// place mid
|
||||
new POVButton(getDeadbandedOperatorController(), 270)
|
||||
.onTrue(new ConditionalCommand(
|
||||
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
|
||||
emptyCommand.asProxy(),
|
||||
() -> readyForPlacement == true)
|
||||
);
|
||||
|
||||
// place low
|
||||
new POVButton(getDeadbandedOperatorController(), 180)
|
||||
.onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
|
||||
|
||||
// confirm
|
||||
new POVButton(getDeadbandedOperatorController(), 90)
|
||||
.onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// return chooser.getSelected();
|
||||
return playbackChooser.getCommand();
|
||||
}
|
||||
|
||||
public DeadbandedXboxController getDeadbandedDriverController() {
|
||||
return this.m_driverXbox;
|
||||
}
|
||||
|
||||
public DeadbandedXboxController getDeadbandedOperatorController() {
|
||||
return this.m_operatorXbox;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -1,279 +1,279 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot;
|
||||
|
||||
import java.lang.reflect.Field;
|
||||
import java.lang.reflect.Member;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
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.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
* testing and modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
|
||||
public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightBack;
|
||||
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
configureDriveMotors();
|
||||
configArmMotors();
|
||||
|
||||
try {
|
||||
setup_motor_tests();
|
||||
} catch (IllegalArgumentException | IllegalAccessException e) {
|
||||
// TODO Auto-generated catch block
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {
|
||||
|
||||
}
|
||||
|
||||
// swerve drive subsystem
|
||||
public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
|
||||
public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
|
||||
public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
|
||||
public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
|
||||
public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
|
||||
|
||||
void configureDriveMotors() {
|
||||
// config factory default
|
||||
leftFrontWheel.configFactoryDefault();
|
||||
leftFrontSteer.configFactoryDefault();
|
||||
|
||||
rightFrontWheel.configFactoryDefault();
|
||||
rightFrontSteer.configFactoryDefault();
|
||||
|
||||
leftBackWheel.configFactoryDefault();
|
||||
leftBackSteer.configFactoryDefault();
|
||||
|
||||
rightBackWheel.configFactoryDefault();
|
||||
rightBackSteer.configFactoryDefault();
|
||||
|
||||
// config open loop ramp
|
||||
leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config closed loop ramp
|
||||
leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config neutral deadband
|
||||
leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// set neutral mode
|
||||
leftFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
leftFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
// initialize SwerveModules
|
||||
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
|
||||
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
|
||||
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
|
||||
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
|
||||
}
|
||||
|
||||
// arm stuff
|
||||
public WPI_TalonFX pivot = new WPI_TalonFX(15);
|
||||
public WPI_TalonFX tele = new WPI_TalonFX(16);
|
||||
public CANCoder pivotEncoder = new CANCoder(17);
|
||||
|
||||
public void configArmMotors() {
|
||||
// config factory default
|
||||
pivot.configFactoryDefault();
|
||||
tele.configFactoryDefault();
|
||||
|
||||
// config open loop ramp
|
||||
pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config closed loop ramp
|
||||
pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config neutral mode to brake
|
||||
pivot.setNeutralMode(NeutralMode.Brake);
|
||||
tele.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
// soft limits
|
||||
pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
|
||||
pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
|
||||
pivot.configForwardSoftLimitEnable(false);
|
||||
pivot.configReverseSoftLimitEnable(false);
|
||||
|
||||
tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
|
||||
tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
|
||||
tele.configForwardSoftLimitEnable(false);
|
||||
tele.configReverseSoftLimitEnable(false);
|
||||
}
|
||||
|
||||
// claw stuff (WHAT IS A SOAP ENGINEER)
|
||||
PWM leftClaw = new PWM(0);
|
||||
PWM rightClaw = new PWM(1);
|
||||
CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
|
||||
|
||||
// ============================================
|
||||
// =============== AUTO TESTING ===============
|
||||
// ============================================
|
||||
|
||||
static class MotorTest {
|
||||
boolean result;
|
||||
String name;
|
||||
|
||||
enum MotorType {FALCON, NEO}
|
||||
|
||||
// why no union :(
|
||||
MotorType motor_type;
|
||||
WPI_TalonFX as_falcon;
|
||||
CANSparkMax as_neo;
|
||||
}
|
||||
|
||||
private static final long TEST_TIME = 500;
|
||||
|
||||
public ArrayList<MotorTest> motor_tests = new ArrayList<>();
|
||||
private int test_count = 0;
|
||||
private long test_time = 0;
|
||||
private double motor_position = 0;
|
||||
|
||||
public void restart_motor_tests() {
|
||||
for (MotorTest test : motor_tests) {
|
||||
test.result = false;
|
||||
}
|
||||
|
||||
test_time = 0;
|
||||
test_count = 0;
|
||||
motor_position = 0;
|
||||
}
|
||||
|
||||
public boolean run_periodic_tests() {
|
||||
if (test_count >= motor_tests.size()) return true;
|
||||
|
||||
MotorTest current_test = motor_tests.get(test_count);
|
||||
|
||||
if (test_time == 0) {
|
||||
test_time = System.currentTimeMillis();
|
||||
|
||||
switch (current_test.motor_type) {
|
||||
case FALCON:
|
||||
motor_position = current_test.as_falcon.getSelectedSensorPosition();
|
||||
current_test.as_falcon.set(.1);
|
||||
break;
|
||||
|
||||
case NEO:
|
||||
// TODO: destroy robot
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (System.currentTimeMillis() - test_time >= TEST_TIME) {
|
||||
switch (current_test.motor_type) {
|
||||
case FALCON:
|
||||
{
|
||||
double new_pos = current_test.as_falcon.getSelectedSensorPosition();
|
||||
if (Math.abs(new_pos - motor_position) > .1) {
|
||||
current_test.result = true;
|
||||
} else {
|
||||
System.out.printf("[DISCONNECTED] %s\n", current_test.name);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NEO: break;
|
||||
}
|
||||
|
||||
test_time = 0;
|
||||
test_count++;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException {
|
||||
Class map_clazz = this.getClass();
|
||||
|
||||
for (Field field : map_clazz.getFields()) {
|
||||
if (field.getType().equals(WPI_TalonFX.class)) {
|
||||
MotorTest test = new MotorTest();
|
||||
test.result = false;
|
||||
test.name = field.getName();
|
||||
test.motor_type = MotorTest.MotorType.FALCON;
|
||||
test.as_falcon = (WPI_TalonFX) field.get(this);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot;
|
||||
|
||||
import java.lang.reflect.Field;
|
||||
import java.lang.reflect.Member;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
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.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
* testing and modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
|
||||
public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightBack;
|
||||
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
configureDriveMotors();
|
||||
configArmMotors();
|
||||
|
||||
try {
|
||||
setup_motor_tests();
|
||||
} catch (IllegalArgumentException | IllegalAccessException e) {
|
||||
// TODO Auto-generated catch block
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {
|
||||
|
||||
}
|
||||
|
||||
// swerve drive subsystem
|
||||
public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
|
||||
public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
|
||||
public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
|
||||
public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
|
||||
|
||||
public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
|
||||
public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
|
||||
|
||||
void configureDriveMotors() {
|
||||
// config factory default
|
||||
leftFrontWheel.configFactoryDefault();
|
||||
leftFrontSteer.configFactoryDefault();
|
||||
|
||||
rightFrontWheel.configFactoryDefault();
|
||||
rightFrontSteer.configFactoryDefault();
|
||||
|
||||
leftBackWheel.configFactoryDefault();
|
||||
leftBackSteer.configFactoryDefault();
|
||||
|
||||
rightBackWheel.configFactoryDefault();
|
||||
rightBackSteer.configFactoryDefault();
|
||||
|
||||
// config open loop ramp
|
||||
leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config closed loop ramp
|
||||
leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config neutral deadband
|
||||
leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// set neutral mode
|
||||
leftFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontWheel.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackWheel.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
leftFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontSteer.setNeutralMode(NeutralMode.Brake);
|
||||
leftBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
rightBackSteer.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
// initialize SwerveModules
|
||||
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
|
||||
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
|
||||
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
|
||||
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
|
||||
}
|
||||
|
||||
// arm stuff
|
||||
public WPI_TalonFX pivot = new WPI_TalonFX(15);
|
||||
public WPI_TalonFX tele = new WPI_TalonFX(16);
|
||||
public CANCoder pivotEncoder = new CANCoder(17);
|
||||
|
||||
public void configArmMotors() {
|
||||
// config factory default
|
||||
pivot.configFactoryDefault();
|
||||
tele.configFactoryDefault();
|
||||
|
||||
// config open loop ramp
|
||||
pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config closed loop ramp
|
||||
pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
|
||||
|
||||
// config neutral mode to brake
|
||||
pivot.setNeutralMode(NeutralMode.Brake);
|
||||
tele.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
// soft limits
|
||||
pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
|
||||
pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
|
||||
pivot.configForwardSoftLimitEnable(false);
|
||||
pivot.configReverseSoftLimitEnable(false);
|
||||
|
||||
tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
|
||||
tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
|
||||
tele.configForwardSoftLimitEnable(false);
|
||||
tele.configReverseSoftLimitEnable(false);
|
||||
}
|
||||
|
||||
// claw stuff (WHAT IS A SOAP ENGINEER)
|
||||
PWM leftClaw = new PWM(0);
|
||||
PWM rightClaw = new PWM(1);
|
||||
CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
|
||||
|
||||
// ============================================
|
||||
// =============== AUTO TESTING ===============
|
||||
// ============================================
|
||||
|
||||
static class MotorTest {
|
||||
boolean result;
|
||||
String name;
|
||||
|
||||
enum MotorType {FALCON, NEO}
|
||||
|
||||
// why no union :(
|
||||
MotorType motor_type;
|
||||
WPI_TalonFX as_falcon;
|
||||
CANSparkMax as_neo;
|
||||
}
|
||||
|
||||
private static final long TEST_TIME = 500;
|
||||
|
||||
public ArrayList<MotorTest> motor_tests = new ArrayList<>();
|
||||
private int test_count = 0;
|
||||
private long test_time = 0;
|
||||
private double motor_position = 0;
|
||||
|
||||
public void restart_motor_tests() {
|
||||
for (MotorTest test : motor_tests) {
|
||||
test.result = false;
|
||||
}
|
||||
|
||||
test_time = 0;
|
||||
test_count = 0;
|
||||
motor_position = 0;
|
||||
}
|
||||
|
||||
public boolean run_periodic_tests() {
|
||||
if (test_count >= motor_tests.size()) return true;
|
||||
|
||||
MotorTest current_test = motor_tests.get(test_count);
|
||||
|
||||
if (test_time == 0) {
|
||||
test_time = System.currentTimeMillis();
|
||||
|
||||
switch (current_test.motor_type) {
|
||||
case FALCON:
|
||||
motor_position = current_test.as_falcon.getSelectedSensorPosition();
|
||||
current_test.as_falcon.set(.1);
|
||||
break;
|
||||
|
||||
case NEO:
|
||||
// TODO: destroy robot
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (System.currentTimeMillis() - test_time >= TEST_TIME) {
|
||||
switch (current_test.motor_type) {
|
||||
case FALCON:
|
||||
{
|
||||
double new_pos = current_test.as_falcon.getSelectedSensorPosition();
|
||||
if (Math.abs(new_pos - motor_position) > .1) {
|
||||
current_test.result = true;
|
||||
} else {
|
||||
System.out.printf("[DISCONNECTED] %s\n", current_test.name);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case NEO: break;
|
||||
}
|
||||
|
||||
test_time = 0;
|
||||
test_count++;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException {
|
||||
Class map_clazz = this.getClass();
|
||||
|
||||
for (Field field : map_clazz.getFields()) {
|
||||
if (field.getType().equals(WPI_TalonFX.class)) {
|
||||
MotorTest test = new MotorTest();
|
||||
test.result = false;
|
||||
test.name = field.getName();
|
||||
test.motor_type = MotorTest.MotorType.FALCON;
|
||||
test.as_falcon = (WPI_TalonFX) field.get(this);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,33 +1,33 @@
|
||||
// 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.Arm;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Arm;
|
||||
|
||||
public class PivotCommand extends PelvicInflammatoryDisease {
|
||||
private final Arm arm;
|
||||
private final double target;
|
||||
|
||||
/** Creates a new ArmCommand. */
|
||||
public PivotCommand(Arm arm, double target) {
|
||||
super(6, 1.5, 0, 0, 0.015);
|
||||
this.arm = arm;
|
||||
this.target = target;
|
||||
addRequirements(arm);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return (target - arm.getArmRotation()) / 360;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
SmartDashboard.putNumber("pivot output", output);
|
||||
arm.setRotVel(output);
|
||||
}
|
||||
}
|
||||
// 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.Arm;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Arm;
|
||||
|
||||
public class PivotCommand extends PelvicInflammatoryDisease {
|
||||
private final Arm arm;
|
||||
private final double target;
|
||||
|
||||
/** Creates a new ArmCommand. */
|
||||
public PivotCommand(Arm arm, double target) {
|
||||
super(6, 1.5, 0, 0, 0.015);
|
||||
this.arm = arm;
|
||||
this.target = target;
|
||||
addRequirements(arm);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return (target - arm.getArmRotation()) / 360;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
SmartDashboard.putNumber("pivot output", output);
|
||||
arm.setRotVel(output);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,39 +1,39 @@
|
||||
// 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.Arm;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Arm;
|
||||
|
||||
public class TeleCommand extends CommandBase {
|
||||
private final Arm arm;
|
||||
private final double target;
|
||||
private boolean goIn;
|
||||
|
||||
/** Creates a new ArmCommand. */
|
||||
public TeleCommand(Arm arm, double target) {
|
||||
this.arm = arm;
|
||||
this.target = target;
|
||||
addRequirements(arm);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
this.goIn = target < arm.getArmLength();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
arm.setTeleVel(goIn ? 1 : -1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (goIn) return arm.getArmLength() < target;
|
||||
else return arm.getArmLength() > target;
|
||||
}
|
||||
}
|
||||
// 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.Arm;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Arm;
|
||||
|
||||
public class TeleCommand extends CommandBase {
|
||||
private final Arm arm;
|
||||
private final double target;
|
||||
private boolean goIn;
|
||||
|
||||
/** Creates a new ArmCommand. */
|
||||
public TeleCommand(Arm arm, double target) {
|
||||
this.arm = arm;
|
||||
this.target = target;
|
||||
addRequirements(arm);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
this.goIn = target < arm.getArmLength();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
arm.setTeleVel(goIn ? 1 : -1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (goIn) return arm.getArmLength() < target;
|
||||
else return arm.getArmLength() > target;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,42 +1,42 @@
|
||||
// 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.Autos;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
public class AutoBalance extends PelvicInflammatoryDisease {
|
||||
RobotGyro gyro;
|
||||
SwerveDrive drive;
|
||||
|
||||
/** Creates a new AutoBalance. */
|
||||
public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
|
||||
super(0.6, 0, 0, 0, 0);
|
||||
|
||||
this.gyro = gyro;
|
||||
this.drive = drive;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
var pitch = gyro.getRoll();
|
||||
SmartDashboard.putNumber("pitch", pitch);
|
||||
return pitch;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
double out2 = MathUtil.clamp(output / 40, -.5, .5);
|
||||
|
||||
if (Math.abs(getError()) < 3) out2 = 0;
|
||||
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
|
||||
}
|
||||
}
|
||||
// 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.Autos;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
public class AutoBalance extends PelvicInflammatoryDisease {
|
||||
RobotGyro gyro;
|
||||
SwerveDrive drive;
|
||||
|
||||
/** Creates a new AutoBalance. */
|
||||
public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
|
||||
super(0.6, 0, 0, 0, 0);
|
||||
|
||||
this.gyro = gyro;
|
||||
this.drive = drive;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
var pitch = gyro.getRoll();
|
||||
SmartDashboard.putNumber("pitch", pitch);
|
||||
return pitch;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
double out2 = MathUtil.clamp(output / 40, -.5, .5);
|
||||
|
||||
if (Math.abs(getError()) < 3) out2 = 0;
|
||||
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,150 +1,150 @@
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,2
|
||||
0.0,0.0,0.0,0.0,18
|
||||
0.0,0.0,0.0,0.0,31
|
||||
0.0,0.0,0.0,0.0,61
|
||||
0.0,0.0,0.0,0.0,72
|
||||
0.0,0.0,0.0,0.0,85
|
||||
0.0,0.0,0.0,0.0,102
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,163
|
||||
0.0,0.0,0.0,0.0,175
|
||||
0.0,0.0,0.0,0.0,188
|
||||
0.0,0.0,0.0,0.0,205
|
||||
0.0,0.0,0.0,0.0,217
|
||||
0.0,0.0,0.0,0.0,229
|
||||
0.0,0.0,0.0,0.0,241
|
||||
0.0,0.0,0.0,0.0,261
|
||||
0.0,0.0,0.0,0.0,281
|
||||
0.0,0.0,0.0,0.0,302
|
||||
0.0,0.0,0.0,0.0,322
|
||||
0.0,0.0,0.0,0.0,341
|
||||
0.0,0.0,0.0,0.0,361
|
||||
0.0,-0.1328125,0.0,0.0,397
|
||||
0.0,-0.1328125,0.0,0.0,412
|
||||
0.0,-0.1484375,0.0,0.0,425
|
||||
0.0,-0.1796875,0.0,0.0,441
|
||||
0.0,-0.1875,0.0,0.0,461
|
||||
0.0,-0.1953125,0.0,0.0,481
|
||||
0.0,-0.234375,0.0,0.0,502
|
||||
0.0,-0.2890625,0.0,0.0,521
|
||||
0.0,-0.3125,0.0,0.0,541
|
||||
0.0,-0.328125,0.0,0.0,561
|
||||
0.0,-0.3671875,0.0,0.0,582
|
||||
0.0,-0.390625,0.0,0.0,601
|
||||
0.0,-0.4609375,0.0,0.0,642
|
||||
0.0,-0.4765625,0.0,0.0,653
|
||||
0.0,-0.4765625,0.0,0.0,666
|
||||
0.0,-0.4765625,0.0,0.0,681
|
||||
0.0,-0.4765625,0.0,0.0,702
|
||||
0.0,-0.4765625,0.0,0.0,721
|
||||
0.0,-0.4765625,0.0,0.0,741
|
||||
0.0,-0.4765625,0.0,0.0,762
|
||||
0.0,-0.4765625,0.0,0.0,782
|
||||
0.0,-0.484375,0.0,0.0,803
|
||||
0.0,-0.484375,0.0,0.0,821
|
||||
0.0,-0.4921875,0.0,0.0,842
|
||||
0.0,-0.5078125,0.0,0.0,878
|
||||
0.0,-0.5234375,0.0,0.0,893
|
||||
0.0,-0.5234375,0.0,0.0,906
|
||||
0.0,-0.53125,0.0,0.0,922
|
||||
0.0,-0.53125,0.0,0.0,942
|
||||
0.0,-0.53125,0.0,0.0,962
|
||||
0.0,-0.53125,0.0,0.0,982
|
||||
0.0,-0.5390625,0.0,0.0,1001
|
||||
0.0,-0.5390625,0.0,0.0,1022
|
||||
0.0,-0.546875,0.0,0.0,1042
|
||||
0.0,-0.546875,0.0,0.0,1061
|
||||
0.0,-0.546875,0.0,0.0,1082
|
||||
0.0,-0.546875,0.0,0.0,1164
|
||||
0.0,-0.546875,0.0,0.0,1176
|
||||
0.0,-0.546875,0.0,0.0,1190
|
||||
0.0,-0.546875,0.0,0.0,1202
|
||||
0.0,-0.546875,0.0,0.0,1219
|
||||
0.0,-0.546875,0.0,0.0,1230
|
||||
0.0,-0.546875,0.0,0.0,1244
|
||||
0.0,-0.546875,0.0,0.0,1258
|
||||
0.0,-0.546875,0.0,0.0,1268
|
||||
0.0,-0.546875,0.0,0.0,1281
|
||||
0.0,-0.546875,0.0,0.0,1301
|
||||
0.0,-0.546875,0.0,0.0,1321
|
||||
0.0,-0.546875,0.0,0.0,1363
|
||||
0.0,-0.546875,0.0,0.0,1376
|
||||
0.0,-0.546875,0.0,0.0,1388
|
||||
0.0,-0.546875,0.0,0.0,1402
|
||||
0.0,-0.546875,0.0,0.0,1422
|
||||
0.0,-0.546875,0.0,0.0,1441
|
||||
0.0,-0.546875,0.0,0.0,1461
|
||||
0.0,-0.546875,0.0,0.0,1482
|
||||
0.0,-0.546875,0.0,0.0,1502
|
||||
0.0,-0.546875,0.0,0.0,1521
|
||||
0.0,-0.546875,0.0,0.0,1542
|
||||
0.0,-0.546875,0.0,0.0,1562
|
||||
0.0,-0.546875,0.0,0.0,1598
|
||||
0.0,-0.546875,0.0,0.0,1609
|
||||
0.0,-0.546875,0.0,0.0,1621
|
||||
0.0,-0.546875,0.0,0.0,1642
|
||||
0.0,-0.546875,0.0,0.0,1661
|
||||
0.0,-0.546875,0.0,0.0,1682
|
||||
0.0,-0.546875,0.0,0.0,1702
|
||||
0.0,-0.546875,0.0,0.0,1722
|
||||
0.0,-0.546875,0.0,0.0,1742
|
||||
0.0,-0.546875,0.0,0.0,1762
|
||||
0.0,-0.546875,0.0,0.0,1781
|
||||
0.0,-0.5390625,0.0,0.0,1801
|
||||
0.0,-0.5390625,0.0,0.0,1835
|
||||
0.0,-0.5390625,0.0,0.0,1847
|
||||
0.0,-0.5390625,0.0,0.0,1861
|
||||
0.0,-0.5390625,0.0,0.0,1882
|
||||
0.0,-0.5390625,0.0,0.0,1901
|
||||
0.0,-0.5390625,0.0,0.0,1921
|
||||
0.0,-0.5390625,0.0,0.0,1941
|
||||
0.0,-0.5390625,0.0,0.0,1962
|
||||
0.0,-0.5390625,0.0,0.0,1983
|
||||
0.0,-0.5390625,0.0,0.0,2001
|
||||
0.0,-0.5390625,0.0,0.0,2022
|
||||
0.0,-0.5390625,0.0,0.0,2042
|
||||
0.0,-0.5390625,0.0,0.0,2085
|
||||
0.0,-0.5390625,0.0,0.0,2097
|
||||
0.0,-0.5390625,0.0,0.0,2113
|
||||
0.0,-0.5390625,0.0,0.0,2124
|
||||
0.0,-0.5390625,0.0,0.0,2141
|
||||
0.0,-0.5390625,0.0,0.0,2161
|
||||
0.0,-0.5390625,0.0,0.0,2181
|
||||
0.0,-0.5390625,0.0,0.0,2201
|
||||
0.0,-0.5390625,0.0,0.0,2221
|
||||
0.0,-0.5390625,0.0,0.0,2241
|
||||
0.0,-0.5390625,0.0,0.0,2262
|
||||
0.0,-0.5390625,0.0,0.0,2283
|
||||
0.0,-0.2265625,0.0,0.0,2377
|
||||
0.0,-0.2265625,0.0,0.0,2390
|
||||
0.0,0.0,0.0,0.0,2402
|
||||
0.0,0.0,0.0,0.0,2417
|
||||
0.0,0.0,0.0,0.0,2428
|
||||
0.0,0.0,0.0,0.0,2440
|
||||
0.0,0.0,0.0,0.0,2451
|
||||
0.0,0.0,0.0,0.0,2463
|
||||
0.0,0.0,0.0,0.0,2475
|
||||
0.0,0.0,0.0,0.0,2493
|
||||
0.0,0.0,0.0,0.0,2505
|
||||
0.0,0.0,0.0,0.0,2521
|
||||
0.0,0.0,0.0,0.0,2558
|
||||
0.0,0.0,0.0,0.0,2576
|
||||
0.0,0.0,0.0,0.0,2592
|
||||
0.0,0.0,0.0,0.0,2603
|
||||
0.0,0.0,0.0,0.0,2621
|
||||
0.0,0.0,0.0,0.0,2641
|
||||
0.0,0.0,0.0,0.0,2661
|
||||
0.0,0.0,0.0,0.0,2681
|
||||
0.0,0.0,0.0,0.0,2702
|
||||
0.0,0.0,0.0,0.0,2721
|
||||
0.0,0.0,0.0,0.0,2742
|
||||
0.0,0.0,0.0,0.0,2762
|
||||
0.0,0.0,0.0,0.0,2805
|
||||
0.0,0.0,0.0,0.0,2814
|
||||
0.0,0.0,0.0,0.0,2825
|
||||
0.0,0.0,0.0,0.0,2841
|
||||
0.0,0.0,0.0,0.0,2861
|
||||
0.0,0.0,0.0,0.0,2882
|
||||
0.0,0.0,0.0,0.0,2901
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,2
|
||||
0.0,0.0,0.0,0.0,18
|
||||
0.0,0.0,0.0,0.0,31
|
||||
0.0,0.0,0.0,0.0,61
|
||||
0.0,0.0,0.0,0.0,72
|
||||
0.0,0.0,0.0,0.0,85
|
||||
0.0,0.0,0.0,0.0,102
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,163
|
||||
0.0,0.0,0.0,0.0,175
|
||||
0.0,0.0,0.0,0.0,188
|
||||
0.0,0.0,0.0,0.0,205
|
||||
0.0,0.0,0.0,0.0,217
|
||||
0.0,0.0,0.0,0.0,229
|
||||
0.0,0.0,0.0,0.0,241
|
||||
0.0,0.0,0.0,0.0,261
|
||||
0.0,0.0,0.0,0.0,281
|
||||
0.0,0.0,0.0,0.0,302
|
||||
0.0,0.0,0.0,0.0,322
|
||||
0.0,0.0,0.0,0.0,341
|
||||
0.0,0.0,0.0,0.0,361
|
||||
0.0,-0.1328125,0.0,0.0,397
|
||||
0.0,-0.1328125,0.0,0.0,412
|
||||
0.0,-0.1484375,0.0,0.0,425
|
||||
0.0,-0.1796875,0.0,0.0,441
|
||||
0.0,-0.1875,0.0,0.0,461
|
||||
0.0,-0.1953125,0.0,0.0,481
|
||||
0.0,-0.234375,0.0,0.0,502
|
||||
0.0,-0.2890625,0.0,0.0,521
|
||||
0.0,-0.3125,0.0,0.0,541
|
||||
0.0,-0.328125,0.0,0.0,561
|
||||
0.0,-0.3671875,0.0,0.0,582
|
||||
0.0,-0.390625,0.0,0.0,601
|
||||
0.0,-0.4609375,0.0,0.0,642
|
||||
0.0,-0.4765625,0.0,0.0,653
|
||||
0.0,-0.4765625,0.0,0.0,666
|
||||
0.0,-0.4765625,0.0,0.0,681
|
||||
0.0,-0.4765625,0.0,0.0,702
|
||||
0.0,-0.4765625,0.0,0.0,721
|
||||
0.0,-0.4765625,0.0,0.0,741
|
||||
0.0,-0.4765625,0.0,0.0,762
|
||||
0.0,-0.4765625,0.0,0.0,782
|
||||
0.0,-0.484375,0.0,0.0,803
|
||||
0.0,-0.484375,0.0,0.0,821
|
||||
0.0,-0.4921875,0.0,0.0,842
|
||||
0.0,-0.5078125,0.0,0.0,878
|
||||
0.0,-0.5234375,0.0,0.0,893
|
||||
0.0,-0.5234375,0.0,0.0,906
|
||||
0.0,-0.53125,0.0,0.0,922
|
||||
0.0,-0.53125,0.0,0.0,942
|
||||
0.0,-0.53125,0.0,0.0,962
|
||||
0.0,-0.53125,0.0,0.0,982
|
||||
0.0,-0.5390625,0.0,0.0,1001
|
||||
0.0,-0.5390625,0.0,0.0,1022
|
||||
0.0,-0.546875,0.0,0.0,1042
|
||||
0.0,-0.546875,0.0,0.0,1061
|
||||
0.0,-0.546875,0.0,0.0,1082
|
||||
0.0,-0.546875,0.0,0.0,1164
|
||||
0.0,-0.546875,0.0,0.0,1176
|
||||
0.0,-0.546875,0.0,0.0,1190
|
||||
0.0,-0.546875,0.0,0.0,1202
|
||||
0.0,-0.546875,0.0,0.0,1219
|
||||
0.0,-0.546875,0.0,0.0,1230
|
||||
0.0,-0.546875,0.0,0.0,1244
|
||||
0.0,-0.546875,0.0,0.0,1258
|
||||
0.0,-0.546875,0.0,0.0,1268
|
||||
0.0,-0.546875,0.0,0.0,1281
|
||||
0.0,-0.546875,0.0,0.0,1301
|
||||
0.0,-0.546875,0.0,0.0,1321
|
||||
0.0,-0.546875,0.0,0.0,1363
|
||||
0.0,-0.546875,0.0,0.0,1376
|
||||
0.0,-0.546875,0.0,0.0,1388
|
||||
0.0,-0.546875,0.0,0.0,1402
|
||||
0.0,-0.546875,0.0,0.0,1422
|
||||
0.0,-0.546875,0.0,0.0,1441
|
||||
0.0,-0.546875,0.0,0.0,1461
|
||||
0.0,-0.546875,0.0,0.0,1482
|
||||
0.0,-0.546875,0.0,0.0,1502
|
||||
0.0,-0.546875,0.0,0.0,1521
|
||||
0.0,-0.546875,0.0,0.0,1542
|
||||
0.0,-0.546875,0.0,0.0,1562
|
||||
0.0,-0.546875,0.0,0.0,1598
|
||||
0.0,-0.546875,0.0,0.0,1609
|
||||
0.0,-0.546875,0.0,0.0,1621
|
||||
0.0,-0.546875,0.0,0.0,1642
|
||||
0.0,-0.546875,0.0,0.0,1661
|
||||
0.0,-0.546875,0.0,0.0,1682
|
||||
0.0,-0.546875,0.0,0.0,1702
|
||||
0.0,-0.546875,0.0,0.0,1722
|
||||
0.0,-0.546875,0.0,0.0,1742
|
||||
0.0,-0.546875,0.0,0.0,1762
|
||||
0.0,-0.546875,0.0,0.0,1781
|
||||
0.0,-0.5390625,0.0,0.0,1801
|
||||
0.0,-0.5390625,0.0,0.0,1835
|
||||
0.0,-0.5390625,0.0,0.0,1847
|
||||
0.0,-0.5390625,0.0,0.0,1861
|
||||
0.0,-0.5390625,0.0,0.0,1882
|
||||
0.0,-0.5390625,0.0,0.0,1901
|
||||
0.0,-0.5390625,0.0,0.0,1921
|
||||
0.0,-0.5390625,0.0,0.0,1941
|
||||
0.0,-0.5390625,0.0,0.0,1962
|
||||
0.0,-0.5390625,0.0,0.0,1983
|
||||
0.0,-0.5390625,0.0,0.0,2001
|
||||
0.0,-0.5390625,0.0,0.0,2022
|
||||
0.0,-0.5390625,0.0,0.0,2042
|
||||
0.0,-0.5390625,0.0,0.0,2085
|
||||
0.0,-0.5390625,0.0,0.0,2097
|
||||
0.0,-0.5390625,0.0,0.0,2113
|
||||
0.0,-0.5390625,0.0,0.0,2124
|
||||
0.0,-0.5390625,0.0,0.0,2141
|
||||
0.0,-0.5390625,0.0,0.0,2161
|
||||
0.0,-0.5390625,0.0,0.0,2181
|
||||
0.0,-0.5390625,0.0,0.0,2201
|
||||
0.0,-0.5390625,0.0,0.0,2221
|
||||
0.0,-0.5390625,0.0,0.0,2241
|
||||
0.0,-0.5390625,0.0,0.0,2262
|
||||
0.0,-0.5390625,0.0,0.0,2283
|
||||
0.0,-0.2265625,0.0,0.0,2377
|
||||
0.0,-0.2265625,0.0,0.0,2390
|
||||
0.0,0.0,0.0,0.0,2402
|
||||
0.0,0.0,0.0,0.0,2417
|
||||
0.0,0.0,0.0,0.0,2428
|
||||
0.0,0.0,0.0,0.0,2440
|
||||
0.0,0.0,0.0,0.0,2451
|
||||
0.0,0.0,0.0,0.0,2463
|
||||
0.0,0.0,0.0,0.0,2475
|
||||
0.0,0.0,0.0,0.0,2493
|
||||
0.0,0.0,0.0,0.0,2505
|
||||
0.0,0.0,0.0,0.0,2521
|
||||
0.0,0.0,0.0,0.0,2558
|
||||
0.0,0.0,0.0,0.0,2576
|
||||
0.0,0.0,0.0,0.0,2592
|
||||
0.0,0.0,0.0,0.0,2603
|
||||
0.0,0.0,0.0,0.0,2621
|
||||
0.0,0.0,0.0,0.0,2641
|
||||
0.0,0.0,0.0,0.0,2661
|
||||
0.0,0.0,0.0,0.0,2681
|
||||
0.0,0.0,0.0,0.0,2702
|
||||
0.0,0.0,0.0,0.0,2721
|
||||
0.0,0.0,0.0,0.0,2742
|
||||
0.0,0.0,0.0,0.0,2762
|
||||
0.0,0.0,0.0,0.0,2805
|
||||
0.0,0.0,0.0,0.0,2814
|
||||
0.0,0.0,0.0,0.0,2825
|
||||
0.0,0.0,0.0,0.0,2841
|
||||
0.0,0.0,0.0,0.0,2861
|
||||
0.0,0.0,0.0,0.0,2882
|
||||
0.0,0.0,0.0,0.0,2901
|
||||
0.0,0.0,0.0,0.0,2922
|
||||
@@ -1,91 +1,91 @@
|
||||
package frc4388.robot.commands.Autos;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import frc4388.robot.commands.Swerve.JoystickPlayback;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class PlaybackChooser {
|
||||
private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
|
||||
private SendableChooser<Command> m_playback = null;
|
||||
private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
|
||||
private final HashMap<String, Command> m_commandPool = new HashMap<>();
|
||||
|
||||
private final File m_dir = new File("/home/lvuser/autos/");
|
||||
private int m_cmdNum = 0;
|
||||
private final SwerveDrive m_swerve;
|
||||
|
||||
// commands
|
||||
private Command m_noAuto = new InstantCommand();
|
||||
|
||||
public PlaybackChooser(SwerveDrive swerve, Object... pool) {
|
||||
m_swerve = swerve;
|
||||
}
|
||||
|
||||
public PlaybackChooser addOption(String name, Command option) {
|
||||
m_commandPool.put(name, option);
|
||||
return this;
|
||||
}
|
||||
|
||||
public PlaybackChooser buildDisplay() {
|
||||
for (int i = 0; i < 10; i++) {
|
||||
appendCommand();
|
||||
}
|
||||
m_playback = m_choosers.get(0);
|
||||
nextChooser();
|
||||
|
||||
Shuffleboard.getTab("Auto Chooser")
|
||||
.add("Add Sequence", new InstantCommand(() -> nextChooser()))
|
||||
.withPosition(4, 0);
|
||||
return this;
|
||||
}
|
||||
|
||||
// This will be bound to a button for the time being
|
||||
public void appendCommand() {
|
||||
var chooser = new SendableChooser<Command>();
|
||||
chooser.setDefaultOption("No Auto", m_noAuto);
|
||||
|
||||
m_choosers.add(chooser);
|
||||
ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
|
||||
.add("Command: " + m_choosers.size(), chooser)
|
||||
.withSize(4, 1)
|
||||
.withPosition(0, m_choosers.size() - 1)
|
||||
.withWidget(BuiltInWidgets.kSplitButtonChooser);
|
||||
|
||||
m_widgets.add(widget);
|
||||
}
|
||||
|
||||
public void nextChooser() {
|
||||
SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
|
||||
|
||||
for (String auto : m_dir.list()) {
|
||||
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
|
||||
}
|
||||
for (var cmd_name : m_commandPool.keySet()) {
|
||||
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
|
||||
}
|
||||
}
|
||||
|
||||
public Command getCommand() {
|
||||
Command command = m_playback.getSelected();
|
||||
command = command == null ? m_noAuto : command.asProxy();
|
||||
|
||||
Command[] commands = new Command[m_cmdNum - 1];
|
||||
for (int i = 0; i < m_cmdNum - 1; i++) {
|
||||
Command command2 = m_choosers.get(i + 1).getSelected();
|
||||
command2 = command2 == null ? m_noAuto : command2.asProxy();
|
||||
|
||||
commands[i] = command2.asProxy();
|
||||
}
|
||||
|
||||
return command.andThen(commands);
|
||||
}
|
||||
}
|
||||
package frc4388.robot.commands.Autos;
|
||||
|
||||
import java.io.File;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import frc4388.robot.commands.Swerve.JoystickPlayback;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class PlaybackChooser {
|
||||
private final ArrayList<SendableChooser<Command>> m_choosers = new ArrayList<>();
|
||||
private SendableChooser<Command> m_playback = null;
|
||||
private final ArrayList<ComplexWidget> m_widgets = new ArrayList<>();
|
||||
private final HashMap<String, Command> m_commandPool = new HashMap<>();
|
||||
|
||||
private final File m_dir = new File("/home/lvuser/autos/");
|
||||
private int m_cmdNum = 0;
|
||||
private final SwerveDrive m_swerve;
|
||||
|
||||
// commands
|
||||
private Command m_noAuto = new InstantCommand();
|
||||
|
||||
public PlaybackChooser(SwerveDrive swerve, Object... pool) {
|
||||
m_swerve = swerve;
|
||||
}
|
||||
|
||||
public PlaybackChooser addOption(String name, Command option) {
|
||||
m_commandPool.put(name, option);
|
||||
return this;
|
||||
}
|
||||
|
||||
public PlaybackChooser buildDisplay() {
|
||||
for (int i = 0; i < 10; i++) {
|
||||
appendCommand();
|
||||
}
|
||||
m_playback = m_choosers.get(0);
|
||||
nextChooser();
|
||||
|
||||
Shuffleboard.getTab("Auto Chooser")
|
||||
.add("Add Sequence", new InstantCommand(() -> nextChooser()))
|
||||
.withPosition(4, 0);
|
||||
return this;
|
||||
}
|
||||
|
||||
// This will be bound to a button for the time being
|
||||
public void appendCommand() {
|
||||
var chooser = new SendableChooser<Command>();
|
||||
chooser.setDefaultOption("No Auto", m_noAuto);
|
||||
|
||||
m_choosers.add(chooser);
|
||||
ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
|
||||
.add("Command: " + m_choosers.size(), chooser)
|
||||
.withSize(4, 1)
|
||||
.withPosition(0, m_choosers.size() - 1)
|
||||
.withWidget(BuiltInWidgets.kSplitButtonChooser);
|
||||
|
||||
m_widgets.add(widget);
|
||||
}
|
||||
|
||||
public void nextChooser() {
|
||||
SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
|
||||
|
||||
for (String auto : m_dir.list()) {
|
||||
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
|
||||
}
|
||||
for (var cmd_name : m_commandPool.keySet()) {
|
||||
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
|
||||
}
|
||||
}
|
||||
|
||||
public Command getCommand() {
|
||||
Command command = m_playback.getSelected();
|
||||
command = command == null ? m_noAuto : command.asProxy();
|
||||
|
||||
Command[] commands = new Command[m_cmdNum - 1];
|
||||
for (int i = 0; i < m_cmdNum - 1; i++) {
|
||||
Command command2 = m_choosers.get(i + 1).getSelected();
|
||||
command2 = command2 == null ? m_noAuto : command2.asProxy();
|
||||
|
||||
commands[i] = command2.asProxy();
|
||||
}
|
||||
|
||||
return command.andThen(commands);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,225 +1,225 @@
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,12
|
||||
0.0,0.0,0.0,0.0,26
|
||||
0.0,0.0,0.0,0.0,37
|
||||
0.0,0.0,0.0,0.0,50
|
||||
0.0,0.0,0.0,0.0,62
|
||||
0.0,0.0,0.0,0.0,73
|
||||
0.0,0.0,0.0,0.0,88
|
||||
0.0,0.0,0.0,0.0,103
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,160
|
||||
0.0,0.0,0.0,0.0,173
|
||||
0.0,0.0,0.0,0.0,185
|
||||
0.0,0.0,0.0,0.0,200
|
||||
0.0,0.0,0.0,0.0,211
|
||||
0.0,0.0,0.0,0.0,223
|
||||
0.0,0.0,0.0,0.0,235
|
||||
0.0,0.0,0.0,0.0,247
|
||||
0.0,0.0,0.0,0.0,263
|
||||
0.0,0.0,0.0,0.0,283
|
||||
0.0,0.0,0.0,0.0,303
|
||||
0.0,-0.109375,0.0,0.0,323
|
||||
0.0,-0.1484375,0.0,0.0,343
|
||||
0.0,-0.2109375,0.0,0.0,363
|
||||
0.0,-0.3671875,0.0,0.0,398
|
||||
0.0,-0.4140625,0.0,0.0,411
|
||||
0.0,-0.4765625,0.0,0.0,425
|
||||
0.0,-0.5078125,0.0,0.0,443
|
||||
0.0,-0.5078125,0.0,0.0,463
|
||||
0.0,-0.53125,0.0,0.0,483
|
||||
0.0,-0.5546875,0.0,0.0,504
|
||||
0.0,-0.5625,0.0,0.0,523
|
||||
0.0,-0.5625,0.0,0.0,544
|
||||
0.0,-0.5703125,0.0,0.0,563
|
||||
0.0,-0.5859375,0.0,0.0,584
|
||||
0.0,-0.5859375,0.0,0.0,603
|
||||
0.0,-0.5859375,0.0,0.0,640
|
||||
0.0,-0.59375,0.0,0.0,657
|
||||
0.0,-0.6015625,0.0,0.0,672
|
||||
0.0,-0.6015625,0.0,0.0,685
|
||||
0.0,-0.6015625,0.0,0.0,703
|
||||
0.0,-0.6015625,0.0,0.0,723
|
||||
0.0,-0.6015625,0.0,0.0,743
|
||||
0.0,-0.6015625,0.0,0.0,763
|
||||
0.0,-0.6015625,0.0,0.0,783
|
||||
0.0,-0.6015625,0.0,0.0,803
|
||||
0.0,-0.6015625,0.0,0.0,823
|
||||
0.0,-0.6015625,0.0,0.0,844
|
||||
0.0,-0.6015625,0.0,0.0,878
|
||||
0.0,-0.6015625,0.0,0.0,893
|
||||
0.0,-0.6015625,0.0,0.0,907
|
||||
0.0,-0.6015625,0.0,0.0,924
|
||||
0.0,-0.609375,0.0,0.0,943
|
||||
0.0,-0.609375,0.0,0.0,963
|
||||
0.0,-0.609375,0.0,0.0,983
|
||||
0.0,-0.609375,0.0,0.0,1004
|
||||
0.0,-0.609375,0.0,0.0,1023
|
||||
0.0,-0.609375,0.0,0.0,1043
|
||||
0.0,-0.609375,0.0,0.0,1064
|
||||
0.0,-0.609375,0.0,0.0,1083
|
||||
0.0,-0.609375,0.0,0.0,1156
|
||||
0.0,-0.609375,0.0,0.0,1172
|
||||
0.0,-0.609375,0.0,0.0,1185
|
||||
0.0,-0.609375,0.0,0.0,1200
|
||||
0.0,-0.609375,0.0,0.0,1215
|
||||
0.0,-0.609375,0.0,0.0,1225
|
||||
0.0,-0.609375,0.0,0.0,1236
|
||||
0.0,-0.609375,0.0,0.0,1249
|
||||
0.0,-0.609375,0.0,0.0,1263
|
||||
0.0,-0.609375,0.0,0.0,1283
|
||||
0.0,-0.609375,0.0,0.0,1303
|
||||
0.0,-0.609375,0.0,0.0,1323
|
||||
0.0,-0.609375,0.0,0.0,1363
|
||||
0.0,-0.6015625,0.0,0.0,1376
|
||||
0.0,-0.6015625,0.0,0.0,1394
|
||||
0.0,-0.6015625,0.0,0.0,1405
|
||||
0.0,-0.6015625,0.0,0.0,1423
|
||||
0.0,-0.6015625,0.0,0.0,1443
|
||||
0.0,-0.6015625,0.0,0.0,1463
|
||||
0.0,-0.6015625,0.0,0.0,1483
|
||||
0.0,-0.6015625,0.0,0.0,1503
|
||||
0.0,-0.6015625,0.0,0.0,1523
|
||||
0.0,-0.6015625,0.0,0.0,1543
|
||||
0.0,-0.6015625,0.0,0.0,1563
|
||||
0.0,-0.6015625,0.0,0.0,1597
|
||||
0.0,-0.6015625,0.0,0.0,1608
|
||||
0.0,-0.6015625,0.0,0.0,1624
|
||||
0.0,-0.6015625,0.0,0.0,1643
|
||||
0.0,-0.6015625,0.0,0.0,1664
|
||||
0.0,-0.5859375,0.0,0.0,1683
|
||||
0.0,-0.5859375,0.0,0.0,1703
|
||||
0.0,-0.5625,0.0,0.0,1723
|
||||
0.0,-0.5625,0.0,0.0,1743
|
||||
0.0,-0.5625,0.0,0.0,1763
|
||||
0.0,-0.5625,0.0,0.0,1783
|
||||
0.0,-0.5625,0.0,0.0,1803
|
||||
0.0,-0.5625,0.0,0.0,1843
|
||||
0.0,-0.5625,0.0,0.0,1855
|
||||
0.0,-0.5625,0.0,0.0,1868
|
||||
0.0,-0.5625,0.0,0.0,1883
|
||||
0.0,-0.5625,0.0,0.0,1903
|
||||
0.0,-0.5625,0.0,0.0,1923
|
||||
0.0,-0.5625,0.0,0.0,1943
|
||||
0.0,-0.5625,0.0,0.0,1963
|
||||
0.0,-0.5625,0.0,0.0,1983
|
||||
0.0,-0.5625,0.0,0.0,2003
|
||||
0.0,-0.5625,0.0,0.0,2024
|
||||
0.0,-0.5625,0.0,0.0,2043
|
||||
0.0,-0.5625,0.0,0.0,2081
|
||||
0.0,-0.5625,0.0,0.0,2093
|
||||
0.0,-0.5625,0.0,0.0,2105
|
||||
0.0,-0.5625,0.0,0.0,2123
|
||||
0.0,-0.5625,0.0,0.0,2143
|
||||
0.0,-0.5625,0.0,0.0,2163
|
||||
0.0,-0.5625,0.0,0.0,2183
|
||||
0.0,-0.5625,0.0,0.0,2203
|
||||
0.0,-0.5625,0.0,0.0,2223
|
||||
0.0,-0.5625,0.0,0.0,2243
|
||||
0.0,-0.5625,0.0,0.0,2263
|
||||
0.0,-0.5625,0.0,0.0,2283
|
||||
0.0,-0.5625,0.0,0.0,2366
|
||||
0.0,-0.5625,0.0,0.0,2377
|
||||
0.0,-0.5625,0.0,0.0,2394
|
||||
0.0,-0.5703125,0.0,0.0,2405
|
||||
0.0,-0.5703125,0.0,0.0,2418
|
||||
0.0,-0.5703125,0.0,0.0,2431
|
||||
0.0,-0.5703125,0.0,0.0,2444
|
||||
0.0,-0.5703125,0.0,0.0,2458
|
||||
0.0,-0.5703125,0.0,0.0,2470
|
||||
0.0,-0.5703125,0.0,0.0,2485
|
||||
0.0,-0.5703125,0.0,0.0,2503
|
||||
0.0,-0.5703125,0.0,0.0,2523
|
||||
0.0,-0.5703125,0.0,0.0,2563
|
||||
0.0,-0.5703125,0.0,0.0,2577
|
||||
0.0,-0.5703125,0.0,0.0,2591
|
||||
0.0,-0.5703125,0.0,0.0,2608
|
||||
0.0,-0.5703125,0.0,0.0,2624
|
||||
0.0,-0.5703125,0.0,0.0,2643
|
||||
0.0,-0.5703125,0.0,0.0,2677
|
||||
0.0,-0.5703125,0.0,0.0,2698
|
||||
0.0,-0.5703125,0.0,0.0,2711
|
||||
0.0,-0.5703125,0.0,0.0,2725
|
||||
0.0,-0.5703125,0.0,0.0,2743
|
||||
0.0,-0.5703125,0.0,0.0,2764
|
||||
0.0,-0.5703125,0.0,0.0,2810
|
||||
0.0,-0.5703125,0.0,0.0,2820
|
||||
0.0,-0.5703125,0.0,0.0,2833
|
||||
0.0,-0.5703125,0.0,0.0,2845
|
||||
0.0,-0.5703125,0.0,0.0,2863
|
||||
0.0,-0.5703125,0.0,0.0,2883
|
||||
0.0,-0.5703125,0.0,0.0,2904
|
||||
0.0,-0.5703125,0.0,0.0,2924
|
||||
0.0,-0.5703125,0.0,0.0,2943
|
||||
0.0,-0.5703125,0.0,0.0,2963
|
||||
0.0,-0.5703125,0.0,0.0,2983
|
||||
0.0,-0.5703125,0.0,0.0,3003
|
||||
0.0,-0.5703125,0.0,0.0,3033
|
||||
0.0,-0.5703125,0.0,0.0,3050
|
||||
0.0,-0.5703125,0.0,0.0,3065
|
||||
0.0,-0.5703125,0.0,0.0,3083
|
||||
0.0,-0.5703125,0.0,0.0,3103
|
||||
0.0,-0.5703125,0.0,0.0,3123
|
||||
0.0,-0.5703125,0.0,0.0,3144
|
||||
0.0,-0.5703125,0.0,0.0,3164
|
||||
0.0,-0.5703125,0.0,0.0,3184
|
||||
0.0,-0.5703125,0.0,0.0,3203
|
||||
0.0,-0.5703125,0.0,0.0,3223
|
||||
0.0,-0.5703125,0.0,0.0,3243
|
||||
0.0,-0.5703125,0.0,0.0,3272
|
||||
0.0,-0.5703125,0.0,0.0,3289
|
||||
0.0,-0.5703125,0.0,0.0,3303
|
||||
0.0,-0.5703125,0.0,0.0,3323
|
||||
0.0,-0.5703125,0.0,0.0,3343
|
||||
0.0,-0.5703125,0.0,0.0,3363
|
||||
0.0,-0.5703125,0.0,0.0,3383
|
||||
0.0,-0.5703125,0.0,0.0,3403
|
||||
0.0,-0.5703125,0.0,0.0,3423
|
||||
0.0,-0.5703125,0.0,0.0,3443
|
||||
0.0,-0.5703125,0.0,0.0,3463
|
||||
0.0,-0.5703125,0.0,0.0,3483
|
||||
0.0,-0.5703125,0.0,0.0,3566
|
||||
0.0,-0.5703125,0.0,0.0,3578
|
||||
0.0,-0.5703125,0.0,0.0,3596
|
||||
0.0,-0.5703125,0.0,0.0,3610
|
||||
0.0,-0.5703125,0.0,0.0,3623
|
||||
0.0,-0.5703125,0.0,0.0,3640
|
||||
0.0,-0.5703125,0.0,0.0,3651
|
||||
0.0,-0.5703125,0.0,0.0,3663
|
||||
0.0,-0.5703125,0.0,0.0,3678
|
||||
0.0,-0.5703125,0.0,0.0,3691
|
||||
0.0,-0.5703125,0.0,0.0,3706
|
||||
0.0,-0.5703125,0.0,0.0,3723
|
||||
0.0,-0.5703125,0.0,0.0,3766
|
||||
0.0,-0.5703125,0.0,0.0,3778
|
||||
0.0,-0.5703125,0.0,0.0,3792
|
||||
0.0,-0.5703125,0.0,0.0,3807
|
||||
0.0,-0.5703125,0.0,0.0,3823
|
||||
0.0,-0.5703125,0.0,0.0,3843
|
||||
0.0,-0.53125,0.0,0.0,3863
|
||||
0.0,-0.53125,0.0,0.0,3884
|
||||
0.0,-0.421875,0.0,0.0,3904
|
||||
0.0,0.0,0.0,0.0,3924
|
||||
0.0,0.0,0.0,0.0,3944
|
||||
0.0,0.0,0.0,0.0,3963
|
||||
0.0,0.0,0.0,0.0,3999
|
||||
0.0,0.0,0.0,0.0,4010
|
||||
0.0,0.0,0.0,0.0,4025
|
||||
0.0,0.0,0.0,0.0,4043
|
||||
0.0,0.0,0.0,0.0,4063
|
||||
0.0,0.0,0.0,0.0,4083
|
||||
0.0,0.0,0.0,0.0,4103
|
||||
0.0,0.0,0.0,0.0,4123
|
||||
0.0,0.0,0.0,0.0,4143
|
||||
0.0,0.0,0.0,0.0,4163
|
||||
0.0,0.0,0.0,0.0,4183
|
||||
0.0,0.0,0.0,0.0,4203
|
||||
0.0,0.0,0.0,0.0,4236
|
||||
0.0,0.0,0.0,0.0,4247
|
||||
0.0,0.0,0.0,0.0,4264
|
||||
0.0,0.0,0.0,0.0,4284
|
||||
0.0,0.0,0.0,0.0,4304
|
||||
0.0,0.0,0.0,0.0,4324
|
||||
0.0,0.0,0.0,0.0,4343
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,0
|
||||
0.0,0.0,0.0,0.0,12
|
||||
0.0,0.0,0.0,0.0,26
|
||||
0.0,0.0,0.0,0.0,37
|
||||
0.0,0.0,0.0,0.0,50
|
||||
0.0,0.0,0.0,0.0,62
|
||||
0.0,0.0,0.0,0.0,73
|
||||
0.0,0.0,0.0,0.0,88
|
||||
0.0,0.0,0.0,0.0,103
|
||||
0.0,0.0,0.0,0.0,116
|
||||
0.0,0.0,0.0,0.0,160
|
||||
0.0,0.0,0.0,0.0,173
|
||||
0.0,0.0,0.0,0.0,185
|
||||
0.0,0.0,0.0,0.0,200
|
||||
0.0,0.0,0.0,0.0,211
|
||||
0.0,0.0,0.0,0.0,223
|
||||
0.0,0.0,0.0,0.0,235
|
||||
0.0,0.0,0.0,0.0,247
|
||||
0.0,0.0,0.0,0.0,263
|
||||
0.0,0.0,0.0,0.0,283
|
||||
0.0,0.0,0.0,0.0,303
|
||||
0.0,-0.109375,0.0,0.0,323
|
||||
0.0,-0.1484375,0.0,0.0,343
|
||||
0.0,-0.2109375,0.0,0.0,363
|
||||
0.0,-0.3671875,0.0,0.0,398
|
||||
0.0,-0.4140625,0.0,0.0,411
|
||||
0.0,-0.4765625,0.0,0.0,425
|
||||
0.0,-0.5078125,0.0,0.0,443
|
||||
0.0,-0.5078125,0.0,0.0,463
|
||||
0.0,-0.53125,0.0,0.0,483
|
||||
0.0,-0.5546875,0.0,0.0,504
|
||||
0.0,-0.5625,0.0,0.0,523
|
||||
0.0,-0.5625,0.0,0.0,544
|
||||
0.0,-0.5703125,0.0,0.0,563
|
||||
0.0,-0.5859375,0.0,0.0,584
|
||||
0.0,-0.5859375,0.0,0.0,603
|
||||
0.0,-0.5859375,0.0,0.0,640
|
||||
0.0,-0.59375,0.0,0.0,657
|
||||
0.0,-0.6015625,0.0,0.0,672
|
||||
0.0,-0.6015625,0.0,0.0,685
|
||||
0.0,-0.6015625,0.0,0.0,703
|
||||
0.0,-0.6015625,0.0,0.0,723
|
||||
0.0,-0.6015625,0.0,0.0,743
|
||||
0.0,-0.6015625,0.0,0.0,763
|
||||
0.0,-0.6015625,0.0,0.0,783
|
||||
0.0,-0.6015625,0.0,0.0,803
|
||||
0.0,-0.6015625,0.0,0.0,823
|
||||
0.0,-0.6015625,0.0,0.0,844
|
||||
0.0,-0.6015625,0.0,0.0,878
|
||||
0.0,-0.6015625,0.0,0.0,893
|
||||
0.0,-0.6015625,0.0,0.0,907
|
||||
0.0,-0.6015625,0.0,0.0,924
|
||||
0.0,-0.609375,0.0,0.0,943
|
||||
0.0,-0.609375,0.0,0.0,963
|
||||
0.0,-0.609375,0.0,0.0,983
|
||||
0.0,-0.609375,0.0,0.0,1004
|
||||
0.0,-0.609375,0.0,0.0,1023
|
||||
0.0,-0.609375,0.0,0.0,1043
|
||||
0.0,-0.609375,0.0,0.0,1064
|
||||
0.0,-0.609375,0.0,0.0,1083
|
||||
0.0,-0.609375,0.0,0.0,1156
|
||||
0.0,-0.609375,0.0,0.0,1172
|
||||
0.0,-0.609375,0.0,0.0,1185
|
||||
0.0,-0.609375,0.0,0.0,1200
|
||||
0.0,-0.609375,0.0,0.0,1215
|
||||
0.0,-0.609375,0.0,0.0,1225
|
||||
0.0,-0.609375,0.0,0.0,1236
|
||||
0.0,-0.609375,0.0,0.0,1249
|
||||
0.0,-0.609375,0.0,0.0,1263
|
||||
0.0,-0.609375,0.0,0.0,1283
|
||||
0.0,-0.609375,0.0,0.0,1303
|
||||
0.0,-0.609375,0.0,0.0,1323
|
||||
0.0,-0.609375,0.0,0.0,1363
|
||||
0.0,-0.6015625,0.0,0.0,1376
|
||||
0.0,-0.6015625,0.0,0.0,1394
|
||||
0.0,-0.6015625,0.0,0.0,1405
|
||||
0.0,-0.6015625,0.0,0.0,1423
|
||||
0.0,-0.6015625,0.0,0.0,1443
|
||||
0.0,-0.6015625,0.0,0.0,1463
|
||||
0.0,-0.6015625,0.0,0.0,1483
|
||||
0.0,-0.6015625,0.0,0.0,1503
|
||||
0.0,-0.6015625,0.0,0.0,1523
|
||||
0.0,-0.6015625,0.0,0.0,1543
|
||||
0.0,-0.6015625,0.0,0.0,1563
|
||||
0.0,-0.6015625,0.0,0.0,1597
|
||||
0.0,-0.6015625,0.0,0.0,1608
|
||||
0.0,-0.6015625,0.0,0.0,1624
|
||||
0.0,-0.6015625,0.0,0.0,1643
|
||||
0.0,-0.6015625,0.0,0.0,1664
|
||||
0.0,-0.5859375,0.0,0.0,1683
|
||||
0.0,-0.5859375,0.0,0.0,1703
|
||||
0.0,-0.5625,0.0,0.0,1723
|
||||
0.0,-0.5625,0.0,0.0,1743
|
||||
0.0,-0.5625,0.0,0.0,1763
|
||||
0.0,-0.5625,0.0,0.0,1783
|
||||
0.0,-0.5625,0.0,0.0,1803
|
||||
0.0,-0.5625,0.0,0.0,1843
|
||||
0.0,-0.5625,0.0,0.0,1855
|
||||
0.0,-0.5625,0.0,0.0,1868
|
||||
0.0,-0.5625,0.0,0.0,1883
|
||||
0.0,-0.5625,0.0,0.0,1903
|
||||
0.0,-0.5625,0.0,0.0,1923
|
||||
0.0,-0.5625,0.0,0.0,1943
|
||||
0.0,-0.5625,0.0,0.0,1963
|
||||
0.0,-0.5625,0.0,0.0,1983
|
||||
0.0,-0.5625,0.0,0.0,2003
|
||||
0.0,-0.5625,0.0,0.0,2024
|
||||
0.0,-0.5625,0.0,0.0,2043
|
||||
0.0,-0.5625,0.0,0.0,2081
|
||||
0.0,-0.5625,0.0,0.0,2093
|
||||
0.0,-0.5625,0.0,0.0,2105
|
||||
0.0,-0.5625,0.0,0.0,2123
|
||||
0.0,-0.5625,0.0,0.0,2143
|
||||
0.0,-0.5625,0.0,0.0,2163
|
||||
0.0,-0.5625,0.0,0.0,2183
|
||||
0.0,-0.5625,0.0,0.0,2203
|
||||
0.0,-0.5625,0.0,0.0,2223
|
||||
0.0,-0.5625,0.0,0.0,2243
|
||||
0.0,-0.5625,0.0,0.0,2263
|
||||
0.0,-0.5625,0.0,0.0,2283
|
||||
0.0,-0.5625,0.0,0.0,2366
|
||||
0.0,-0.5625,0.0,0.0,2377
|
||||
0.0,-0.5625,0.0,0.0,2394
|
||||
0.0,-0.5703125,0.0,0.0,2405
|
||||
0.0,-0.5703125,0.0,0.0,2418
|
||||
0.0,-0.5703125,0.0,0.0,2431
|
||||
0.0,-0.5703125,0.0,0.0,2444
|
||||
0.0,-0.5703125,0.0,0.0,2458
|
||||
0.0,-0.5703125,0.0,0.0,2470
|
||||
0.0,-0.5703125,0.0,0.0,2485
|
||||
0.0,-0.5703125,0.0,0.0,2503
|
||||
0.0,-0.5703125,0.0,0.0,2523
|
||||
0.0,-0.5703125,0.0,0.0,2563
|
||||
0.0,-0.5703125,0.0,0.0,2577
|
||||
0.0,-0.5703125,0.0,0.0,2591
|
||||
0.0,-0.5703125,0.0,0.0,2608
|
||||
0.0,-0.5703125,0.0,0.0,2624
|
||||
0.0,-0.5703125,0.0,0.0,2643
|
||||
0.0,-0.5703125,0.0,0.0,2677
|
||||
0.0,-0.5703125,0.0,0.0,2698
|
||||
0.0,-0.5703125,0.0,0.0,2711
|
||||
0.0,-0.5703125,0.0,0.0,2725
|
||||
0.0,-0.5703125,0.0,0.0,2743
|
||||
0.0,-0.5703125,0.0,0.0,2764
|
||||
0.0,-0.5703125,0.0,0.0,2810
|
||||
0.0,-0.5703125,0.0,0.0,2820
|
||||
0.0,-0.5703125,0.0,0.0,2833
|
||||
0.0,-0.5703125,0.0,0.0,2845
|
||||
0.0,-0.5703125,0.0,0.0,2863
|
||||
0.0,-0.5703125,0.0,0.0,2883
|
||||
0.0,-0.5703125,0.0,0.0,2904
|
||||
0.0,-0.5703125,0.0,0.0,2924
|
||||
0.0,-0.5703125,0.0,0.0,2943
|
||||
0.0,-0.5703125,0.0,0.0,2963
|
||||
0.0,-0.5703125,0.0,0.0,2983
|
||||
0.0,-0.5703125,0.0,0.0,3003
|
||||
0.0,-0.5703125,0.0,0.0,3033
|
||||
0.0,-0.5703125,0.0,0.0,3050
|
||||
0.0,-0.5703125,0.0,0.0,3065
|
||||
0.0,-0.5703125,0.0,0.0,3083
|
||||
0.0,-0.5703125,0.0,0.0,3103
|
||||
0.0,-0.5703125,0.0,0.0,3123
|
||||
0.0,-0.5703125,0.0,0.0,3144
|
||||
0.0,-0.5703125,0.0,0.0,3164
|
||||
0.0,-0.5703125,0.0,0.0,3184
|
||||
0.0,-0.5703125,0.0,0.0,3203
|
||||
0.0,-0.5703125,0.0,0.0,3223
|
||||
0.0,-0.5703125,0.0,0.0,3243
|
||||
0.0,-0.5703125,0.0,0.0,3272
|
||||
0.0,-0.5703125,0.0,0.0,3289
|
||||
0.0,-0.5703125,0.0,0.0,3303
|
||||
0.0,-0.5703125,0.0,0.0,3323
|
||||
0.0,-0.5703125,0.0,0.0,3343
|
||||
0.0,-0.5703125,0.0,0.0,3363
|
||||
0.0,-0.5703125,0.0,0.0,3383
|
||||
0.0,-0.5703125,0.0,0.0,3403
|
||||
0.0,-0.5703125,0.0,0.0,3423
|
||||
0.0,-0.5703125,0.0,0.0,3443
|
||||
0.0,-0.5703125,0.0,0.0,3463
|
||||
0.0,-0.5703125,0.0,0.0,3483
|
||||
0.0,-0.5703125,0.0,0.0,3566
|
||||
0.0,-0.5703125,0.0,0.0,3578
|
||||
0.0,-0.5703125,0.0,0.0,3596
|
||||
0.0,-0.5703125,0.0,0.0,3610
|
||||
0.0,-0.5703125,0.0,0.0,3623
|
||||
0.0,-0.5703125,0.0,0.0,3640
|
||||
0.0,-0.5703125,0.0,0.0,3651
|
||||
0.0,-0.5703125,0.0,0.0,3663
|
||||
0.0,-0.5703125,0.0,0.0,3678
|
||||
0.0,-0.5703125,0.0,0.0,3691
|
||||
0.0,-0.5703125,0.0,0.0,3706
|
||||
0.0,-0.5703125,0.0,0.0,3723
|
||||
0.0,-0.5703125,0.0,0.0,3766
|
||||
0.0,-0.5703125,0.0,0.0,3778
|
||||
0.0,-0.5703125,0.0,0.0,3792
|
||||
0.0,-0.5703125,0.0,0.0,3807
|
||||
0.0,-0.5703125,0.0,0.0,3823
|
||||
0.0,-0.5703125,0.0,0.0,3843
|
||||
0.0,-0.53125,0.0,0.0,3863
|
||||
0.0,-0.53125,0.0,0.0,3884
|
||||
0.0,-0.421875,0.0,0.0,3904
|
||||
0.0,0.0,0.0,0.0,3924
|
||||
0.0,0.0,0.0,0.0,3944
|
||||
0.0,0.0,0.0,0.0,3963
|
||||
0.0,0.0,0.0,0.0,3999
|
||||
0.0,0.0,0.0,0.0,4010
|
||||
0.0,0.0,0.0,0.0,4025
|
||||
0.0,0.0,0.0,0.0,4043
|
||||
0.0,0.0,0.0,0.0,4063
|
||||
0.0,0.0,0.0,0.0,4083
|
||||
0.0,0.0,0.0,0.0,4103
|
||||
0.0,0.0,0.0,0.0,4123
|
||||
0.0,0.0,0.0,0.0,4143
|
||||
0.0,0.0,0.0,0.0,4163
|
||||
0.0,0.0,0.0,0.0,4183
|
||||
0.0,0.0,0.0,0.0,4203
|
||||
0.0,0.0,0.0,0.0,4236
|
||||
0.0,0.0,0.0,0.0,4247
|
||||
0.0,0.0,0.0,0.0,4264
|
||||
0.0,0.0,0.0,0.0,4284
|
||||
0.0,0.0,0.0,0.0,4304
|
||||
0.0,0.0,0.0,0.0,4324
|
||||
0.0,0.0,0.0,0.0,4343
|
||||
0.0,0.0,0.0,0.0,4363
|
||||
@@ -1,67 +1,67 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
|
||||
public class BooleanCommand extends CommandBase {
|
||||
|
||||
private Supplier<Command> onTrue;
|
||||
private Supplier<Command> onFalse;
|
||||
|
||||
private Supplier<Boolean> condition;
|
||||
|
||||
private Supplier<Command> selected;
|
||||
|
||||
|
||||
/** Creates a new BooleanCommand. */
|
||||
public BooleanCommand(Supplier<Command> onTrue, Supplier<Command> onFalse, Supplier<Boolean> condition) {
|
||||
this.onTrue = onTrue;
|
||||
this.onFalse = onFalse;
|
||||
this.condition = condition;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
if (condition.get()) {
|
||||
selected = onTrue;
|
||||
} else {
|
||||
selected = onFalse;
|
||||
}
|
||||
if (selected.get() != null) {
|
||||
selected.get().initialize();
|
||||
}
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (selected.get() != null) {
|
||||
selected.get().execute();
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
if (selected.get() != null) {
|
||||
selected.get().end(interrupted);
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (selected.get() != null) {
|
||||
return selected.get().isFinished();
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
|
||||
public class BooleanCommand extends CommandBase {
|
||||
|
||||
private Supplier<Command> onTrue;
|
||||
private Supplier<Command> onFalse;
|
||||
|
||||
private Supplier<Boolean> condition;
|
||||
|
||||
private Supplier<Command> selected;
|
||||
|
||||
|
||||
/** Creates a new BooleanCommand. */
|
||||
public BooleanCommand(Supplier<Command> onTrue, Supplier<Command> onFalse, Supplier<Boolean> condition) {
|
||||
this.onTrue = onTrue;
|
||||
this.onFalse = onFalse;
|
||||
this.condition = condition;
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
if (condition.get()) {
|
||||
selected = onTrue;
|
||||
} else {
|
||||
selected = onFalse;
|
||||
}
|
||||
if (selected.get() != null) {
|
||||
selected.get().initialize();
|
||||
}
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (selected.get() != null) {
|
||||
selected.get().execute();
|
||||
}
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
if (selected.get() != null) {
|
||||
selected.get().end(interrupted);
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (selected.get() != null) {
|
||||
return selected.get().isFinished();
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,60 +1,60 @@
|
||||
// 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.utility.Gains;
|
||||
|
||||
public abstract class PelvicInflammatoryDisease extends CommandBase {
|
||||
protected Gains gains;
|
||||
private double output = 0;
|
||||
private double tolerance = 0;
|
||||
|
||||
/** Creates a new PelvicInflammatoryDisease. */
|
||||
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
|
||||
gains = new Gains(kp, ki, kd, kf, 0);
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
public PelvicInflammatoryDisease(Gains gains, double tolerance) {
|
||||
this.gains = gains;
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
/** produces the error from the setpoint */
|
||||
public abstract double getError();
|
||||
|
||||
/** figure it out bitch */
|
||||
public abstract void runWithOutput(double output);
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public final void initialize() {
|
||||
output = 0;
|
||||
}
|
||||
|
||||
private double prevError, cumError = 0;
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public final void execute() {
|
||||
double error = getError();
|
||||
cumError += error * .02; // 20 ms
|
||||
double delta = error - prevError;
|
||||
|
||||
output = error * gains.kP;
|
||||
output += cumError * gains.kI;
|
||||
output += delta * gains.kD;
|
||||
output += gains.kF;
|
||||
|
||||
runWithOutput(output);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
return Math.abs(getError()) < tolerance;
|
||||
}
|
||||
}
|
||||
// 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.utility.Gains;
|
||||
|
||||
public abstract class PelvicInflammatoryDisease extends CommandBase {
|
||||
protected Gains gains;
|
||||
private double output = 0;
|
||||
private double tolerance = 0;
|
||||
|
||||
/** Creates a new PelvicInflammatoryDisease. */
|
||||
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
|
||||
gains = new Gains(kp, ki, kd, kf, 0);
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
public PelvicInflammatoryDisease(Gains gains, double tolerance) {
|
||||
this.gains = gains;
|
||||
this.tolerance = tolerance;
|
||||
}
|
||||
|
||||
/** produces the error from the setpoint */
|
||||
public abstract double getError();
|
||||
|
||||
/** figure it out bitch */
|
||||
public abstract void runWithOutput(double output);
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public final void initialize() {
|
||||
output = 0;
|
||||
}
|
||||
|
||||
private double prevError, cumError = 0;
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public final void execute() {
|
||||
double error = getError();
|
||||
cumError += error * .02; // 20 ms
|
||||
double delta = error - prevError;
|
||||
|
||||
output = error * gains.kP;
|
||||
output += cumError * gains.kI;
|
||||
output += delta * gains.kD;
|
||||
output += gains.kF;
|
||||
|
||||
runWithOutput(output);
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public final boolean isFinished() {
|
||||
return Math.abs(getError()) < tolerance;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,42 +1,42 @@
|
||||
// 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.Placement;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class AprilRotAlign extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
Limelight lime;
|
||||
|
||||
/** Creates a new AprilRotAlign. */
|
||||
public AprilRotAlign(SwerveDrive drive, Limelight lime) {
|
||||
super(0.1, 0.2, 0.0, 0.0, 0.0);
|
||||
|
||||
this.drive = drive;
|
||||
this.lime = lime;
|
||||
|
||||
addRequirements(drive, lime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
double err = 0.0;
|
||||
|
||||
try {
|
||||
err = lime.getAprilSkew();
|
||||
} catch (NullPointerException ex) {}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
|
||||
}
|
||||
}
|
||||
// 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.Placement;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class AprilRotAlign extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
Limelight lime;
|
||||
|
||||
/** Creates a new AprilRotAlign. */
|
||||
public AprilRotAlign(SwerveDrive drive, Limelight lime) {
|
||||
super(0.1, 0.2, 0.0, 0.0, 0.0);
|
||||
|
||||
this.drive = drive;
|
||||
this.lime = lime;
|
||||
|
||||
addRequirements(drive, lime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
double err = 0.0;
|
||||
|
||||
try {
|
||||
err = lime.getAprilSkew();
|
||||
} catch (NullPointerException ex) {}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,44 +1,44 @@
|
||||
// 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.Placement;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class DriveToLimeDistance extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
Limelight lime;
|
||||
|
||||
double targetDistance;
|
||||
DoubleSupplier ds;
|
||||
|
||||
/** Creates a new DriveToLimeDistance. */
|
||||
public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
|
||||
super(0.5, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.lime = lime;
|
||||
this.targetDistance = targetDistance;
|
||||
this.ds = ds;
|
||||
|
||||
addRequirements(drive, lime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetDistance - ds.getAsDouble();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
System.out.println(output / Math.abs(getError()));
|
||||
drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);
|
||||
}
|
||||
}
|
||||
// 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.Placement;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class DriveToLimeDistance extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
Limelight lime;
|
||||
|
||||
double targetDistance;
|
||||
DoubleSupplier ds;
|
||||
|
||||
/** Creates a new DriveToLimeDistance. */
|
||||
public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
|
||||
super(0.5, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.lime = lime;
|
||||
this.targetDistance = targetDistance;
|
||||
this.ds = ds;
|
||||
|
||||
addRequirements(drive, lime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetDistance - ds.getAsDouble();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
System.out.println(output / Math.abs(getError()));
|
||||
drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,48 +1,48 @@
|
||||
// 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.Placement;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class LimeAlign extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
Limelight lime;
|
||||
|
||||
DoubleSupplier ds;
|
||||
|
||||
public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
|
||||
super(0.4, 0.4, 0.0, 0.0, tolerance);
|
||||
|
||||
this.drive = drive;
|
||||
this.lime = lime;
|
||||
this.ds = ds;
|
||||
|
||||
addRequirements(drive, lime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
double err = 0.0;
|
||||
|
||||
try {
|
||||
System.out.println(ds.getAsDouble());
|
||||
err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
|
||||
} catch (NullPointerException ex) {}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
|
||||
}
|
||||
}
|
||||
// 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.Placement;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class LimeAlign extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
Limelight lime;
|
||||
|
||||
DoubleSupplier ds;
|
||||
|
||||
public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
|
||||
super(0.4, 0.4, 0.0, 0.0, tolerance);
|
||||
|
||||
this.drive = drive;
|
||||
this.lime = lime;
|
||||
this.ds = ds;
|
||||
|
||||
addRequirements(drive, lime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
double err = 0.0;
|
||||
|
||||
try {
|
||||
System.out.println(ds.getAsDouble());
|
||||
err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
|
||||
} catch (NullPointerException ex) {}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,141 +1,141 @@
|
||||
// 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.Swerve;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.FileNotFoundException;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Scanner;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.UtilityStructs.TimedOutput;
|
||||
|
||||
public class JoystickPlayback extends CommandBase {
|
||||
private final SwerveDrive swerve;
|
||||
private String filename;
|
||||
private int mult = 1;
|
||||
private Scanner input;
|
||||
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
|
||||
private int counter = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
private int lastIndex;
|
||||
private boolean m_finished = false; // ! find a better way
|
||||
|
||||
/** Creates a new JoystickPlayback. */
|
||||
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
|
||||
this.swerve = swerve;
|
||||
this.filename = filename;
|
||||
this.mult = mult;
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/** Creates a new JoystickPlayback. */
|
||||
public JoystickPlayback(SwerveDrive swerve, String filename) {
|
||||
this(swerve, filename, 1);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
outputs.clear();
|
||||
m_finished = false;
|
||||
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
lastIndex = 0;
|
||||
try {
|
||||
input = new Scanner(new File("/home/lvuser/autos/" + filename));
|
||||
|
||||
String line = "";
|
||||
while (input.hasNextLine()) {
|
||||
line = input.nextLine();
|
||||
|
||||
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
|
||||
continue;
|
||||
}
|
||||
|
||||
String[] values = line.split(",");
|
||||
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
|
||||
|
||||
var out = new TimedOutput();
|
||||
out.leftX = Double.parseDouble(values[0]) * mult;
|
||||
out.leftY = Double.parseDouble(values[1]);
|
||||
out.rightX = Double.parseDouble(values[2]);
|
||||
out.rightY = Double.parseDouble(values[3]);
|
||||
|
||||
out.timedOffset = Long.parseLong(values[4]);
|
||||
|
||||
outputs.add(out);
|
||||
}
|
||||
|
||||
input.close();
|
||||
} catch (FileNotFoundException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (counter == 0) {
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
} else {
|
||||
playbackTime = System.currentTimeMillis() - startTime;
|
||||
}
|
||||
|
||||
// skip to reasonable time frame
|
||||
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
|
||||
{
|
||||
int i = lastIndex == 0 ? 1 : lastIndex;
|
||||
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
|
||||
i++;
|
||||
}
|
||||
|
||||
if (i >= outputs.size()) {
|
||||
m_finished = true; // ! kind of a hack
|
||||
return;
|
||||
}
|
||||
lastIndex = i;
|
||||
}
|
||||
|
||||
TimedOutput lastOut = outputs.get(lastIndex - 1);
|
||||
TimedOutput out = outputs.get(lastIndex);
|
||||
|
||||
double deltaTime = out.timedOffset - lastOut.timedOffset;
|
||||
double playbackDelta = playbackTime - lastOut.timedOffset;
|
||||
|
||||
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
|
||||
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
|
||||
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
|
||||
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
|
||||
|
||||
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
|
||||
// new Translation2d(out.rightX, out.rightY),
|
||||
// true);
|
||||
|
||||
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
|
||||
new Translation2d(lerpRX, lerpRY),
|
||||
true);
|
||||
|
||||
counter++;
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
input.close();
|
||||
swerve.stopModules();
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
// 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.Swerve;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.FileNotFoundException;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Scanner;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.UtilityStructs.TimedOutput;
|
||||
|
||||
public class JoystickPlayback extends CommandBase {
|
||||
private final SwerveDrive swerve;
|
||||
private String filename;
|
||||
private int mult = 1;
|
||||
private Scanner input;
|
||||
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
|
||||
private int counter = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
private int lastIndex;
|
||||
private boolean m_finished = false; // ! find a better way
|
||||
|
||||
/** Creates a new JoystickPlayback. */
|
||||
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
|
||||
this.swerve = swerve;
|
||||
this.filename = filename;
|
||||
this.mult = mult;
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
/** Creates a new JoystickPlayback. */
|
||||
public JoystickPlayback(SwerveDrive swerve, String filename) {
|
||||
this(swerve, filename, 1);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
outputs.clear();
|
||||
m_finished = false;
|
||||
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
lastIndex = 0;
|
||||
try {
|
||||
input = new Scanner(new File("/home/lvuser/autos/" + filename));
|
||||
|
||||
String line = "";
|
||||
while (input.hasNextLine()) {
|
||||
line = input.nextLine();
|
||||
|
||||
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
|
||||
continue;
|
||||
}
|
||||
|
||||
String[] values = line.split(",");
|
||||
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
|
||||
|
||||
var out = new TimedOutput();
|
||||
out.leftX = Double.parseDouble(values[0]) * mult;
|
||||
out.leftY = Double.parseDouble(values[1]);
|
||||
out.rightX = Double.parseDouble(values[2]);
|
||||
out.rightY = Double.parseDouble(values[3]);
|
||||
|
||||
out.timedOffset = Long.parseLong(values[4]);
|
||||
|
||||
outputs.add(out);
|
||||
}
|
||||
|
||||
input.close();
|
||||
} catch (FileNotFoundException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (counter == 0) {
|
||||
startTime = System.currentTimeMillis();
|
||||
playbackTime = 0;
|
||||
} else {
|
||||
playbackTime = System.currentTimeMillis() - startTime;
|
||||
}
|
||||
|
||||
// skip to reasonable time frame
|
||||
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
|
||||
{
|
||||
int i = lastIndex == 0 ? 1 : lastIndex;
|
||||
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
|
||||
i++;
|
||||
}
|
||||
|
||||
if (i >= outputs.size()) {
|
||||
m_finished = true; // ! kind of a hack
|
||||
return;
|
||||
}
|
||||
lastIndex = i;
|
||||
}
|
||||
|
||||
TimedOutput lastOut = outputs.get(lastIndex - 1);
|
||||
TimedOutput out = outputs.get(lastIndex);
|
||||
|
||||
double deltaTime = out.timedOffset - lastOut.timedOffset;
|
||||
double playbackDelta = playbackTime - lastOut.timedOffset;
|
||||
|
||||
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
|
||||
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
|
||||
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
|
||||
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
|
||||
|
||||
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
|
||||
// new Translation2d(out.rightX, out.rightY),
|
||||
// true);
|
||||
|
||||
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
|
||||
new Translation2d(lerpRX, lerpRY),
|
||||
true);
|
||||
|
||||
counter++;
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
input.close();
|
||||
swerve.stopModules();
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return m_finished;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,97 +1,99 @@
|
||||
// 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.Swerve;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.io.PrintWriter;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.UtilityStructs.TimedOutput;
|
||||
|
||||
public class JoystickRecorder extends CommandBase {
|
||||
public final SwerveDrive swerve;
|
||||
|
||||
public final Supplier<Double> leftX;
|
||||
public final Supplier<Double> leftY;
|
||||
public final Supplier<Double> rightX;
|
||||
public final Supplier<Double> rightY;
|
||||
private String filename;
|
||||
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
|
||||
private long startTime = -1;
|
||||
|
||||
|
||||
/** Creates a new JoystickRecorder. */
|
||||
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
|
||||
Supplier<Double> rightX, Supplier<Double> rightY,
|
||||
String filename)
|
||||
{
|
||||
this.swerve = swerve;
|
||||
this.leftX = leftX;
|
||||
this.leftY = leftY;
|
||||
this.rightX = rightX;
|
||||
this.rightY = rightY;
|
||||
this.filename = filename;
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
outputs.clear();
|
||||
|
||||
this.startTime = System.currentTimeMillis();
|
||||
|
||||
outputs.add(new TimedOutput());
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
var inputs = new TimedOutput();
|
||||
inputs.leftX = leftX.get();
|
||||
inputs.leftY = leftY.get();
|
||||
inputs.rightX = rightX.get();
|
||||
inputs.rightY = rightY.get();
|
||||
inputs.timedOffset = System.currentTimeMillis() - startTime;
|
||||
|
||||
outputs.add(inputs);
|
||||
|
||||
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
|
||||
new Translation2d(inputs.rightX, inputs.rightY),
|
||||
true);
|
||||
|
||||
System.out.println("RECORDING");
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
File output = new File("/home/lvuser/autos/" + filename);
|
||||
|
||||
try (PrintWriter writer = new PrintWriter(output)) {
|
||||
for (var input : outputs) {
|
||||
writer.println( input.leftX + "," + input.leftY + "," +
|
||||
input.rightX + "," + input.rightY + "," +
|
||||
input.timedOffset);
|
||||
}
|
||||
|
||||
writer.close();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// 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.Swerve;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.io.PrintWriter;
|
||||
import java.util.ArrayList;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.UtilityStructs.TimedOutput;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
|
||||
public class JoystickRecorder extends CommandBase {
|
||||
public final SwerveDrive swerve;
|
||||
public final XboxController driveXbox;
|
||||
public final XboxController operatorXbox;
|
||||
|
||||
private String filename;
|
||||
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
|
||||
private long startTime = -1;
|
||||
|
||||
/** Creates a new JoystickRecorder. */
|
||||
public JoystickRecorder(SwerveDrive swerve, XboxController driveXbox,
|
||||
XboxController operatorXbox, String filename) {
|
||||
this.swerve = swerve;
|
||||
this.driveXbox = driveXbox;
|
||||
this.operatorXbox = operatorXbox;
|
||||
this.filename = filename;
|
||||
|
||||
addRequirements(this.swerve);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
outputs.clear();
|
||||
|
||||
this.startTime = System.currentTimeMillis();
|
||||
|
||||
outputs.add(new TimedOutput());
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
var inputs = new TimedOutput();
|
||||
inputs.driverLeftX = driveXbox.getLeftXAxis();
|
||||
inputs.driverLeftY = driveXbox.getLeftYAxis();
|
||||
inputs.driverRightX = driveXbox.getRightXAxis();
|
||||
inputs.driverRightY = driveXbox.getRightYAxis();
|
||||
|
||||
inputs.operatorLeftX = driveXbox.getLeftXAxis();
|
||||
inputs.operatorLeftY = driveXbox.getLeftYAxis();
|
||||
inputs.operatorRightX = driveXbox.getRightXAxis();
|
||||
inputs.operatorRightY = driveXbox.getRightYAxis();
|
||||
|
||||
inputs.timedOffset = System.currentTimeMillis() - startTime;
|
||||
|
||||
outputs.add(inputs);
|
||||
|
||||
swerve.driveWithInput(new Translation2d(inputs.driverLeftX, inputs.driverLeftX),
|
||||
new Translation2d(inputs.driverRightX, inputs.driverRightY),
|
||||
true);
|
||||
|
||||
System.out.println("RECORDING");
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
File output = new File("/home/lvuser/autos/" + filename);
|
||||
|
||||
try (PrintWriter writer = new PrintWriter(output)) {
|
||||
for (var input : outputs) {
|
||||
writer.println( input.driverLeftX + "," + input.driverLeftX + "," +
|
||||
input.driverRightX + "," + input.driverRightY + "," +
|
||||
input.operatorLeftX + "," + input.operatorLeftX + "," +
|
||||
input.operatorRightX + "," + input.operatorRightY + "," +
|
||||
input.timedOffset);
|
||||
}
|
||||
|
||||
writer.close();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,35 +1,35 @@
|
||||
// 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.Swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class RotateToAngle extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
double targetAngle;
|
||||
|
||||
/** Creates a new RotateToAngle. */
|
||||
public RotateToAngle(SwerveDrive drive, double targetAngle) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.targetAngle = targetAngle;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetAngle - drive.getGyroAngle();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
|
||||
}
|
||||
}
|
||||
// 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.Swerve;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import frc4388.robot.commands.PelvicInflammatoryDisease;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class RotateToAngle extends PelvicInflammatoryDisease {
|
||||
|
||||
SwerveDrive drive;
|
||||
double targetAngle;
|
||||
|
||||
/** Creates a new RotateToAngle. */
|
||||
public RotateToAngle(SwerveDrive drive, double targetAngle) {
|
||||
super(0.3, 0.0, 0.0, 0.0, 1);
|
||||
|
||||
this.drive = drive;
|
||||
this.targetAngle = targetAngle;
|
||||
|
||||
addRequirements(drive);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getError() {
|
||||
return targetAngle - drive.getGyroAngle();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runWithOutput(double output) {
|
||||
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,36 +1,36 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
//import edu.wpi.first.apriltag.AprilTag;
|
||||
//import edu.wpi.first.math.geometry.Pose3d;
|
||||
//import edu.wpi.first.math.geometry.Rotation3d;
|
||||
//import edu.wpi.first.networktables.NetworkTable;
|
||||
//import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
|
||||
public class Apriltags {
|
||||
public static class Tag {
|
||||
public boolean visible = true;
|
||||
public double x, y, z = 0;
|
||||
public double ry, rp, rr = 0;
|
||||
}
|
||||
|
||||
public Tag getTagPosRot() {
|
||||
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
|
||||
|
||||
final Tag tag = new Tag();
|
||||
tag.visible = isAprilTag();
|
||||
tag.x = tagTable.getEntry("TagPosX").getDouble(0);
|
||||
tag.y = tagTable.getEntry("TagPosY").getDouble(0);
|
||||
tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
|
||||
tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
|
||||
tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
|
||||
tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
|
||||
|
||||
return tag;
|
||||
}
|
||||
|
||||
public boolean isAprilTag() {
|
||||
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
|
||||
return tagTable.getEntry("IsTag").getBoolean(false);
|
||||
}
|
||||
}
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
//import edu.wpi.first.apriltag.AprilTag;
|
||||
//import edu.wpi.first.math.geometry.Pose3d;
|
||||
//import edu.wpi.first.math.geometry.Rotation3d;
|
||||
//import edu.wpi.first.networktables.NetworkTable;
|
||||
//import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
|
||||
public class Apriltags {
|
||||
public static class Tag {
|
||||
public boolean visible = true;
|
||||
public double x, y, z = 0;
|
||||
public double ry, rp, rr = 0;
|
||||
}
|
||||
|
||||
public Tag getTagPosRot() {
|
||||
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
|
||||
|
||||
final Tag tag = new Tag();
|
||||
tag.visible = isAprilTag();
|
||||
tag.x = tagTable.getEntry("TagPosX").getDouble(0);
|
||||
tag.y = tagTable.getEntry("TagPosY").getDouble(0);
|
||||
tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
|
||||
tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
|
||||
tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
|
||||
tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
|
||||
|
||||
return tag;
|
||||
}
|
||||
|
||||
public boolean isAprilTag() {
|
||||
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
|
||||
return tagTable.getEntry("IsTag").getBoolean(false);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,159 +1,159 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Arm extends SubsystemBase {
|
||||
private WPI_TalonFX m_tele;
|
||||
public WPI_TalonFX m_pivot;
|
||||
private CANCoder m_pivotEncoder;
|
||||
|
||||
// Moves arm to distance [dist] then returns new ang
|
||||
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
|
||||
m_tele = tele;
|
||||
m_pivot = pivot;
|
||||
m_pivotEncoder = encoder;
|
||||
|
||||
m_tele.configFactoryDefault();
|
||||
m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
|
||||
m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
|
||||
m_pivot.configFactoryDefault();
|
||||
|
||||
// * Example of deferred code
|
||||
new DeferredBlock(() -> resetTeleSoftLimit());
|
||||
|
||||
CANCoderConfiguration config = new CANCoderConfiguration();
|
||||
config.magnetOffsetDegrees = ArmConstants.OFFSET;
|
||||
m_pivotEncoder.configAllSettings(config);
|
||||
}
|
||||
|
||||
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
|
||||
this(pivot, tele, encoder, false);
|
||||
}
|
||||
|
||||
public void setRotVel(double vel) {
|
||||
if (vel > 1) vel = 1;
|
||||
|
||||
var degrees = Math.abs(getArmRotation()) - 135;
|
||||
SmartDashboard.putNumber("arm degrees", degrees);
|
||||
SmartDashboard.putNumber("arm rot vel", vel);
|
||||
|
||||
if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
|
||||
m_pivot.set(ControlMode.PercentOutput, 0);
|
||||
} else if (degrees > 90 && vel > 0) {
|
||||
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
|
||||
} else if (degrees < 25 && vel < 0) {
|
||||
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
|
||||
} else {
|
||||
m_pivot.set(ControlMode.PercentOutput, .3 * vel);
|
||||
}
|
||||
}
|
||||
|
||||
public void setTeleVel(double vel) {
|
||||
m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
|
||||
}
|
||||
|
||||
public void armSetRotation(double rot) {
|
||||
if (rot > 1 || rot < 0) return;
|
||||
// Move arm code
|
||||
m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
|
||||
ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
|
||||
}
|
||||
|
||||
public void armSetLength(double len) {
|
||||
if (len > 1 || len < 0) return;
|
||||
// Move arm code
|
||||
m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
|
||||
ArmConstants.TELE_FORWARD_SOFT_LIMIT);
|
||||
|
||||
if (m_tele.isRevLimitSwitchClosed() == 1) {
|
||||
m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
|
||||
} else if (m_tele.isFwdLimitSwitchClosed() == 1) {
|
||||
m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
|
||||
}
|
||||
}
|
||||
|
||||
public double getArmLength() {
|
||||
return m_tele.getSelectedSensorPosition() - tele_soft;
|
||||
}
|
||||
|
||||
public double getArmRotation() {
|
||||
return m_pivotEncoder.getAbsolutePosition();
|
||||
}
|
||||
|
||||
public void runPivotTele(double pivot, double tele) {
|
||||
double abs_pivot = Math.toRadians(getArmRotation() - 135);
|
||||
double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
|
||||
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
|
||||
|
||||
if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
|
||||
setRotVel(pivot);
|
||||
setTeleVel(tele);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks that an input is within bounds
|
||||
* @param _len length from 0 to 1
|
||||
* @param _theta radians from the zero (straight up)
|
||||
* @return
|
||||
*/
|
||||
public static boolean checkLimits(double _len, double _theta) {
|
||||
var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN;
|
||||
var x = len * Math.cos(_theta);
|
||||
var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
|
||||
|
||||
var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
|
||||
return y < minHeight;
|
||||
}
|
||||
|
||||
boolean tele_softLimit = false;
|
||||
double tele_soft = 0;
|
||||
public void resetTeleSoftLimit() {
|
||||
if (!tele_softLimit) {
|
||||
tele_soft = m_tele.getSelectedSensorPosition();
|
||||
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
|
||||
m_tele.configReverseSoftLimitThreshold(tele_soft);
|
||||
m_tele.configForwardSoftLimitEnable(true);
|
||||
m_tele.configReverseSoftLimitEnable(true);
|
||||
} else {
|
||||
m_tele.configForwardSoftLimitEnable(false);
|
||||
m_tele.configReverseSoftLimitEnable(false);
|
||||
}
|
||||
|
||||
tele_softLimit = !tele_softLimit;
|
||||
}
|
||||
|
||||
boolean resetable = true;
|
||||
boolean tele_reset = true;
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
|
||||
var tele_soft = m_tele.getSelectedSensorPosition();
|
||||
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
|
||||
m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
|
||||
m_tele.configForwardSoftLimitEnable(true);
|
||||
m_tele.configReverseSoftLimitEnable(true);
|
||||
tele_reset = false;
|
||||
} else if (m_tele.isFwdLimitSwitchClosed() == 0) {
|
||||
tele_reset = true;
|
||||
}
|
||||
|
||||
// double x = Math.cos(Math.toRadians(degrees));
|
||||
SmartDashboard.putNumber("arm length", getArmLength());
|
||||
}
|
||||
|
||||
public void killSoftLimits() {
|
||||
resetTeleSoftLimit();
|
||||
}
|
||||
}
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
import frc4388.robot.Constants.ArmConstants;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Arm extends SubsystemBase {
|
||||
private WPI_TalonFX m_tele;
|
||||
public WPI_TalonFX m_pivot;
|
||||
private CANCoder m_pivotEncoder;
|
||||
|
||||
// Moves arm to distance [dist] then returns new ang
|
||||
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
|
||||
m_tele = tele;
|
||||
m_pivot = pivot;
|
||||
m_pivotEncoder = encoder;
|
||||
|
||||
m_tele.configFactoryDefault();
|
||||
m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
|
||||
m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
|
||||
m_pivot.configFactoryDefault();
|
||||
|
||||
// * Example of deferred code
|
||||
new DeferredBlock(() -> resetTeleSoftLimit());
|
||||
|
||||
CANCoderConfiguration config = new CANCoderConfiguration();
|
||||
config.magnetOffsetDegrees = ArmConstants.OFFSET;
|
||||
m_pivotEncoder.configAllSettings(config);
|
||||
}
|
||||
|
||||
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
|
||||
this(pivot, tele, encoder, false);
|
||||
}
|
||||
|
||||
public void setRotVel(double vel) {
|
||||
if (vel > 1) vel = 1;
|
||||
|
||||
var degrees = Math.abs(getArmRotation()) - 135;
|
||||
SmartDashboard.putNumber("arm degrees", degrees);
|
||||
SmartDashboard.putNumber("arm rot vel", vel);
|
||||
|
||||
if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
|
||||
m_pivot.set(ControlMode.PercentOutput, 0);
|
||||
} else if (degrees > 90 && vel > 0) {
|
||||
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
|
||||
} else if (degrees < 25 && vel < 0) {
|
||||
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
|
||||
} else {
|
||||
m_pivot.set(ControlMode.PercentOutput, .3 * vel);
|
||||
}
|
||||
}
|
||||
|
||||
public void setTeleVel(double vel) {
|
||||
m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
|
||||
}
|
||||
|
||||
public void armSetRotation(double rot) {
|
||||
if (rot > 1 || rot < 0) return;
|
||||
// Move arm code
|
||||
m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
|
||||
ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
|
||||
}
|
||||
|
||||
public void armSetLength(double len) {
|
||||
if (len > 1 || len < 0) return;
|
||||
// Move arm code
|
||||
m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
|
||||
ArmConstants.TELE_FORWARD_SOFT_LIMIT);
|
||||
|
||||
if (m_tele.isRevLimitSwitchClosed() == 1) {
|
||||
m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
|
||||
} else if (m_tele.isFwdLimitSwitchClosed() == 1) {
|
||||
m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
|
||||
}
|
||||
}
|
||||
|
||||
public double getArmLength() {
|
||||
return m_tele.getSelectedSensorPosition() - tele_soft;
|
||||
}
|
||||
|
||||
public double getArmRotation() {
|
||||
return m_pivotEncoder.getAbsolutePosition();
|
||||
}
|
||||
|
||||
public void runPivotTele(double pivot, double tele) {
|
||||
double abs_pivot = Math.toRadians(getArmRotation() - 135);
|
||||
double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
|
||||
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
|
||||
|
||||
if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
|
||||
setRotVel(pivot);
|
||||
setTeleVel(tele);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks that an input is within bounds
|
||||
* @param _len length from 0 to 1
|
||||
* @param _theta radians from the zero (straight up)
|
||||
* @return
|
||||
*/
|
||||
public static boolean checkLimits(double _len, double _theta) {
|
||||
var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN;
|
||||
var x = len * Math.cos(_theta);
|
||||
var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
|
||||
|
||||
var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
|
||||
return y < minHeight;
|
||||
}
|
||||
|
||||
boolean tele_softLimit = false;
|
||||
double tele_soft = 0;
|
||||
public void resetTeleSoftLimit() {
|
||||
if (!tele_softLimit) {
|
||||
tele_soft = m_tele.getSelectedSensorPosition();
|
||||
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
|
||||
m_tele.configReverseSoftLimitThreshold(tele_soft);
|
||||
m_tele.configForwardSoftLimitEnable(true);
|
||||
m_tele.configReverseSoftLimitEnable(true);
|
||||
} else {
|
||||
m_tele.configForwardSoftLimitEnable(false);
|
||||
m_tele.configReverseSoftLimitEnable(false);
|
||||
}
|
||||
|
||||
tele_softLimit = !tele_softLimit;
|
||||
}
|
||||
|
||||
boolean resetable = true;
|
||||
boolean tele_reset = true;
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
|
||||
var tele_soft = m_tele.getSelectedSensorPosition();
|
||||
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
|
||||
m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
|
||||
m_tele.configForwardSoftLimitEnable(true);
|
||||
m_tele.configReverseSoftLimitEnable(true);
|
||||
tele_reset = false;
|
||||
} else if (m_tele.isFwdLimitSwitchClosed() == 0) {
|
||||
tele_reset = true;
|
||||
}
|
||||
|
||||
// double x = Math.cos(Math.toRadians(degrees));
|
||||
SmartDashboard.putNumber("arm length", getArmLength());
|
||||
}
|
||||
|
||||
public void killSoftLimits() {
|
||||
resetTeleSoftLimit();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,61 +1,61 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.Timer;
|
||||
import java.util.TimerTask;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Claw extends SubsystemBase {
|
||||
|
||||
private final PWM m_leftMotor;
|
||||
private final PWM m_rightMotor;
|
||||
private final CANSparkMax m_spinnyspin;
|
||||
|
||||
private boolean m_open = false;
|
||||
|
||||
// Opens claw
|
||||
public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
|
||||
m_leftMotor = leftMotor;
|
||||
m_rightMotor = rightMotor;
|
||||
m_spinnyspin = spinnyspin;
|
||||
|
||||
setClaw(true);
|
||||
}
|
||||
|
||||
public void setClaw(boolean open) {
|
||||
// Open claw
|
||||
m_open = open;
|
||||
m_leftMotor.setRaw(m_open ? 1000 : 2000);
|
||||
m_rightMotor.setRaw(m_open ? 2000 : 1000);
|
||||
|
||||
if (!m_open) {
|
||||
m_spinnyspin.set(-0.2);
|
||||
|
||||
new Timer().schedule(new TimerTask() {
|
||||
@Override
|
||||
public void run() {
|
||||
nospinnyspin();
|
||||
}
|
||||
}, 750);
|
||||
}
|
||||
}
|
||||
|
||||
public void toggle() {
|
||||
setClaw(!m_open);
|
||||
}
|
||||
|
||||
public boolean isClawOpen() {
|
||||
return m_open;
|
||||
}
|
||||
|
||||
public void yesspinnyspin() {
|
||||
m_spinnyspin.set(0.2);
|
||||
}
|
||||
|
||||
public void nospinnyspin() {
|
||||
m_spinnyspin.set(0);
|
||||
}
|
||||
}
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.Timer;
|
||||
import java.util.TimerTask;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
|
||||
import edu.wpi.first.wpilibj.PWM;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class Claw extends SubsystemBase {
|
||||
|
||||
private final PWM m_leftMotor;
|
||||
private final PWM m_rightMotor;
|
||||
private final CANSparkMax m_spinnyspin;
|
||||
|
||||
private boolean m_open = false;
|
||||
|
||||
// Opens claw
|
||||
public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
|
||||
m_leftMotor = leftMotor;
|
||||
m_rightMotor = rightMotor;
|
||||
m_spinnyspin = spinnyspin;
|
||||
|
||||
setClaw(true);
|
||||
}
|
||||
|
||||
public void setClaw(boolean open) {
|
||||
// Open claw
|
||||
m_open = open;
|
||||
m_leftMotor.setRaw(m_open ? 1000 : 2000);
|
||||
m_rightMotor.setRaw(m_open ? 2000 : 1000);
|
||||
|
||||
if (!m_open) {
|
||||
m_spinnyspin.set(-0.2);
|
||||
|
||||
new Timer().schedule(new TimerTask() {
|
||||
@Override
|
||||
public void run() {
|
||||
nospinnyspin();
|
||||
}
|
||||
}, 750);
|
||||
}
|
||||
}
|
||||
|
||||
public void toggle() {
|
||||
setClaw(!m_open);
|
||||
}
|
||||
|
||||
public boolean isClawOpen() {
|
||||
return m_open;
|
||||
}
|
||||
|
||||
public void yesspinnyspin() {
|
||||
m_spinnyspin.set(0.2);
|
||||
}
|
||||
|
||||
public void nospinnyspin() {
|
||||
m_spinnyspin.set(0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,63 +1,63 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 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.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
|
||||
* Driver
|
||||
*/
|
||||
public class LED extends SubsystemBase {
|
||||
|
||||
private LEDPatterns m_currentPattern;
|
||||
private Spark m_LEDController;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public LED(Spark LEDController){
|
||||
m_LEDController = LEDController;
|
||||
setPattern(LEDConstants.DEFAULT_PATTERN);
|
||||
updateLED();
|
||||
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void updateLED(){
|
||||
m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void setPattern(LEDPatterns pattern){
|
||||
m_currentPattern = pattern;
|
||||
m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
* @return
|
||||
*/
|
||||
public LEDPatterns getPattern() {
|
||||
return m_currentPattern;
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 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.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
|
||||
* Driver
|
||||
*/
|
||||
public class LED extends SubsystemBase {
|
||||
|
||||
private LEDPatterns m_currentPattern;
|
||||
private Spark m_LEDController;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public LED(Spark LEDController){
|
||||
m_LEDController = LEDController;
|
||||
setPattern(LEDConstants.DEFAULT_PATTERN);
|
||||
updateLED();
|
||||
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void updateLED(){
|
||||
m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public void setPattern(LEDPatterns pattern){
|
||||
m_currentPattern = pattern;
|
||||
m_LEDController.set(m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
* @return
|
||||
*/
|
||||
public LEDPatterns getPattern() {
|
||||
return m_currentPattern;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,165 +1,165 @@
|
||||
// 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.IOException;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.common.hardware.VisionLEDMode;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
import org.photonvision.targeting.TargetCorner;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
|
||||
public class Limelight extends SubsystemBase {
|
||||
|
||||
private PhotonCamera cam;
|
||||
private PhotonPoseEstimator photonPoseEstimator;
|
||||
|
||||
private boolean lightOn;
|
||||
|
||||
/** Creates a new Limelight. */
|
||||
public Limelight() {
|
||||
cam = new PhotonCamera(VisionConstants.NAME);
|
||||
cam.setDriverMode(false);
|
||||
}
|
||||
|
||||
public void setLEDs(boolean on) {
|
||||
lightOn = on;
|
||||
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
|
||||
public void toggleLEDs() {
|
||||
lightOn = !lightOn;
|
||||
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
|
||||
public void setDriverMode(boolean driverMode) {
|
||||
cam.setDriverMode(driverMode);
|
||||
}
|
||||
|
||||
public void setToLimePipeline() {
|
||||
cam.setPipelineIndex(1);
|
||||
setLEDs(true);
|
||||
}
|
||||
|
||||
public void setToAprilPipeline() {
|
||||
cam.setPipelineIndex(0);
|
||||
setLEDs(false);
|
||||
}
|
||||
|
||||
public PhotonTrackedTarget getAprilPoint() {
|
||||
if (!cam.isConnected()) return null;
|
||||
|
||||
PhotonPipelineResult result = cam.getLatestResult();
|
||||
|
||||
if (!result.hasTargets()) return null;
|
||||
|
||||
return result.getBestTarget();
|
||||
}
|
||||
|
||||
private List<TargetCorner> getAprilCorners() {
|
||||
if (!cam.isConnected()) return null;
|
||||
|
||||
PhotonPipelineResult result = cam.getLatestResult();
|
||||
|
||||
if (!result.hasTargets()) return null;
|
||||
|
||||
return result.getBestTarget().getDetectedCorners();
|
||||
}
|
||||
|
||||
public double getAprilSkew() {
|
||||
List<TargetCorner> corners = getAprilCorners();
|
||||
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
|
||||
|
||||
if (bottomSide == null) return 0;
|
||||
|
||||
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
|
||||
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
|
||||
|
||||
return bottomLeft.y - bottomRight.y;
|
||||
}
|
||||
|
||||
private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
|
||||
if (box == null) return null;
|
||||
|
||||
ArrayList<TargetCorner> bottomSide = new ArrayList<>();
|
||||
|
||||
TargetCorner l1 = new TargetCorner(-1, -1);
|
||||
TargetCorner l2 = new TargetCorner(-1, -1);
|
||||
|
||||
for (TargetCorner c : box) {
|
||||
if (c.y > l1.y) l1 = c;
|
||||
}
|
||||
|
||||
for (TargetCorner c : box) {
|
||||
if (c.y == l1.y) continue;
|
||||
if (c.y > l2.y) l2 = c;
|
||||
}
|
||||
|
||||
bottomSide.add(l1);
|
||||
bottomSide.add(l2);
|
||||
|
||||
return bottomSide;
|
||||
}
|
||||
|
||||
public double getDistanceToApril() {
|
||||
PhotonTrackedTarget aprilPoint = getAprilPoint();
|
||||
if (aprilPoint == null) return -1;
|
||||
|
||||
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
|
||||
double theta = 35.0 + aprilPoint.getPitch();
|
||||
|
||||
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
|
||||
return distanceToApril;
|
||||
}
|
||||
|
||||
public PhotonTrackedTarget getLowestTape() {
|
||||
if (!cam.isConnected()) return null;
|
||||
|
||||
PhotonPipelineResult result = cam.getLatestResult();
|
||||
|
||||
if (!result.hasTargets()) return null;
|
||||
|
||||
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
|
||||
|
||||
PhotonTrackedTarget lowest = points.get(0);
|
||||
for (PhotonTrackedTarget point : points) {
|
||||
if (point.getPitch() < lowest.getPitch()) {
|
||||
lowest = point;
|
||||
}
|
||||
}
|
||||
|
||||
return lowest;
|
||||
}
|
||||
|
||||
public double getDistanceToTape() {
|
||||
PhotonTrackedTarget tapePoint = getLowestTape();
|
||||
if (tapePoint == null) return -1;
|
||||
|
||||
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
|
||||
double theta = 35.0 + tapePoint.getPitch();
|
||||
|
||||
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
|
||||
return distanceToTape;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {}
|
||||
}
|
||||
// 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.IOException;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
|
||||
import org.photonvision.EstimatedRobotPose;
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.PhotonPoseEstimator;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.common.hardware.VisionLEDMode;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
import org.photonvision.targeting.TargetCorner;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
|
||||
public class Limelight extends SubsystemBase {
|
||||
|
||||
private PhotonCamera cam;
|
||||
private PhotonPoseEstimator photonPoseEstimator;
|
||||
|
||||
private boolean lightOn;
|
||||
|
||||
/** Creates a new Limelight. */
|
||||
public Limelight() {
|
||||
cam = new PhotonCamera(VisionConstants.NAME);
|
||||
cam.setDriverMode(false);
|
||||
}
|
||||
|
||||
public void setLEDs(boolean on) {
|
||||
lightOn = on;
|
||||
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
|
||||
public void toggleLEDs() {
|
||||
lightOn = !lightOn;
|
||||
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
|
||||
public void setDriverMode(boolean driverMode) {
|
||||
cam.setDriverMode(driverMode);
|
||||
}
|
||||
|
||||
public void setToLimePipeline() {
|
||||
cam.setPipelineIndex(1);
|
||||
setLEDs(true);
|
||||
}
|
||||
|
||||
public void setToAprilPipeline() {
|
||||
cam.setPipelineIndex(0);
|
||||
setLEDs(false);
|
||||
}
|
||||
|
||||
public PhotonTrackedTarget getAprilPoint() {
|
||||
if (!cam.isConnected()) return null;
|
||||
|
||||
PhotonPipelineResult result = cam.getLatestResult();
|
||||
|
||||
if (!result.hasTargets()) return null;
|
||||
|
||||
return result.getBestTarget();
|
||||
}
|
||||
|
||||
private List<TargetCorner> getAprilCorners() {
|
||||
if (!cam.isConnected()) return null;
|
||||
|
||||
PhotonPipelineResult result = cam.getLatestResult();
|
||||
|
||||
if (!result.hasTargets()) return null;
|
||||
|
||||
return result.getBestTarget().getDetectedCorners();
|
||||
}
|
||||
|
||||
public double getAprilSkew() {
|
||||
List<TargetCorner> corners = getAprilCorners();
|
||||
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
|
||||
|
||||
if (bottomSide == null) return 0;
|
||||
|
||||
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
|
||||
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
|
||||
|
||||
return bottomLeft.y - bottomRight.y;
|
||||
}
|
||||
|
||||
private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
|
||||
if (box == null) return null;
|
||||
|
||||
ArrayList<TargetCorner> bottomSide = new ArrayList<>();
|
||||
|
||||
TargetCorner l1 = new TargetCorner(-1, -1);
|
||||
TargetCorner l2 = new TargetCorner(-1, -1);
|
||||
|
||||
for (TargetCorner c : box) {
|
||||
if (c.y > l1.y) l1 = c;
|
||||
}
|
||||
|
||||
for (TargetCorner c : box) {
|
||||
if (c.y == l1.y) continue;
|
||||
if (c.y > l2.y) l2 = c;
|
||||
}
|
||||
|
||||
bottomSide.add(l1);
|
||||
bottomSide.add(l2);
|
||||
|
||||
return bottomSide;
|
||||
}
|
||||
|
||||
public double getDistanceToApril() {
|
||||
PhotonTrackedTarget aprilPoint = getAprilPoint();
|
||||
if (aprilPoint == null) return -1;
|
||||
|
||||
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
|
||||
double theta = 35.0 + aprilPoint.getPitch();
|
||||
|
||||
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
|
||||
return distanceToApril;
|
||||
}
|
||||
|
||||
public PhotonTrackedTarget getLowestTape() {
|
||||
if (!cam.isConnected()) return null;
|
||||
|
||||
PhotonPipelineResult result = cam.getLatestResult();
|
||||
|
||||
if (!result.hasTargets()) return null;
|
||||
|
||||
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
|
||||
|
||||
PhotonTrackedTarget lowest = points.get(0);
|
||||
for (PhotonTrackedTarget point : points) {
|
||||
if (point.getPitch() < lowest.getPitch()) {
|
||||
lowest = point;
|
||||
}
|
||||
}
|
||||
|
||||
return lowest;
|
||||
}
|
||||
|
||||
public double getDistanceToTape() {
|
||||
PhotonTrackedTarget tapePoint = getLowestTape();
|
||||
if (tapePoint == null) return -1;
|
||||
|
||||
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
|
||||
double theta = 35.0 + tapePoint.getPitch();
|
||||
|
||||
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
|
||||
return distanceToTape;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {}
|
||||
}
|
||||
|
||||
@@ -1,27 +1,27 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import frc4388.robot.subsystems.Apriltags.Tag;
|
||||
|
||||
public class Location {
|
||||
final Apriltags apriltag = new Apriltags();
|
||||
|
||||
private boolean isLimelight = false;
|
||||
private boolean isApriltag = false;
|
||||
|
||||
//Determines which source to get pos and rot from and also resets
|
||||
private void reoderPrio(){
|
||||
isLimelight = false; //If limelight gets position and if within a certain range of poles
|
||||
isApriltag = apriltag.isAprilTag();
|
||||
}
|
||||
|
||||
public Tag getPosRot() {
|
||||
reoderPrio();
|
||||
if(isApriltag){
|
||||
return apriltag.getTagPosRot();
|
||||
} else if (isLimelight) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
}
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import frc4388.robot.subsystems.Apriltags.Tag;
|
||||
|
||||
public class Location {
|
||||
final Apriltags apriltag = new Apriltags();
|
||||
|
||||
private boolean isLimelight = false;
|
||||
private boolean isApriltag = false;
|
||||
|
||||
//Determines which source to get pos and rot from and also resets
|
||||
private void reoderPrio(){
|
||||
isLimelight = false; //If limelight gets position and if within a certain range of poles
|
||||
isApriltag = apriltag.isAprilTag();
|
||||
}
|
||||
|
||||
public Tag getPosRot() {
|
||||
reoderPrio();
|
||||
if(isApriltag){
|
||||
return apriltag.getTagPosRot();
|
||||
} else if (isLimelight) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,195 +1,195 @@
|
||||
// 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.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
private SwerveModule leftFront;
|
||||
private SwerveModule rightFront;
|
||||
private SwerveModule leftBack;
|
||||
private SwerveModule rightBack;
|
||||
|
||||
private SwerveModule[] modules;
|
||||
|
||||
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
||||
this.leftFront = leftFront;
|
||||
this.rightFront = rightFront;
|
||||
this.leftBack = leftBack;
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.gyro = gyro;
|
||||
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
boolean stopped = false;
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
stopModules();
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
SmartDashboard.putBoolean("drift correction", true);
|
||||
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
|
||||
} else {
|
||||
// Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
double currentAngle = getGyroAngle();
|
||||
double error = angle - currentAngle;
|
||||
|
||||
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
rotTarget = 0.0;
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
for (SwerveModule module : this.modules) {
|
||||
module.stop();
|
||||
}
|
||||
}
|
||||
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
return this.kinematics;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
|
||||
|
||||
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
}
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
|
||||
} else {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void toggleGear(double angle) {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
// 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.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
private SwerveModule leftFront;
|
||||
private SwerveModule rightFront;
|
||||
private SwerveModule leftBack;
|
||||
private SwerveModule rightBack;
|
||||
|
||||
private SwerveModule[] modules;
|
||||
|
||||
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
|
||||
|
||||
private RobotGyro gyro;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
|
||||
this.leftFront = leftFront;
|
||||
this.rightFront = rightFront;
|
||||
this.leftBack = leftBack;
|
||||
this.rightBack = rightBack;
|
||||
|
||||
this.gyro = gyro;
|
||||
|
||||
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
|
||||
}
|
||||
|
||||
boolean stopped = false;
|
||||
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
if (fieldRelative) {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
stopModules();
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
SmartDashboard.putBoolean("drift correction", true);
|
||||
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
}
|
||||
|
||||
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
|
||||
|
||||
// Convert field-relative speeds to robot-relative speeds.
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
|
||||
} else {
|
||||
// Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
|
||||
public boolean rotateToTarget(double angle) {
|
||||
double currentAngle = getGyroAngle();
|
||||
double error = angle - currentAngle;
|
||||
|
||||
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
|
||||
|
||||
if (Math.abs(angle - getGyroAngle()) < 5.0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public double getGyroAngle() {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
|
||||
public void resetGyro() {
|
||||
gyro.reset();
|
||||
rotTarget = 0.0;
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
for (SwerveModule module : this.modules) {
|
||||
module.stop();
|
||||
}
|
||||
}
|
||||
|
||||
public SwerveDriveKinematics getKinematics() {
|
||||
return this.kinematics;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run\
|
||||
SmartDashboard.putNumber("Gyro", getGyroAngle());
|
||||
}
|
||||
|
||||
public void shiftDown() {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
|
||||
|
||||
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
}
|
||||
}
|
||||
|
||||
public void setToSlow() {
|
||||
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
System.out.println("SLOW");
|
||||
}
|
||||
|
||||
public void setToFast() {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
System.out.println("FAST");
|
||||
}
|
||||
|
||||
public void setToTurbo() {
|
||||
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
System.out.println("TURBO");
|
||||
}
|
||||
|
||||
public void shiftUp() {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
|
||||
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
|
||||
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
|
||||
} else {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public void toggleGear(double angle) {
|
||||
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
} else {
|
||||
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -1,161 +1,161 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder encoder;
|
||||
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.encoder = encoder;
|
||||
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
|
||||
angleConfig.slot0.kP = swerveGains.kP;
|
||||
angleConfig.slot0.kI = swerveGains.kI;
|
||||
angleConfig.slot0.kD = swerveGains.kD;
|
||||
|
||||
// use the CANcoder as the remote sensor for the primary TalonFX PID
|
||||
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
|
||||
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
|
||||
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleConfig);
|
||||
|
||||
encoder.configMagnetOffset(offset);
|
||||
|
||||
driveMotor.setSelectedSensorPosition(0);
|
||||
driveMotor.config_kP(0, 0.2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the drive motor of the SwerveModule
|
||||
* @return the drive motor of the SwerveModule
|
||||
*/
|
||||
public WPI_TalonFX getDriveMotor() {
|
||||
return this.driveMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle motor of the SwerveModule
|
||||
* @return the angle motor of the SwerveModule
|
||||
*/
|
||||
public WPI_TalonFX getAngleMotor() {
|
||||
return this.angleMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the CANcoder of the SwerveModule
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANCoder getEncoder() {
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle of a SwerveModule as a Rotation2d
|
||||
* @return the angle of a SwerveModule as a Rotation2d
|
||||
*/
|
||||
public Rotation2d getAngle() {
|
||||
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
public double getAngularVel() {
|
||||
return this.angleMotor.getSelectedSensorVelocity();
|
||||
}
|
||||
|
||||
public double getDrivePos() {
|
||||
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
}
|
||||
|
||||
public double getDriveVel() {
|
||||
return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
|
||||
public void rotateToAngle(double angle) {
|
||||
angleMotor.set(TalonFXControlMode.Position, angle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get state of swerve module
|
||||
* @return speed in m/s and angle in degrees
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
|
||||
getAngle()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current position of the SwerveModule
|
||||
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
|
||||
* @param desiredState a SwerveModuleState representing the desired new state of the module
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
Rotation2d currentRotation = this.getAngle();
|
||||
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// calculate the difference between our current rotational position and our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
|
||||
// calculate the new absolute position of the SwerveModule based on the difference in rotation
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
|
||||
|
||||
// convert the CANCoder from its position reading to ticks
|
||||
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
|
||||
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
|
||||
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
}
|
||||
|
||||
public void reset(double position) {
|
||||
encoder.setPositionToAbsolute();
|
||||
}
|
||||
|
||||
public double getCurrent() {
|
||||
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
}
|
||||
|
||||
public double getVoltage() {
|
||||
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
}
|
||||
}
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder encoder;
|
||||
|
||||
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.encoder = encoder;
|
||||
|
||||
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
|
||||
angleConfig.slot0.kP = swerveGains.kP;
|
||||
angleConfig.slot0.kI = swerveGains.kI;
|
||||
angleConfig.slot0.kD = swerveGains.kD;
|
||||
|
||||
// use the CANcoder as the remote sensor for the primary TalonFX PID
|
||||
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
|
||||
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
|
||||
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleConfig);
|
||||
|
||||
encoder.configMagnetOffset(offset);
|
||||
|
||||
driveMotor.setSelectedSensorPosition(0);
|
||||
driveMotor.config_kP(0, 0.2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the drive motor of the SwerveModule
|
||||
* @return the drive motor of the SwerveModule
|
||||
*/
|
||||
public WPI_TalonFX getDriveMotor() {
|
||||
return this.driveMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle motor of the SwerveModule
|
||||
* @return the angle motor of the SwerveModule
|
||||
*/
|
||||
public WPI_TalonFX getAngleMotor() {
|
||||
return this.angleMotor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the CANcoder of the SwerveModule
|
||||
* @return the CANcoder of the SwerveModule
|
||||
*/
|
||||
public CANCoder getEncoder() {
|
||||
return this.encoder;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle of a SwerveModule as a Rotation2d
|
||||
* @return the angle of a SwerveModule as a Rotation2d
|
||||
*/
|
||||
public Rotation2d getAngle() {
|
||||
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
public double getAngularVel() {
|
||||
return this.angleMotor.getSelectedSensorVelocity();
|
||||
}
|
||||
|
||||
public double getDrivePos() {
|
||||
return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
|
||||
}
|
||||
|
||||
public double getDriveVel() {
|
||||
return this.driveMotor.getSelectedSensorVelocity(0);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
|
||||
public void rotateToAngle(double angle) {
|
||||
angleMotor.set(TalonFXControlMode.Position, angle);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get state of swerve module
|
||||
* @return speed in m/s and angle in degrees
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
return new SwerveModuleState(
|
||||
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
|
||||
getAngle()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current position of the SwerveModule
|
||||
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
|
||||
*/
|
||||
public SwerveModulePosition getPosition() {
|
||||
return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
|
||||
* @param desiredState a SwerveModuleState representing the desired new state of the module
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
Rotation2d currentRotation = this.getAngle();
|
||||
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// calculate the difference between our current rotational position and our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
|
||||
// calculate the new absolute position of the SwerveModule based on the difference in rotation
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
|
||||
|
||||
// convert the CANCoder from its position reading to ticks
|
||||
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
|
||||
|
||||
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
|
||||
driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
|
||||
}
|
||||
|
||||
public void reset(double position) {
|
||||
encoder.setPositionToAbsolute();
|
||||
}
|
||||
|
||||
public double getCurrent() {
|
||||
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
|
||||
}
|
||||
|
||||
public double getVoltage() {
|
||||
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,38 +1,38 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
|
||||
public class Vision {
|
||||
private final NetworkTableEntry m_isTags;
|
||||
private final NetworkTableEntry m_xPoses;
|
||||
private final NetworkTableEntry m_yPoses;
|
||||
private final NetworkTableEntry m_zPoses;
|
||||
|
||||
public Vision() {
|
||||
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
|
||||
|
||||
m_isTags = tagTable.getEntry("IsTag");
|
||||
m_xPoses = tagTable.getEntry("TagPosX");
|
||||
m_yPoses = tagTable.getEntry("TagPosY");
|
||||
m_zPoses = tagTable.getEntry("TagPosZ");
|
||||
}
|
||||
|
||||
public AprilTag[] getAprilTags() {
|
||||
if (!m_isTags.getBoolean(false)) return new AprilTag[0];
|
||||
|
||||
double xarr[] = m_xPoses.getDoubleArray(new double[] {});
|
||||
double yarr[] = m_yPoses.getDoubleArray(new double[] {});
|
||||
double zarr[] = m_zPoses.getDoubleArray(new double[] {});
|
||||
|
||||
AprilTag tags[] = new AprilTag[xarr.length];
|
||||
for (int i = 0; i < tags.length; i++) {
|
||||
tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
|
||||
}
|
||||
|
||||
return tags;
|
||||
}
|
||||
}
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTag;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
|
||||
public class Vision {
|
||||
private final NetworkTableEntry m_isTags;
|
||||
private final NetworkTableEntry m_xPoses;
|
||||
private final NetworkTableEntry m_yPoses;
|
||||
private final NetworkTableEntry m_zPoses;
|
||||
|
||||
public Vision() {
|
||||
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
|
||||
|
||||
m_isTags = tagTable.getEntry("IsTag");
|
||||
m_xPoses = tagTable.getEntry("TagPosX");
|
||||
m_yPoses = tagTable.getEntry("TagPosY");
|
||||
m_zPoses = tagTable.getEntry("TagPosZ");
|
||||
}
|
||||
|
||||
public AprilTag[] getAprilTags() {
|
||||
if (!m_isTags.getBoolean(false)) return new AprilTag[0];
|
||||
|
||||
double xarr[] = m_xPoses.getDoubleArray(new double[] {});
|
||||
double yarr[] = m_yPoses.getDoubleArray(new double[] {});
|
||||
double zarr[] = m_zPoses.getDoubleArray(new double[] {});
|
||||
|
||||
AprilTag tags[] = new AprilTag[xarr.length];
|
||||
for (int i = 0; i < tags.length; i++) {
|
||||
tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
|
||||
}
|
||||
|
||||
return tags;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,43 +1,43 @@
|
||||
// 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;
|
||||
|
||||
/** Exception that occurs if the limelight can't see enough points
|
||||
* @author Abhi Sachi
|
||||
* @author Aarav Shah
|
||||
*/
|
||||
public class AbhiIsADumbass extends Exception {
|
||||
/**
|
||||
* Creates new AbhiIsADumbass with error text 'null'
|
||||
*/
|
||||
public AbhiIsADumbass() {
|
||||
super("Unable to see sufficient vision points (Abhi is a dumbass)");
|
||||
}
|
||||
|
||||
/** Creates new AbhiIsADumbass with error text message
|
||||
*
|
||||
* @param message Error text message
|
||||
*/
|
||||
public AbhiIsADumbass(String message) {
|
||||
super(message);
|
||||
}
|
||||
|
||||
/** Creates new AbhiIsADumbass with error text message and detailed stack trace
|
||||
*
|
||||
* @param message Error text message
|
||||
* @param cause Root cause of error
|
||||
*/
|
||||
public AbhiIsADumbass(String message, Throwable cause) {
|
||||
super(message, cause);
|
||||
}
|
||||
|
||||
/** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace
|
||||
*
|
||||
* @param cause Root cause of error
|
||||
*/
|
||||
public AbhiIsADumbass(Throwable cause) {
|
||||
super("Unable to see sufficient vision points (Abhi is a dumbass)", cause);
|
||||
}
|
||||
}
|
||||
// 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;
|
||||
|
||||
/** Exception that occurs if the limelight can't see enough points
|
||||
* @author Abhi Sachi
|
||||
* @author Aarav Shah
|
||||
*/
|
||||
public class AbhiIsADumbass extends Exception {
|
||||
/**
|
||||
* Creates new AbhiIsADumbass with error text 'null'
|
||||
*/
|
||||
public AbhiIsADumbass() {
|
||||
super("Unable to see sufficient vision points (Abhi is a dumbass)");
|
||||
}
|
||||
|
||||
/** Creates new AbhiIsADumbass with error text message
|
||||
*
|
||||
* @param message Error text message
|
||||
*/
|
||||
public AbhiIsADumbass(String message) {
|
||||
super(message);
|
||||
}
|
||||
|
||||
/** Creates new AbhiIsADumbass with error text message and detailed stack trace
|
||||
*
|
||||
* @param message Error text message
|
||||
* @param cause Root cause of error
|
||||
*/
|
||||
public AbhiIsADumbass(String message, Throwable cause) {
|
||||
super(message, cause);
|
||||
}
|
||||
|
||||
/** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace
|
||||
*
|
||||
* @param cause Root cause of error
|
||||
*/
|
||||
public AbhiIsADumbass(Throwable cause) {
|
||||
super("Unable to see sufficient vision points (Abhi is a dumbass)", cause);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,13 +1,13 @@
|
||||
package frc4388.utility;
|
||||
|
||||
// This is a seperate class in case I want to encode rotation or other
|
||||
// information about the tag
|
||||
public class AprilTag {
|
||||
public final double x, y, z;
|
||||
|
||||
public AprilTag(double _x, double _y, double _z) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
}
|
||||
}
|
||||
package frc4388.utility;
|
||||
|
||||
// This is a seperate class in case I want to encode rotation or other
|
||||
// information about the tag
|
||||
public class AprilTag {
|
||||
public final double x, y, z;
|
||||
|
||||
public AprilTag(double _x, double _y, double _z) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,23 +1,23 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class DeferredBlock {
|
||||
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
|
||||
private static boolean m_hasRun = false;
|
||||
|
||||
public DeferredBlock(Runnable block) {
|
||||
m_blocks.add(block);
|
||||
}
|
||||
|
||||
public static void execute() {
|
||||
if (m_hasRun) return;
|
||||
|
||||
for (Runnable block : m_blocks) {
|
||||
block.run();
|
||||
}
|
||||
|
||||
m_blocks.clear(); // for garbage collection
|
||||
m_hasRun = true;
|
||||
}
|
||||
}
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class DeferredBlock {
|
||||
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
|
||||
private static boolean m_hasRun = false;
|
||||
|
||||
public DeferredBlock(Runnable block) {
|
||||
m_blocks.add(block);
|
||||
}
|
||||
|
||||
public static void execute() {
|
||||
if (m_hasRun) return;
|
||||
|
||||
for (Runnable block : m_blocks) {
|
||||
block.run();
|
||||
}
|
||||
|
||||
m_blocks.clear(); // for garbage collection
|
||||
m_hasRun = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,83 +1,83 @@
|
||||
// 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;
|
||||
|
||||
/** Add your docs here. */
|
||||
public class Gains {
|
||||
public double kP;
|
||||
public double kI;
|
||||
public double kD;
|
||||
public double kF;
|
||||
public int kIZone;
|
||||
public double kPeakOutput;
|
||||
public double kMaxOutput;
|
||||
public double kMinOutput;
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kPeakOutput = kPeakOutput;
|
||||
this.kMaxOutput = kPeakOutput;
|
||||
this.kMinOutput = -kPeakOutput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kPeakOutput = 1.0;
|
||||
this.kMaxOutput = 1.0;
|
||||
this.kMinOutput = -1.0;
|
||||
}
|
||||
|
||||
public Gains(double kP, double kI, double kD) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
|
||||
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kMaxOutput = kMaxOutput;
|
||||
this.kMinOutput = kMinOutput;
|
||||
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
|
||||
}
|
||||
// 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;
|
||||
|
||||
/** Add your docs here. */
|
||||
public class Gains {
|
||||
public double kP;
|
||||
public double kI;
|
||||
public double kD;
|
||||
public double kF;
|
||||
public int kIZone;
|
||||
public double kPeakOutput;
|
||||
public double kMaxOutput;
|
||||
public double kMinOutput;
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kPeakOutput = kPeakOutput;
|
||||
this.kMaxOutput = kPeakOutput;
|
||||
this.kMinOutput = -kPeakOutput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kPeakOutput = 1.0;
|
||||
this.kMaxOutput = 1.0;
|
||||
this.kMinOutput = -1.0;
|
||||
}
|
||||
|
||||
public Gains(double kP, double kI, double kD) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
|
||||
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kMaxOutput = kMaxOutput;
|
||||
this.kMinOutput = kMinOutput;
|
||||
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
|
||||
}
|
||||
}
|
||||
@@ -1,45 +1,45 @@
|
||||
package frc4388.utility;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public enum LEDPatterns {
|
||||
/* PALLETTE PATTERNS */
|
||||
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
|
||||
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
|
||||
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
|
||||
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
|
||||
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
|
||||
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
|
||||
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
|
||||
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
|
||||
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
|
||||
|
||||
/* COLOR 1 PATTERNS */
|
||||
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
|
||||
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
|
||||
|
||||
/* COLOR 2 PATTERNS */
|
||||
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
|
||||
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
|
||||
|
||||
/* COLOR 1 AND 2 PATTERNS */
|
||||
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
|
||||
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
|
||||
|
||||
/* SOLID COLORS */
|
||||
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
|
||||
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
|
||||
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
|
||||
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
|
||||
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
|
||||
|
||||
/* GETTERS/SETTERS */
|
||||
private final float id;
|
||||
LEDPatterns(float id) {
|
||||
this.id = id;
|
||||
}
|
||||
public float getValue() {
|
||||
return id;
|
||||
}
|
||||
}
|
||||
package frc4388.utility;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public enum LEDPatterns {
|
||||
/* PALLETTE PATTERNS */
|
||||
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
|
||||
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
|
||||
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
|
||||
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
|
||||
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
|
||||
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
|
||||
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
|
||||
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
|
||||
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
|
||||
|
||||
/* COLOR 1 PATTERNS */
|
||||
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
|
||||
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
|
||||
|
||||
/* COLOR 2 PATTERNS */
|
||||
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
|
||||
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
|
||||
|
||||
/* COLOR 1 AND 2 PATTERNS */
|
||||
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
|
||||
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
|
||||
|
||||
/* SOLID COLORS */
|
||||
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
|
||||
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
|
||||
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
|
||||
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
|
||||
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
|
||||
|
||||
/* GETTERS/SETTERS */
|
||||
private final float id;
|
||||
LEDPatterns(float id) {
|
||||
this.id = id;
|
||||
}
|
||||
public float getValue() {
|
||||
return id;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,200 +1,200 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
// import edu.wpi.first.wpilibj.GyroBase;
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
|
||||
/**
|
||||
* Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
*/
|
||||
public class RobotGyro implements Gyro {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private WPI_Pigeon2 m_pigeon = null;
|
||||
private AHRS m_navX = null;
|
||||
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
|
||||
|
||||
private double m_lastPigeonAngle;
|
||||
private double m_deltaPigeonAngle;
|
||||
|
||||
private double pitchZero = 0;
|
||||
private double rollZero = 0;
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a pigeon
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(WPI_Pigeon2 gyro) {
|
||||
m_pigeon = gyro;
|
||||
m_isGyroAPigeon = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a navX
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(AHRS gyro){
|
||||
m_navX = gyro;
|
||||
m_isGyroAPigeon = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets yaw, pitch, and roll.
|
||||
*/
|
||||
public void resetZeroValues() {
|
||||
if (!m_isGyroAPigeon) return;
|
||||
|
||||
pitchZero = m_pigeon.getPitch();
|
||||
rollZero = m_pigeon.getRoll();
|
||||
}
|
||||
|
||||
/**
|
||||
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
|
||||
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
|
||||
*/
|
||||
public void updatePigeonDeltas() {
|
||||
double currentPigeonAngle = getAngle();
|
||||
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
|
||||
m_lastPigeonAngle = currentPigeonAngle;
|
||||
}
|
||||
|
||||
/**
|
||||
* <p>NavX:
|
||||
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
|
||||
* the center value as the Accumulator center value for subsequent measurements. It's important to
|
||||
* make sure that the robot is not moving while the centering calculations are in progress, this
|
||||
* is typically done when the robot is first turned on while it's sitting at rest before the
|
||||
* competition starts.
|
||||
*
|
||||
* <p>Pigeon:
|
||||
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
|
||||
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
|
||||
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
|
||||
* make sure that the robot is not moving while the tempurature calculations are in progress, this
|
||||
* is typically done when the robot is first turned on while it's sitting at rest before the
|
||||
* competition starts.
|
||||
*/
|
||||
@Override
|
||||
public void calibrate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.calibrate();
|
||||
} else {
|
||||
m_navX.calibrate();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void reset() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(0);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get Yaw, Pitch, and Roll data.
|
||||
*
|
||||
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
|
||||
* Yaw is within [-368,640, +368,640] degrees.
|
||||
* Pitch is within [-90,+90] degrees.
|
||||
* Roll is within [-90,+90] degrees.
|
||||
*/
|
||||
private double[] getPigeonAngles() {
|
||||
double[] ypr = new double[3];
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
|
||||
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getAngle() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return getPigeonAngles()[0];
|
||||
} else {
|
||||
return m_navX.getAngle();
|
||||
}
|
||||
}
|
||||
|
||||
public double getYaw() {
|
||||
return this.getAngle();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading() {
|
||||
return getHeading(getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading(double angle) {
|
||||
return Math.IEEEremainder(angle, 360);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current pitch value (in degrees, from -90 to 90)
|
||||
* reported by the sensor. Pitch is a measure of rotation around
|
||||
* the Y Axis.
|
||||
* @return The current pitch value in degrees (-90 to 90).
|
||||
*/
|
||||
public double getPitch() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
|
||||
} else {
|
||||
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current roll value (in degrees, from -90 to 90)
|
||||
* reported by the sensor. Roll is a measure of rotation around
|
||||
* the X Axis.
|
||||
* @return The current roll value in degrees (-90 to 90).
|
||||
*/
|
||||
public double getRoll() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
|
||||
} else {
|
||||
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getRate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
} else {
|
||||
return m_navX.getRate();
|
||||
}
|
||||
}
|
||||
|
||||
public WPI_Pigeon2 getPigeon(){
|
||||
return m_pigeon;
|
||||
}
|
||||
|
||||
public AHRS getNavX(){
|
||||
return m_navX;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() throws Exception {
|
||||
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
// import edu.wpi.first.wpilibj.GyroBase;
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
|
||||
/**
|
||||
* Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
*/
|
||||
public class RobotGyro implements Gyro {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private WPI_Pigeon2 m_pigeon = null;
|
||||
private AHRS m_navX = null;
|
||||
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
|
||||
|
||||
private double m_lastPigeonAngle;
|
||||
private double m_deltaPigeonAngle;
|
||||
|
||||
private double pitchZero = 0;
|
||||
private double rollZero = 0;
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a pigeon
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(WPI_Pigeon2 gyro) {
|
||||
m_pigeon = gyro;
|
||||
m_isGyroAPigeon = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a navX
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(AHRS gyro){
|
||||
m_navX = gyro;
|
||||
m_isGyroAPigeon = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets yaw, pitch, and roll.
|
||||
*/
|
||||
public void resetZeroValues() {
|
||||
if (!m_isGyroAPigeon) return;
|
||||
|
||||
pitchZero = m_pigeon.getPitch();
|
||||
rollZero = m_pigeon.getRoll();
|
||||
}
|
||||
|
||||
/**
|
||||
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
|
||||
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
|
||||
*/
|
||||
public void updatePigeonDeltas() {
|
||||
double currentPigeonAngle = getAngle();
|
||||
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
|
||||
m_lastPigeonAngle = currentPigeonAngle;
|
||||
}
|
||||
|
||||
/**
|
||||
* <p>NavX:
|
||||
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
|
||||
* the center value as the Accumulator center value for subsequent measurements. It's important to
|
||||
* make sure that the robot is not moving while the centering calculations are in progress, this
|
||||
* is typically done when the robot is first turned on while it's sitting at rest before the
|
||||
* competition starts.
|
||||
*
|
||||
* <p>Pigeon:
|
||||
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
|
||||
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
|
||||
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
|
||||
* make sure that the robot is not moving while the tempurature calculations are in progress, this
|
||||
* is typically done when the robot is first turned on while it's sitting at rest before the
|
||||
* competition starts.
|
||||
*/
|
||||
@Override
|
||||
public void calibrate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.calibrate();
|
||||
} else {
|
||||
m_navX.calibrate();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void reset() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(0);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get Yaw, Pitch, and Roll data.
|
||||
*
|
||||
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
|
||||
* Yaw is within [-368,640, +368,640] degrees.
|
||||
* Pitch is within [-90,+90] degrees.
|
||||
* Roll is within [-90,+90] degrees.
|
||||
*/
|
||||
private double[] getPigeonAngles() {
|
||||
double[] ypr = new double[3];
|
||||
m_pigeon.getYawPitchRoll(ypr);
|
||||
|
||||
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getAngle() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return getPigeonAngles()[0];
|
||||
} else {
|
||||
return m_navX.getAngle();
|
||||
}
|
||||
}
|
||||
|
||||
public double getYaw() {
|
||||
return this.getAngle();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading() {
|
||||
return getHeading(getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading(double angle) {
|
||||
return Math.IEEEremainder(angle, 360);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current pitch value (in degrees, from -90 to 90)
|
||||
* reported by the sensor. Pitch is a measure of rotation around
|
||||
* the Y Axis.
|
||||
* @return The current pitch value in degrees (-90 to 90).
|
||||
*/
|
||||
public double getPitch() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
|
||||
} else {
|
||||
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current roll value (in degrees, from -90 to 90)
|
||||
* reported by the sensor. Roll is a measure of rotation around
|
||||
* the X Axis.
|
||||
* @return The current roll value in degrees (-90 to 90).
|
||||
*/
|
||||
public double getRoll() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
|
||||
} else {
|
||||
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getRate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
} else {
|
||||
return m_navX.getRate();
|
||||
}
|
||||
}
|
||||
|
||||
public WPI_Pigeon2 getPigeon(){
|
||||
return m_pigeon;
|
||||
}
|
||||
|
||||
public AHRS getNavX(){
|
||||
return m_navX;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() throws Exception {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,79 +1,79 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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;
|
||||
|
||||
/**
|
||||
* <p>Keeps track of Robot times like time passed, delta time, etc
|
||||
* <p>All times are in milliseconds
|
||||
*/
|
||||
public class RobotTime {
|
||||
private long m_currTime = System.currentTimeMillis();
|
||||
public long m_deltaTime = 0;
|
||||
|
||||
private long m_startRobotTime = m_currTime;
|
||||
public long m_robotTime = 0;
|
||||
public long m_lastRobotTime = 0;
|
||||
|
||||
private long m_startMatchTime = 0;
|
||||
public long m_matchTime = 0;
|
||||
public long m_lastMatchTime = 0;
|
||||
|
||||
public long m_frameNumber = 0;
|
||||
|
||||
/**
|
||||
* Private constructor prevents other classes from instantiating
|
||||
*/
|
||||
private RobotTime(){}
|
||||
|
||||
private static RobotTime instance = null;
|
||||
|
||||
/**
|
||||
* Gets the instance of Robot Time. If there is no instance running one will be created.
|
||||
* @return instance of Robot Time
|
||||
*/
|
||||
public static RobotTime getInstance() {
|
||||
if (instance == null) {
|
||||
instance = new RobotTime();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this once per periodic loop.
|
||||
*/
|
||||
public void updateTimes() {
|
||||
m_lastRobotTime = m_robotTime;
|
||||
m_lastMatchTime = m_matchTime;
|
||||
|
||||
m_currTime = System.currentTimeMillis();
|
||||
m_robotTime = m_currTime - m_startRobotTime;
|
||||
m_deltaTime = m_robotTime - m_lastRobotTime;
|
||||
m_frameNumber++;
|
||||
|
||||
if (m_startMatchTime != 0) {
|
||||
m_matchTime = m_currTime - m_startMatchTime;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this in both the auto and periodic inits
|
||||
*/
|
||||
public void startMatchTime() {
|
||||
if (m_startMatchTime == 0) {
|
||||
m_startMatchTime = m_currTime;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this in disabled init
|
||||
*/
|
||||
public void endMatchTime() {
|
||||
m_startMatchTime = 0;
|
||||
m_matchTime = 0;
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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;
|
||||
|
||||
/**
|
||||
* <p>Keeps track of Robot times like time passed, delta time, etc
|
||||
* <p>All times are in milliseconds
|
||||
*/
|
||||
public class RobotTime {
|
||||
private long m_currTime = System.currentTimeMillis();
|
||||
public long m_deltaTime = 0;
|
||||
|
||||
private long m_startRobotTime = m_currTime;
|
||||
public long m_robotTime = 0;
|
||||
public long m_lastRobotTime = 0;
|
||||
|
||||
private long m_startMatchTime = 0;
|
||||
public long m_matchTime = 0;
|
||||
public long m_lastMatchTime = 0;
|
||||
|
||||
public long m_frameNumber = 0;
|
||||
|
||||
/**
|
||||
* Private constructor prevents other classes from instantiating
|
||||
*/
|
||||
private RobotTime(){}
|
||||
|
||||
private static RobotTime instance = null;
|
||||
|
||||
/**
|
||||
* Gets the instance of Robot Time. If there is no instance running one will be created.
|
||||
* @return instance of Robot Time
|
||||
*/
|
||||
public static RobotTime getInstance() {
|
||||
if (instance == null) {
|
||||
instance = new RobotTime();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this once per periodic loop.
|
||||
*/
|
||||
public void updateTimes() {
|
||||
m_lastRobotTime = m_robotTime;
|
||||
m_lastMatchTime = m_matchTime;
|
||||
|
||||
m_currTime = System.currentTimeMillis();
|
||||
m_robotTime = m_currTime - m_startRobotTime;
|
||||
m_deltaTime = m_robotTime - m_lastRobotTime;
|
||||
m_frameNumber++;
|
||||
|
||||
if (m_startMatchTime != 0) {
|
||||
m_matchTime = m_currTime - m_startMatchTime;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this in both the auto and periodic inits
|
||||
*/
|
||||
public void startMatchTime() {
|
||||
if (m_startMatchTime == 0) {
|
||||
m_startMatchTime = m_currTime;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this in disabled init
|
||||
*/
|
||||
public void endMatchTime() {
|
||||
m_startMatchTime = 0;
|
||||
m_matchTime = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,27 +1,27 @@
|
||||
// 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;
|
||||
|
||||
/** Aarav's good units class (better than WPILib)
|
||||
* @author Aarav Shah */
|
||||
|
||||
public class RobotUnits {
|
||||
// constants
|
||||
|
||||
// angle conversions
|
||||
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
|
||||
|
||||
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
|
||||
|
||||
// falcon conversions
|
||||
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
|
||||
|
||||
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
|
||||
|
||||
// distance conversions
|
||||
public static double metersToFeet(final double meters) {return meters * 3.28084;}
|
||||
|
||||
public static double feetToMeters(final double feet) {return feet / 3.28084;}
|
||||
}
|
||||
// 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;
|
||||
|
||||
/** Aarav's good units class (better than WPILib)
|
||||
* @author Aarav Shah */
|
||||
|
||||
public class RobotUnits {
|
||||
// constants
|
||||
|
||||
// angle conversions
|
||||
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
|
||||
|
||||
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
|
||||
|
||||
// falcon conversions
|
||||
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
|
||||
|
||||
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
|
||||
|
||||
// distance conversions
|
||||
public static double metersToFeet(final double meters) {return meters * 3.28084;}
|
||||
|
||||
public static double feetToMeters(final double feet) {return feet / 3.28084;}
|
||||
}
|
||||
|
||||
@@ -1,12 +1,17 @@
|
||||
package frc4388.utility;
|
||||
|
||||
public class UtilityStructs {
|
||||
public static class TimedOutput {
|
||||
public double leftX = 0.0;
|
||||
public double leftY = 0.0;
|
||||
public double rightX = 0.0;
|
||||
public double rightY = 0.0;
|
||||
|
||||
public long timedOffset = 0;
|
||||
}
|
||||
}
|
||||
package frc4388.utility;
|
||||
|
||||
public class UtilityStructs {
|
||||
public static class TimedOutput {
|
||||
public double driverLeftX = 0.0;
|
||||
public double driverLeftY = 0.0;
|
||||
public double driverRightX = 0.0;
|
||||
public double driverRightY = 0.0;
|
||||
|
||||
public double operatorLeftX = 0.0;
|
||||
public double operatorLeftY = 0.0;
|
||||
public double operatorRightX = 0.0;
|
||||
public double operatorRightY = 0.0;
|
||||
|
||||
public long timedOffset = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,27 +1,27 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class DeadbandedXboxController extends XboxController {
|
||||
public DeadbandedXboxController(int port) { super(port); }
|
||||
|
||||
@Override public double getLeftX() { return getLeft().getX(); }
|
||||
@Override public double getLeftY() { return getLeft().getY(); }
|
||||
@Override public double getRightX() { return getRight().getX(); }
|
||||
@Override public double getRightY() { return getRight().getY(); }
|
||||
|
||||
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
|
||||
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
|
||||
|
||||
public static Translation2d skewToDeadzonedCircle(double x, double y) {
|
||||
Translation2d translation2d = new Translation2d(x, y);
|
||||
double magnitude = translation2d.getNorm();
|
||||
|
||||
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
|
||||
|
||||
return translation2d;
|
||||
}
|
||||
}
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class DeadbandedXboxController extends XboxController {
|
||||
public DeadbandedXboxController(int port) { super(port); }
|
||||
|
||||
@Override public double getLeftX() { return getLeft().getX(); }
|
||||
@Override public double getLeftY() { return getLeft().getY(); }
|
||||
@Override public double getRightX() { return getRight().getX(); }
|
||||
@Override public double getRightY() { return getRight().getY(); }
|
||||
|
||||
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
|
||||
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
|
||||
|
||||
public static Translation2d skewToDeadzonedCircle(double x, double y) {
|
||||
Translation2d translation2d = new Translation2d(x, y);
|
||||
double magnitude = translation2d.getNorm();
|
||||
|
||||
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
|
||||
|
||||
return translation2d;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,21 +1,21 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public interface IHandController {
|
||||
|
||||
public double getLeftXAxis();
|
||||
|
||||
public double getLeftYAxis();
|
||||
|
||||
public double getRightXAxis();
|
||||
|
||||
public double getRightYAxis();
|
||||
|
||||
public double getLeftTriggerAxis();
|
||||
|
||||
public double getRightTriggerAxis();
|
||||
|
||||
public int getDpadAngle();
|
||||
}
|
||||
package frc4388.utility.controller;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public interface IHandController {
|
||||
|
||||
public double getLeftXAxis();
|
||||
|
||||
public double getLeftYAxis();
|
||||
|
||||
public double getRightXAxis();
|
||||
|
||||
public double getRightYAxis();
|
||||
|
||||
public double getLeftTriggerAxis();
|
||||
|
||||
public double getRightTriggerAxis();
|
||||
|
||||
public int getDpadAngle();
|
||||
}
|
||||
|
||||
@@ -1,218 +1,218 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
|
||||
/**
|
||||
* This is a wrapper for the WPILib Joystick class that represents an XBox
|
||||
* controller.
|
||||
* @author frc1675
|
||||
*/
|
||||
public class XboxController implements IHandController
|
||||
{
|
||||
public static final int LEFT_X_AXIS = 0;
|
||||
public static final int LEFT_Y_AXIS = 1;
|
||||
public static final int LEFT_TRIGGER_AXIS = 2;
|
||||
public static final int RIGHT_TRIGGER_AXIS = 3;
|
||||
public static final int RIGHT_X_AXIS = 4;
|
||||
public static final int RIGHT_Y_AXIS = 5;
|
||||
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
|
||||
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
|
||||
|
||||
public static final int A_BUTTON = 1;
|
||||
public static final int B_BUTTON = 2;
|
||||
public static final int X_BUTTON = 3;
|
||||
public static final int Y_BUTTON = 4;
|
||||
public static final int LEFT_BUMPER_BUTTON = 5;
|
||||
public static final int RIGHT_BUMPER_BUTTON = 6;
|
||||
public static final int BACK_BUTTON = 7;
|
||||
public static final int START_BUTTON = 8;
|
||||
|
||||
public static final int LEFT_JOYSTICK_BUTTON = 9;
|
||||
public static final int RIGHT_JOYSTICK_BUTTON = 10;
|
||||
|
||||
private static final double LEFT_DPAD_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
|
||||
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
|
||||
private static final double TOP_DPAD_TOLERANCE = 0.9;
|
||||
|
||||
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
|
||||
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
|
||||
|
||||
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double DEADZONE = 0.1;
|
||||
|
||||
private Joystick m_stick;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public XboxController(int portNumber){
|
||||
m_stick = new Joystick(portNumber);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getJoyStick() {
|
||||
return m_stick;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the input falls within the deadzone.
|
||||
* @param input from an axis on the controller
|
||||
* @return true if input falls in deadzone, false if input falls outside deadzone
|
||||
*/
|
||||
private boolean inDeadZone(double input){
|
||||
return (Math.abs(input) < DEADZONE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates an input to have a deadzone around the 0 position
|
||||
* @param input from an axis on the controller
|
||||
* @return updated input
|
||||
*/
|
||||
private double getAxisWithDeadZoneCheck(double input){
|
||||
if(inDeadZone(input)){
|
||||
return 0.0;
|
||||
} else {
|
||||
return input;
|
||||
}
|
||||
}
|
||||
|
||||
public boolean getAButton(){
|
||||
return m_stick.getRawButton(A_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getXButton(){
|
||||
return m_stick.getRawButton(X_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBButton(){
|
||||
return m_stick.getRawButton(B_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getYButton(){
|
||||
return m_stick.getRawButton(Y_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBackButton(){
|
||||
return m_stick.getRawButton(BACK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getStartButton(){
|
||||
return m_stick.getRawButton(START_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftBumperButton(){
|
||||
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightBumperButton(){
|
||||
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftJoystickButton(){
|
||||
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightJoystickButton(){
|
||||
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public double getLeftXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getRightXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getRightYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
public double getRightTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle input from the dpad.
|
||||
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
|
||||
*/
|
||||
public int getDpadAngle() {
|
||||
return m_stick.getPOV(0);
|
||||
}
|
||||
|
||||
public boolean getDPadLeft(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadRight(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadTop(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadBottom(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftTrigger(){
|
||||
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightTrigger(){
|
||||
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisUpTrigger(){
|
||||
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisDownTrigger(){
|
||||
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisLeftTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisRightTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisUpTrigger(){
|
||||
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisDownTrigger(){
|
||||
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisLeftTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisRightTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
}
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
|
||||
/**
|
||||
* This is a wrapper for the WPILib Joystick class that represents an XBox
|
||||
* controller.
|
||||
* @author frc1675
|
||||
*/
|
||||
public class XboxController implements IHandController
|
||||
{
|
||||
public static final int LEFT_X_AXIS = 0;
|
||||
public static final int LEFT_Y_AXIS = 1;
|
||||
public static final int LEFT_TRIGGER_AXIS = 2;
|
||||
public static final int RIGHT_TRIGGER_AXIS = 3;
|
||||
public static final int RIGHT_X_AXIS = 4;
|
||||
public static final int RIGHT_Y_AXIS = 5;
|
||||
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
|
||||
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
|
||||
|
||||
public static final int A_BUTTON = 1;
|
||||
public static final int B_BUTTON = 2;
|
||||
public static final int X_BUTTON = 3;
|
||||
public static final int Y_BUTTON = 4;
|
||||
public static final int LEFT_BUMPER_BUTTON = 5;
|
||||
public static final int RIGHT_BUMPER_BUTTON = 6;
|
||||
public static final int BACK_BUTTON = 7;
|
||||
public static final int START_BUTTON = 8;
|
||||
|
||||
public static final int LEFT_JOYSTICK_BUTTON = 9;
|
||||
public static final int RIGHT_JOYSTICK_BUTTON = 10;
|
||||
|
||||
private static final double LEFT_DPAD_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
|
||||
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
|
||||
private static final double TOP_DPAD_TOLERANCE = 0.9;
|
||||
|
||||
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
|
||||
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
|
||||
|
||||
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double DEADZONE = 0.1;
|
||||
|
||||
private Joystick m_stick;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public XboxController(int portNumber){
|
||||
m_stick = new Joystick(portNumber);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getJoyStick() {
|
||||
return m_stick;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the input falls within the deadzone.
|
||||
* @param input from an axis on the controller
|
||||
* @return true if input falls in deadzone, false if input falls outside deadzone
|
||||
*/
|
||||
private boolean inDeadZone(double input){
|
||||
return (Math.abs(input) < DEADZONE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates an input to have a deadzone around the 0 position
|
||||
* @param input from an axis on the controller
|
||||
* @return updated input
|
||||
*/
|
||||
private double getAxisWithDeadZoneCheck(double input){
|
||||
if(inDeadZone(input)){
|
||||
return 0.0;
|
||||
} else {
|
||||
return input;
|
||||
}
|
||||
}
|
||||
|
||||
public boolean getAButton(){
|
||||
return m_stick.getRawButton(A_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getXButton(){
|
||||
return m_stick.getRawButton(X_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBButton(){
|
||||
return m_stick.getRawButton(B_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getYButton(){
|
||||
return m_stick.getRawButton(Y_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBackButton(){
|
||||
return m_stick.getRawButton(BACK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getStartButton(){
|
||||
return m_stick.getRawButton(START_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftBumperButton(){
|
||||
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightBumperButton(){
|
||||
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftJoystickButton(){
|
||||
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightJoystickButton(){
|
||||
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public double getLeftXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getRightXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getRightYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
public double getRightTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle input from the dpad.
|
||||
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
|
||||
*/
|
||||
public int getDpadAngle() {
|
||||
return m_stick.getPOV(0);
|
||||
}
|
||||
|
||||
public boolean getDPadLeft(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadRight(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadTop(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadBottom(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftTrigger(){
|
||||
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightTrigger(){
|
||||
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisUpTrigger(){
|
||||
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisDownTrigger(){
|
||||
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisLeftTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisRightTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisUpTrigger(){
|
||||
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisDownTrigger(){
|
||||
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisLeftTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisRightTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,68 +1,68 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.button.Button;
|
||||
|
||||
/**
|
||||
* Mapping for the Xbox controller triggers to allow triggers to be defined as
|
||||
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
|
||||
* exceeds a tolerance defined in {@link XboxController}.
|
||||
*/
|
||||
public class XboxTriggerButton extends Button {
|
||||
public static final int RIGHT_TRIGGER = 0;
|
||||
public static final int LEFT_TRIGGER = 1;
|
||||
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
|
||||
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
|
||||
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
|
||||
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
|
||||
public static final int LEFT_AXIS_UP_TRIGGER = 6;
|
||||
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
|
||||
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
|
||||
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
|
||||
|
||||
private XboxController m_controller;
|
||||
private int m_trigger;
|
||||
|
||||
/**
|
||||
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
|
||||
*/
|
||||
public XboxTriggerButton(XboxController controller, int trigger) {
|
||||
m_controller = controller;
|
||||
m_trigger = trigger;
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
// @Override
|
||||
public boolean get() {
|
||||
if (m_trigger == RIGHT_TRIGGER) {
|
||||
return m_controller.getRightTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_TRIGGER) {
|
||||
return m_controller.getLeftTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
|
||||
return m_controller.getRightAxisUpTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
|
||||
return m_controller.getRightAxisDownTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
|
||||
return m_controller.getRightAxisRightTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
|
||||
return m_controller.getRightAxisLeftTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
|
||||
return m_controller.getLeftAxisUpTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
|
||||
return m_controller.getLeftAxisDownTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
|
||||
return m_controller.getLeftAxisRightTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
|
||||
return m_controller.getLeftAxisLeftTrigger();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.button.Button;
|
||||
|
||||
/**
|
||||
* Mapping for the Xbox controller triggers to allow triggers to be defined as
|
||||
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
|
||||
* exceeds a tolerance defined in {@link XboxController}.
|
||||
*/
|
||||
public class XboxTriggerButton extends Button {
|
||||
public static final int RIGHT_TRIGGER = 0;
|
||||
public static final int LEFT_TRIGGER = 1;
|
||||
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
|
||||
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
|
||||
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
|
||||
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
|
||||
public static final int LEFT_AXIS_UP_TRIGGER = 6;
|
||||
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
|
||||
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
|
||||
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
|
||||
|
||||
private XboxController m_controller;
|
||||
private int m_trigger;
|
||||
|
||||
/**
|
||||
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
|
||||
*/
|
||||
public XboxTriggerButton(XboxController controller, int trigger) {
|
||||
m_controller = controller;
|
||||
m_trigger = trigger;
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
// @Override
|
||||
public boolean get() {
|
||||
if (m_trigger == RIGHT_TRIGGER) {
|
||||
return m_controller.getRightTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_TRIGGER) {
|
||||
return m_controller.getLeftTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
|
||||
return m_controller.getRightAxisUpTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
|
||||
return m_controller.getRightAxisDownTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
|
||||
return m_controller.getRightAxisRightTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
|
||||
return m_controller.getRightAxisLeftTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
|
||||
return m_controller.getLeftAxisUpTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
|
||||
return m_controller.getLeftAxisDownTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
|
||||
return m_controller.getLeftAxisRightTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
|
||||
return m_controller.getLeftAxisLeftTrigger();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,54 +1,54 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.mocks;
|
||||
|
||||
import com.ctre.phoenix.ErrorCode;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class MockPigeonIMU extends PigeonIMU {
|
||||
public int m_deviceNumber;
|
||||
public double currentYaw;
|
||||
public double currentPitch;
|
||||
public double currentRoll;
|
||||
|
||||
public MockPigeonIMU(int deviceNumber) {
|
||||
super(deviceNumber);
|
||||
m_deviceNumber = deviceNumber;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ErrorCode setYaw(double angleDeg) {
|
||||
currentYaw = angleDeg;
|
||||
return ErrorCode.OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @param currentPitch the Pitch to set
|
||||
*/
|
||||
public void setCurrentPitch(double currentPitch) {
|
||||
this.currentPitch = currentPitch;
|
||||
}
|
||||
|
||||
/**
|
||||
* @param currentRoll the Roll to set
|
||||
*/
|
||||
public void setCurrentRoll(double currentRoll) {
|
||||
this.currentRoll = currentRoll;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ErrorCode getYawPitchRoll(double[] ypr_deg) {
|
||||
ypr_deg[0] = currentYaw;
|
||||
ypr_deg[1] = currentPitch;
|
||||
ypr_deg[2] = currentRoll;
|
||||
return ErrorCode.OK;
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.mocks;
|
||||
|
||||
import com.ctre.phoenix.ErrorCode;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class MockPigeonIMU extends PigeonIMU {
|
||||
public int m_deviceNumber;
|
||||
public double currentYaw;
|
||||
public double currentPitch;
|
||||
public double currentRoll;
|
||||
|
||||
public MockPigeonIMU(int deviceNumber) {
|
||||
super(deviceNumber);
|
||||
m_deviceNumber = deviceNumber;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ErrorCode setYaw(double angleDeg) {
|
||||
currentYaw = angleDeg;
|
||||
return ErrorCode.OK;
|
||||
}
|
||||
|
||||
/**
|
||||
* @param currentPitch the Pitch to set
|
||||
*/
|
||||
public void setCurrentPitch(double currentPitch) {
|
||||
this.currentPitch = currentPitch;
|
||||
}
|
||||
|
||||
/**
|
||||
* @param currentRoll the Roll to set
|
||||
*/
|
||||
public void setCurrentRoll(double currentRoll) {
|
||||
this.currentRoll = currentRoll;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ErrorCode getYawPitchRoll(double[] ypr_deg) {
|
||||
ypr_deg[0] = currentYaw;
|
||||
ypr_deg[1] = currentPitch;
|
||||
ypr_deg[2] = currentRoll;
|
||||
return ErrorCode.OK;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,61 +1,61 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot.subsystems;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
// import static org.junit.Assert.assertEquals;
|
||||
// import static org.mockito.Mockito.mock;
|
||||
// import org.junit.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class LEDSubsystemTest {
|
||||
@Test
|
||||
public void testConstructor() {
|
||||
// Arrange
|
||||
Spark ledController = new Spark(0);
|
||||
|
||||
// Act
|
||||
LED led = new LED(ledController);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testPatterns() {
|
||||
// Arrange
|
||||
Spark ledController = new Spark(0);
|
||||
LED led = new LED(ledController);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.BLUE_BREATH);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.SOLID_BLACK);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.robot.subsystems;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
// import static org.junit.Assert.assertEquals;
|
||||
// import static org.mockito.Mockito.mock;
|
||||
// import org.junit.Test;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class LEDSubsystemTest {
|
||||
@Test
|
||||
public void testConstructor() {
|
||||
// Arrange
|
||||
Spark ledController = new Spark(0);
|
||||
|
||||
// Act
|
||||
LED led = new LED(ledController);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testPatterns() {
|
||||
// Arrange
|
||||
Spark ledController = new Spark(0);
|
||||
LED led = new LED(ledController);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.BLUE_BREATH);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.SOLID_BLACK);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
// package frc4388.utility;
|
||||
|
||||
// public class Delay {
|
||||
// void delaymillis(long millis) {
|
||||
// delay(millis)
|
||||
|
||||
// }
|
||||
// }
|
||||
@@ -1,185 +1,185 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
// import static org.junit.Assert.*;
|
||||
// import static org.mockito.Mockito.*;
|
||||
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
import frc4388.mocks.MockPigeonIMU;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class RobotGyroUtilityTest {
|
||||
// TODO UNTESTED: most functions for NavX
|
||||
|
||||
private RobotGyro gyroPigeon;
|
||||
private RobotGyro gyroNavX;
|
||||
|
||||
@Test
|
||||
public void testConstructor() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
AHRS navX = new AHRS();
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
gyroNavX = new RobotGyro(navX);
|
||||
|
||||
// Assert
|
||||
assertEquals(true, gyroPigeon.m_isGyroAPigeon);
|
||||
assertEquals(pigeon, gyroPigeon.getPigeon());
|
||||
assertEquals(null, gyroPigeon.getNavX());
|
||||
assertEquals(false, gyroNavX.m_isGyroAPigeon);
|
||||
assertEquals(navX, gyroNavX.getNavX());
|
||||
assertEquals(null, gyroNavX.getPigeon());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testHeadingPigeon() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
|
||||
// Act & Assert
|
||||
assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
|
||||
assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
|
||||
assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
|
||||
assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
|
||||
assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
|
||||
assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
|
||||
assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
|
||||
assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
|
||||
assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testYawPitchRollPigeon() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getAngle(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setYaw(40);
|
||||
|
||||
// Assert
|
||||
assertEquals(40, gyroPigeon.getAngle(), 0.0001);
|
||||
|
||||
// Act
|
||||
gyroPigeon.reset();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getAngle(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setYaw(-1457);
|
||||
pigeon.setCurrentPitch(100);
|
||||
pigeon.setCurrentRoll(100);
|
||||
|
||||
// Assert
|
||||
assertEquals(-1457, gyroPigeon.getAngle(), 0.0001);
|
||||
assertEquals(90, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(90, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(45);
|
||||
pigeon.setCurrentRoll(45);
|
||||
|
||||
// Assert
|
||||
assertEquals(45, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(45, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(0);
|
||||
pigeon.setCurrentRoll(0);
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(0, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(-60);
|
||||
pigeon.setCurrentRoll(-60);
|
||||
|
||||
// Assert
|
||||
assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(-90);
|
||||
pigeon.setCurrentRoll(-90);
|
||||
|
||||
// Assert
|
||||
assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(-100);
|
||||
pigeon.setCurrentRoll(-100);
|
||||
|
||||
// Assert
|
||||
assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRatesPigeon() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
RobotTime robotTime = RobotTime.getInstance();
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 5;
|
||||
pigeon.setYaw(0);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getRate(), 1);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 5;
|
||||
pigeon.setYaw(90);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(18000, gyroPigeon.getRate(), 0.001);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 5;
|
||||
pigeon.setYaw(90);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getRate(), 0.001);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 3;
|
||||
pigeon.setYaw(-30);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(-40000, gyroPigeon.getRate(), 0.001);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 6;
|
||||
pigeon.setYaw(690);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(120000, gyroPigeon.getRate(), 0.001);
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
// import static org.junit.Assert.*;
|
||||
// import static org.mockito.Mockito.*;
|
||||
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
import frc4388.mocks.MockPigeonIMU;
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class RobotGyroUtilityTest {
|
||||
// TODO UNTESTED: most functions for NavX
|
||||
|
||||
private RobotGyro gyroPigeon;
|
||||
private RobotGyro gyroNavX;
|
||||
|
||||
@Test
|
||||
public void testConstructor() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
AHRS navX = new AHRS();
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
gyroNavX = new RobotGyro(navX);
|
||||
|
||||
// Assert
|
||||
assertEquals(true, gyroPigeon.m_isGyroAPigeon);
|
||||
assertEquals(pigeon, gyroPigeon.getPigeon());
|
||||
assertEquals(null, gyroPigeon.getNavX());
|
||||
assertEquals(false, gyroNavX.m_isGyroAPigeon);
|
||||
assertEquals(navX, gyroNavX.getNavX());
|
||||
assertEquals(null, gyroNavX.getPigeon());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testHeadingPigeon() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
|
||||
// Act & Assert
|
||||
assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
|
||||
assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
|
||||
assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
|
||||
assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
|
||||
assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
|
||||
assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
|
||||
assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
|
||||
assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
|
||||
assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testYawPitchRollPigeon() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getAngle(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setYaw(40);
|
||||
|
||||
// Assert
|
||||
assertEquals(40, gyroPigeon.getAngle(), 0.0001);
|
||||
|
||||
// Act
|
||||
gyroPigeon.reset();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getAngle(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setYaw(-1457);
|
||||
pigeon.setCurrentPitch(100);
|
||||
pigeon.setCurrentRoll(100);
|
||||
|
||||
// Assert
|
||||
assertEquals(-1457, gyroPigeon.getAngle(), 0.0001);
|
||||
assertEquals(90, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(90, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(45);
|
||||
pigeon.setCurrentRoll(45);
|
||||
|
||||
// Assert
|
||||
assertEquals(45, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(45, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(0);
|
||||
pigeon.setCurrentRoll(0);
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(0, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(-60);
|
||||
pigeon.setCurrentRoll(-60);
|
||||
|
||||
// Assert
|
||||
assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(-90);
|
||||
pigeon.setCurrentRoll(-90);
|
||||
|
||||
// Assert
|
||||
assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
|
||||
|
||||
// Act
|
||||
pigeon.setCurrentPitch(-100);
|
||||
pigeon.setCurrentRoll(-100);
|
||||
|
||||
// Assert
|
||||
assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
|
||||
assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testRatesPigeon() {
|
||||
// Arrange
|
||||
MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
|
||||
gyroPigeon = new RobotGyro(pigeon);
|
||||
RobotTime robotTime = RobotTime.getInstance();
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 5;
|
||||
pigeon.setYaw(0);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getRate(), 1);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 5;
|
||||
pigeon.setYaw(90);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(18000, gyroPigeon.getRate(), 0.001);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 5;
|
||||
pigeon.setYaw(90);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, gyroPigeon.getRate(), 0.001);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 3;
|
||||
pigeon.setYaw(-30);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(-40000, gyroPigeon.getRate(), 0.001);
|
||||
|
||||
// Act
|
||||
robotTime.m_deltaTime = 6;
|
||||
pigeon.setYaw(690);
|
||||
gyroPigeon.updatePigeonDeltas();
|
||||
|
||||
// Assert
|
||||
assertEquals(120000, gyroPigeon.getRate(), 0.001);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,105 +1,105 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
// import static org.junit.Assert.*;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class RobotTimeUtilityTest {
|
||||
RobotTime robotTime = RobotTime.getInstance();
|
||||
|
||||
@Test
|
||||
public void testUpdateTimes() {
|
||||
// Arrange
|
||||
long lastTime;
|
||||
robotTime.m_deltaTime = 0;
|
||||
robotTime.m_robotTime = 0;
|
||||
robotTime.m_lastRobotTime = 0;
|
||||
robotTime.m_frameNumber = 0;
|
||||
robotTime.endMatchTime();
|
||||
robotTime.m_lastMatchTime = 0;
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_deltaTime);
|
||||
assertEquals(0, robotTime.m_robotTime);
|
||||
assertEquals(0, robotTime.m_lastRobotTime);
|
||||
assertEquals(0, robotTime.m_frameNumber);
|
||||
lastTime = robotTime.m_robotTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(true, robotTime.m_deltaTime > 0);
|
||||
assertEquals(true, robotTime.m_robotTime > 0);
|
||||
assertEquals(lastTime, robotTime.m_lastRobotTime);
|
||||
assertEquals(1, robotTime.m_frameNumber);
|
||||
lastTime = robotTime.m_robotTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(true, robotTime.m_deltaTime > 0);
|
||||
assertEquals(true, robotTime.m_robotTime > 0);
|
||||
assertEquals(lastTime, robotTime.m_lastRobotTime);
|
||||
assertEquals(2, robotTime.m_frameNumber);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testMatchTime() {
|
||||
// Arrange
|
||||
long lastTime;
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_matchTime);
|
||||
assertEquals(0, robotTime.m_lastMatchTime);
|
||||
lastTime = robotTime.m_matchTime;
|
||||
|
||||
// Act
|
||||
robotTime.startMatchTime();
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(true, robotTime.m_matchTime > 0);
|
||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
||||
lastTime = robotTime.m_matchTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
robotTime.endMatchTime();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_matchTime);
|
||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
||||
lastTime = robotTime.m_matchTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_matchTime);
|
||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
||||
}
|
||||
|
||||
private void wait(int millis) {
|
||||
try {
|
||||
Thread.sleep(millis);
|
||||
} catch (Exception e) {}
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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 org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
// import static org.junit.Assert.*;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class RobotTimeUtilityTest {
|
||||
RobotTime robotTime = RobotTime.getInstance();
|
||||
|
||||
@Test
|
||||
public void testUpdateTimes() {
|
||||
// Arrange
|
||||
long lastTime;
|
||||
robotTime.m_deltaTime = 0;
|
||||
robotTime.m_robotTime = 0;
|
||||
robotTime.m_lastRobotTime = 0;
|
||||
robotTime.m_frameNumber = 0;
|
||||
robotTime.endMatchTime();
|
||||
robotTime.m_lastMatchTime = 0;
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_deltaTime);
|
||||
assertEquals(0, robotTime.m_robotTime);
|
||||
assertEquals(0, robotTime.m_lastRobotTime);
|
||||
assertEquals(0, robotTime.m_frameNumber);
|
||||
lastTime = robotTime.m_robotTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(true, robotTime.m_deltaTime > 0);
|
||||
assertEquals(true, robotTime.m_robotTime > 0);
|
||||
assertEquals(lastTime, robotTime.m_lastRobotTime);
|
||||
assertEquals(1, robotTime.m_frameNumber);
|
||||
lastTime = robotTime.m_robotTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(true, robotTime.m_deltaTime > 0);
|
||||
assertEquals(true, robotTime.m_robotTime > 0);
|
||||
assertEquals(lastTime, robotTime.m_lastRobotTime);
|
||||
assertEquals(2, robotTime.m_frameNumber);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testMatchTime() {
|
||||
// Arrange
|
||||
long lastTime;
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_matchTime);
|
||||
assertEquals(0, robotTime.m_lastMatchTime);
|
||||
lastTime = robotTime.m_matchTime;
|
||||
|
||||
// Act
|
||||
robotTime.startMatchTime();
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(true, robotTime.m_matchTime > 0);
|
||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
||||
lastTime = robotTime.m_matchTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
robotTime.endMatchTime();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_matchTime);
|
||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
||||
lastTime = robotTime.m_matchTime;
|
||||
|
||||
// Act
|
||||
wait(1);
|
||||
robotTime.updateTimes();
|
||||
|
||||
// Assert
|
||||
assertEquals(0, robotTime.m_matchTime);
|
||||
assertEquals(lastTime, robotTime.m_lastMatchTime);
|
||||
}
|
||||
|
||||
private void wait(int millis) {
|
||||
try {
|
||||
Thread.sleep(millis);
|
||||
} catch (Exception e) {}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
Revised Autosystem:
|
||||
- Autoplayback
|
||||
- Fix Arm Respool
|
||||
- Via Commands
|
||||
- Arm
|
||||
- Sequential Command System
|
||||
- Manual Auto Creation
|
||||
- Shuffleboard IO
|
||||
Revised Autosystem:
|
||||
- Autoplayback
|
||||
\ Fix Arm Respool
|
||||
x Via Commands
|
||||
- Arm
|
||||
- Sequential Command System
|
||||
- Manual Auto Creation
|
||||
- Shuffleboard IO
|
||||
|
||||
+422
-422
@@ -1,423 +1,423 @@
|
||||
{
|
||||
"fileName": "Phoenix.json",
|
||||
"name": "CTRE-Phoenix (v5)",
|
||||
"version": "5.30.2",
|
||||
"frcYear": 2023,
|
||||
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
|
||||
"mavenUrls": [
|
||||
"https://maven.ctr-electronics.com/release/"
|
||||
],
|
||||
"jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "api-java",
|
||||
"version": "5.30.2"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "wpiapi-java",
|
||||
"version": "5.30.2"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "cci",
|
||||
"version": "5.30.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "cci-sim",
|
||||
"version": "5.30.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro",
|
||||
"artifactId": "tools",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonFX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "wpiapi-cpp",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_Phoenix_WPI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "api-cpp",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_Phoenix",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "cci",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_PhoenixCCI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro",
|
||||
"artifactId": "tools",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_PhoenixTools",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "wpiapi-cpp-sim",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_Phoenix_WPISim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "api-cpp-sim",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_PhoenixSim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "cci-sim",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_PhoenixCCISim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_PhoenixTools_Sim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimTalonSRX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonFX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimTalonFX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimVictorSPX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimPigeonIMU",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimCANCoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimProTalonFX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimProCANcoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimProPigeon2",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
}
|
||||
]
|
||||
{
|
||||
"fileName": "Phoenix.json",
|
||||
"name": "CTRE-Phoenix (v5)",
|
||||
"version": "5.30.2",
|
||||
"frcYear": 2023,
|
||||
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
|
||||
"mavenUrls": [
|
||||
"https://maven.ctr-electronics.com/release/"
|
||||
],
|
||||
"jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "api-java",
|
||||
"version": "5.30.2"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "wpiapi-java",
|
||||
"version": "5.30.2"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "cci",
|
||||
"version": "5.30.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "cci-sim",
|
||||
"version": "5.30.2",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro",
|
||||
"artifactId": "tools",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonFX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "23.0.1",
|
||||
"isJar": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "wpiapi-cpp",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_Phoenix_WPI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "api-cpp",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_Phoenix",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix",
|
||||
"artifactId": "cci",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_PhoenixCCI",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro",
|
||||
"artifactId": "tools",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_PhoenixTools",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"linuxathena"
|
||||
],
|
||||
"simMode": "hwsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "wpiapi-cpp-sim",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_Phoenix_WPISim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "api-cpp-sim",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_PhoenixSim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenix.sim",
|
||||
"artifactId": "cci-sim",
|
||||
"version": "5.30.2",
|
||||
"libName": "CTRE_PhoenixCCISim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "tools-sim",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_PhoenixTools_Sim",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonSRX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimTalonSRX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simTalonFX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimTalonFX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simVictorSPX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimVictorSPX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simPigeonIMU",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimPigeonIMU",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simCANCoder",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimCANCoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProTalonFX",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimProTalonFX",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProCANcoder",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimProCANcoder",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
},
|
||||
{
|
||||
"groupId": "com.ctre.phoenixpro.sim",
|
||||
"artifactId": "simProPigeon2",
|
||||
"version": "23.0.1",
|
||||
"libName": "CTRE_SimProPigeon2",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
],
|
||||
"simMode": "swsim"
|
||||
}
|
||||
]
|
||||
}
|
||||
+72
-72
@@ -1,73 +1,73 @@
|
||||
{
|
||||
"fileName": "REVLib.json",
|
||||
"name": "REVLib",
|
||||
"version": "2023.1.3",
|
||||
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
|
||||
"mavenUrls": [
|
||||
"https://maven.revrobotics.com/"
|
||||
],
|
||||
"jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-java",
|
||||
"version": "2023.1.3"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2023.1.3",
|
||||
"skipInvalidPlatforms": true,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-cpp",
|
||||
"version": "2023.1.3",
|
||||
"libName": "REVLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
},
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2023.1.3",
|
||||
"libName": "REVLibDriver",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
]
|
||||
{
|
||||
"fileName": "REVLib.json",
|
||||
"name": "REVLib",
|
||||
"version": "2023.1.3",
|
||||
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
|
||||
"mavenUrls": [
|
||||
"https://maven.revrobotics.com/"
|
||||
],
|
||||
"jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-java",
|
||||
"version": "2023.1.3"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2023.1.3",
|
||||
"skipInvalidPlatforms": true,
|
||||
"isJar": false,
|
||||
"validPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-cpp",
|
||||
"version": "2023.1.3",
|
||||
"libName": "REVLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
},
|
||||
{
|
||||
"groupId": "com.revrobotics.frc",
|
||||
"artifactId": "REVLib-driver",
|
||||
"version": "2023.1.3",
|
||||
"libName": "REVLibDriver",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxarm64",
|
||||
"linuxx86-64",
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -1,37 +1,37 @@
|
||||
{
|
||||
"fileName": "WPILibNewCommands.json",
|
||||
"name": "WPILib-New-Commands",
|
||||
"version": "1.0.0",
|
||||
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
|
||||
"mavenUrls": [],
|
||||
"jsonUrl": "",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "edu.wpi.first.wpilibNewCommands",
|
||||
"artifactId": "wpilibNewCommands-java",
|
||||
"version": "wpilib"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "edu.wpi.first.wpilibNewCommands",
|
||||
"artifactId": "wpilibNewCommands-cpp",
|
||||
"version": "wpilib",
|
||||
"libName": "wpilibNewCommands",
|
||||
"headerClassifier": "headers",
|
||||
"sourcesClassifier": "sources",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"linuxarm64",
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
{
|
||||
"fileName": "WPILibNewCommands.json",
|
||||
"name": "WPILib-New-Commands",
|
||||
"version": "1.0.0",
|
||||
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
|
||||
"mavenUrls": [],
|
||||
"jsonUrl": "",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "edu.wpi.first.wpilibNewCommands",
|
||||
"artifactId": "wpilibNewCommands-java",
|
||||
"version": "wpilib"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "edu.wpi.first.wpilibNewCommands",
|
||||
"artifactId": "wpilibNewCommands-cpp",
|
||||
"version": "wpilib",
|
||||
"libName": "wpilibNewCommands",
|
||||
"headerClassifier": "headers",
|
||||
"sourcesClassifier": "sources",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxarm32",
|
||||
"linuxarm64",
|
||||
"windowsx86-64",
|
||||
"windowsx86",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
+34
-34
@@ -1,35 +1,35 @@
|
||||
{
|
||||
"fileName": "navx_frc.json",
|
||||
"name": "KauaiLabs_navX_FRC",
|
||||
"version": "4.0.447",
|
||||
"uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
|
||||
"mavenUrls": [
|
||||
"https://repo1.maven.org/maven2/"
|
||||
],
|
||||
"jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.kauailabs.navx.frc",
|
||||
"artifactId": "navx-java",
|
||||
"version": "4.0.447"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.kauailabs.navx.frc",
|
||||
"artifactId": "navx-cpp",
|
||||
"version": "4.0.447",
|
||||
"headerClassifier": "headers",
|
||||
"sourcesClassifier": "sources",
|
||||
"sharedLibrary": false,
|
||||
"libName": "navx_frc",
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"windowsx86-64"
|
||||
]
|
||||
}
|
||||
]
|
||||
{
|
||||
"fileName": "navx_frc.json",
|
||||
"name": "KauaiLabs_navX_FRC",
|
||||
"version": "4.0.447",
|
||||
"uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
|
||||
"mavenUrls": [
|
||||
"https://repo1.maven.org/maven2/"
|
||||
],
|
||||
"jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.kauailabs.navx.frc",
|
||||
"artifactId": "navx-java",
|
||||
"version": "4.0.447"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.kauailabs.navx.frc",
|
||||
"artifactId": "navx-cpp",
|
||||
"version": "4.0.447",
|
||||
"headerClassifier": "headers",
|
||||
"sourcesClassifier": "sources",
|
||||
"sharedLibrary": false,
|
||||
"libName": "navx_frc",
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"linuxathena",
|
||||
"linuxraspbian",
|
||||
"windowsx86-64"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
+40
-40
@@ -1,41 +1,41 @@
|
||||
{
|
||||
"fileName": "photonlib.json",
|
||||
"name": "photonlib",
|
||||
"version": "v2023.4.2",
|
||||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
|
||||
"mavenUrls": [
|
||||
"https://maven.photonvision.org/repository/internal",
|
||||
"https://maven.photonvision.org/repository/snapshots"
|
||||
],
|
||||
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonLib-cpp",
|
||||
"version": "v2023.4.2",
|
||||
"libName": "Photon",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonLib-java",
|
||||
"version": "v2023.4.2"
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonTargeting-java",
|
||||
"version": "v2023.4.2"
|
||||
}
|
||||
]
|
||||
{
|
||||
"fileName": "photonlib.json",
|
||||
"name": "photonlib",
|
||||
"version": "v2023.4.2",
|
||||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
|
||||
"mavenUrls": [
|
||||
"https://maven.photonvision.org/repository/internal",
|
||||
"https://maven.photonvision.org/repository/snapshots"
|
||||
],
|
||||
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonLib-cpp",
|
||||
"version": "v2023.4.2",
|
||||
"libName": "Photon",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxuniversal"
|
||||
]
|
||||
}
|
||||
],
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonLib-java",
|
||||
"version": "v2023.4.2"
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonTargeting-java",
|
||||
"version": "v2023.4.2"
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user