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

Cleaned up Math, added unit testing to AimToCenter

Browse Source
This commit is contained in:
Ryan Manley
2022-02-26 13:48:50 -07:00
parent 31346ff646
commit 350cf33dd0
9 changed files with 262 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simgui-ds.json
+1 -2
View File
@@ -91,10 +91,9 @@
], ],
"robotJoysticks": [ "robotJoysticks": [
{ {
"useGamepad": true "guid": "78696e70757401000000000000000000"
}, },
{ {
"guid": "78696e70757401000000000000000000",
"useGamepad": true "useGamepad": true
} }
] ]
simgui.json
+8
View File
@@ -30,5 +30,13 @@
"/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
"/LiveWindow/Vision": "Subsystem" "/LiveWindow/Vision": "Subsystem"
} }
},
"NetworkTables": {
"SmartDashboard": {
"Recording": {
"open": true
},
"open": true
}
} }
} }
src/main/deploy/Distances.csv
View File
src/main/java/frc4388/robot/Constants.java
+22 -22
View File
@@ -31,28 +31,28 @@ public final class Constants {
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5; public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
public static final double MAX_SPEED_FEET_PER_SEC = 16; public static final double MAX_SPEED_FEET_PER_SEC = 16;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
// public static final int LEFT_FRONT_STEER_CAN_ID = 2; public static final int LEFT_FRONT_STEER_CAN_ID = 2;
// public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
// public static final int RIGHT_FRONT_STEER_CAN_ID = 4; public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
// public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
// public static final int LEFT_BACK_STEER_CAN_ID = 6; public static final int LEFT_BACK_STEER_CAN_ID = 6;
// public static final int LEFT_BACK_WHEEL_CAN_ID = 7; public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
// public static final int RIGHT_BACK_STEER_CAN_ID = 8; public static final int RIGHT_BACK_STEER_CAN_ID = 8;
// public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
// public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
// public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
// public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
// public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
// ofsets are in degrees // ofsets are in degrees
//ofsets are in degrees //ofsets are in degrees
// public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
// public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531; public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594; public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; // public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; // public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; // public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
// swerve PID constants // swerve PID constants
public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_SLOT_IDX = 0;
@@ -110,8 +110,8 @@ public final class Constants {
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
public static final double SHOOTER_TURRET_MIN = -1.0; public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double DEADZONE_LEFT = -20.0; public static final double DEADZONE_LEFT = 0.0;
public static final double DEADZONE_RIGHT = 0.0; public static final double DEADZONE_RIGHT = 340.0;
public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later//
public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"//
src/main/java/frc4388/robot/Robot.java
+1
View File
@@ -30,6 +30,7 @@ public class Robot extends TimedRobot {
public void robotInit() { public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our // Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard. // autonomous chooser on the dashboard.
//AimToCenterTest.RunAll();
m_robotContainer = new RobotContainer(); m_robotContainer = new RobotContainer();
} }
src/main/java/frc4388/robot/RobotMap.java
+101 -112
View File
@@ -7,6 +7,7 @@ package frc4388.robot;
import javax.swing.text.WrappedPlainView; import javax.swing.text.WrappedPlainView;
import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
@@ -37,124 +38,112 @@ public class RobotMap {
} }
/* Swerve Subsystem */ /* Swerve Subsystem */
public final WPI_TalonFX leftFrontSteerMotor = null; // public final WPI_TalonFX leftFrontSteerMotor = null;
public final WPI_TalonFX leftFrontWheelMotor = null; // public final WPI_TalonFX leftFrontWheelMotor = null;
public final WPI_TalonFX rightFrontSteerMotor = null; // public final WPI_TalonFX rightFrontSteerMotor = null;
public final WPI_TalonFX rightFrontWheelMotor = null; // public final WPI_TalonFX rightFrontWheelMotor = null;
public final WPI_TalonFX leftBackSteerMotor = null; // public final WPI_TalonFX leftBackSteerMotor = null;
public final WPI_TalonFX leftBackWheelMotor = null; // public final WPI_TalonFX leftBackWheelMotor = null;
public final WPI_TalonFX rightBackSteerMotor = null; // public final WPI_TalonFX rightBackSteerMotor = null;
public final WPI_TalonFX rightBackWheelMotor = null; // public final WPI_TalonFX rightBackWheelMotor = null;
public final CANCoder leftFrontEncoder = null; // public final CANCoder leftFrontEncoder = null;
public final CANCoder rightFrontEncoder = null; // public final CANCoder rightFrontEncoder = null;
public final CANCoder leftBackEncoder = null; // public final CANCoder leftBackEncoder = null;
public final CANCoder rightBackEncoder = null; // public final CANCoder rightBackEncoder = null;
// public final WPI_TalonFX leftFrontSteerMotor = new
// WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
// public final WPI_TalonFX leftFrontWheelMotor = new
// WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
// public final WPI_TalonFX rightFrontSteerMotor = new
// WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
// public final WPI_TalonFX rightFrontWheelMotor = new
// WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
// public final WPI_TalonFX leftBackSteerMotor = new
// WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
// public final WPI_TalonFX leftBackWheelMotor = new
// WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
// public final WPI_TalonFX rightBackSteerMotor = new
// WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
// public final WPI_TalonFX rightBackWheelMotor = new
// WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
// public final CANCoder leftFrontEncoder = new
// CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
// public final CANCoder rightFrontEncoder = new
// CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
// public final CANCoder leftBackEncoder = new
// CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
// public final CANCoder rightBackEncoder = new
// CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
// void configureSwerveMotorControllers() { public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
// leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
// rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
// leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
// rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
// leftFrontSteerMotor.configFactoryDefault(); void configureSwerveMotorControllers() {
// leftFrontWheelMotor.configFactoryDefault(); leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
// rightFrontSteerMotor.configFactoryDefault(); rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
// rightFrontWheelMotor.configFactoryDefault(); leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
// leftBackSteerMotor.configFactoryDefault(); rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
// leftBackWheelMotor.configFactoryDefault();
// rightBackSteerMotor.configFactoryDefault();
// rightBackWheelMotor.configFactoryDefault();
// leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, leftFrontSteerMotor.configFactoryDefault();
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontWheelMotor.configFactoryDefault();
// leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, rightFrontSteerMotor.configFactoryDefault();
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontWheelMotor.configFactoryDefault();
// rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, leftBackSteerMotor.configFactoryDefault();
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configFactoryDefault();
// rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, rightBackSteerMotor.configFactoryDefault();
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configFactoryDefault();
// leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// // config cancoder as remote encoder for swerve steer motors leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
// } SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// config cancoder as remote encoder for swerve steer motors
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
// Shooter Config // Shooter Config
/* Boom Boom Subsystem */ /* Boom Boom Subsystem */
src/main/java/frc4388/robot/commands/AimToCenter.java
+13 -20
View File
@@ -17,6 +17,7 @@ public class AimToCenter extends CommandBase {
// use odometry to find x and y later // use odometry to find x and y later
double x = 0; double x = 0;
double y = 0; double y = 0;
double angle = 0;
double m_targetAngle; double m_targetAngle;
// public static Gains m_aimGains; // public static Gains m_aimGains;
@@ -31,34 +32,26 @@ public class AimToCenter extends CommandBase {
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {
x = 0;
y = 0;
} }
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
if (x > 0) { m_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle());
m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle();
}
else if (x < 0) {
m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle();
}
else if (x == 0 && y > 0) {
m_targetAngle = 270 - m_drive.gyro.getAngle();
}
else if (x == 0 && y < 0) {
m_targetAngle = 90 - m_drive.gyro.getAngle();
}
m_turret.runshooterRotatePID(m_targetAngle); m_turret.runshooterRotatePID(m_targetAngle);
} }
public boolean isDeadzone() { public static double angleToCenter(double x, double y, double gyro) {
if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) { double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
return true; return(angle);
} else { //m_turret.runshooterRotatePID(m_targetAngle);
return false; }
}
public static boolean isDeadzone(double angle) {
if ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT)) return true;
else return false;
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
src/test/java/frc4388/commands/AimToCenterTest.java
+113
View File
@@ -0,0 +1,113 @@
package frc4388.commands;
import org.junit.Test;
import frc4388.robot.commands.AimToCenter;
import org.junit.Assert;
public class AimToCenterTest {
private static final double DELTA = 1e-15;
@Test
public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
boolean output;
//20 deg
output = AimToCenter.isDeadzone(20.);
Assert.assertFalse(output);
//-10 deg
output = AimToCenter.isDeadzone(-10.);
Assert.assertTrue(output);
//-1 deg
output = AimToCenter.isDeadzone(-1.);
Assert.assertTrue(output);
//341 deg
output = AimToCenter.isDeadzone(341.);
Assert.assertTrue(output);
//340 deg
output = AimToCenter.isDeadzone(340.);
Assert.assertFalse(output);
//0 deg
output = AimToCenter.isDeadzone(0.);
Assert.assertFalse(output);
//200 deg
output = AimToCenter.isDeadzone(200.);
Assert.assertFalse(output);
//2000000 deg
output = AimToCenter.isDeadzone(2000000.);
Assert.assertTrue(output);
//NaN deg
output = AimToCenter.isDeadzone(Double.NaN);
Assert.assertFalse(output);
}
@Test
public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() {
double actual;
double expected;
//(5,5) Gyro = 0 deg
actual = AimToCenter.angleToCenter(5., 5., 0.);
expected = 180. + 45.;
Assert.assertEquals(expected, actual, DELTA);
//(-5,5) Gyro = 0 deg
actual = AimToCenter.angleToCenter(-5.0, 5., 0.);
expected = 180. + 90. + 45.;
Assert.assertEquals(expected, actual, DELTA);
//(-5,-5) Gyro = 0 deg
actual = AimToCenter.angleToCenter(-5.0, -5., 0.);
expected = 45.;
Assert.assertEquals(expected, actual, DELTA);
//(5,-5) Gyro = 0 deg
actual = AimToCenter.angleToCenter(5., -5., 0.);
expected = 90. + 45.;
Assert.assertEquals(expected, actual, DELTA);
//(0,0) Gyro = 0 deg
actual = AimToCenter.angleToCenter(0., 0., 0.);
Assert.assertNotNull(actual);
//(5,5) Gyro = 180 deg
actual = AimToCenter.angleToCenter(5., 5., 180.);
expected = 45.;
Assert.assertEquals(expected, actual, DELTA);
//(100,100) Gyro = 90 deg
actual = AimToCenter.angleToCenter(100., 100., 90.);
expected = 90. + 45.;
Assert.assertEquals(expected, actual, DELTA);
//(null,5) Gyro = 0 deg
actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.);
expected = Double.NaN;
Assert.assertEquals(expected, actual, DELTA);
//(null,null) Gyro = 0 deg
actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.);
expected = Double.NaN;
Assert.assertEquals(expected, actual, DELTA);
//(null,null) Gyro = null deg
actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN);
expected = Double.NaN;
Assert.assertEquals(expected, actual, DELTA);
//(5,5) Gyro = -20 deg
actual = AimToCenter.angleToCenter(5., -5., -45.);
expected = 180.;
Assert.assertEquals(expected, actual, DELTA);
}
}
src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
+3 -9
View File
@@ -17,23 +17,17 @@ import frc4388.utility.LEDPatterns;
* Add your docs here. * Add your docs here.
*/ */
public class LEDSubsystemTest { public class LEDSubsystemTest {
// Arrange
Spark ledController = mock(Spark.class);
LED led = new LED(ledController);
@Test @Test
public void testConstructor() { public void testConstructor() {
// Arrange
Spark ledController = mock(Spark.class);
// Act
LED led = new LED(ledController);
// Assert // Assert
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
} }
@Test @Test
public void testPatterns() { public void testPatterns() {
// Arrange
Spark ledController = mock(Spark.class);
LED led = new LED(ledController);
// Act // Act
led.setPattern(LEDPatterns.RAINBOW_RAINBOW); led.setPattern(LEDPatterns.RAINBOW_RAINBOW);