mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Cleaned up Math, added unit testing to AimToCenter
This commit is contained in:
Ryan Manley
@@ -31,28 +31,28 @@ public final class Constants {
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
|
||||
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 int LEFT_FRONT_STEER_CAN_ID = 2;
|
||||
// public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
|
||||
// public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
|
||||
// public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
|
||||
// public static final int LEFT_BACK_STEER_CAN_ID = 6;
|
||||
// public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
|
||||
// public static final int RIGHT_BACK_STEER_CAN_ID = 8;
|
||||
// public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
|
||||
// public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
|
||||
// public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
|
||||
// public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
|
||||
// public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
|
||||
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
|
||||
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
|
||||
public static final int LEFT_BACK_STEER_CAN_ID = 6;
|
||||
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
|
||||
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
|
||||
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
|
||||
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
// ofsets are in degrees
|
||||
//ofsets are in degrees
|
||||
// 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 LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
|
||||
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
|
||||
public static final double LEFT_FRONT_ENCODER_OFFSET = 180.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 RIGHT_BACK_ENCODER_OFFSET = 0.0;
|
||||
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 LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
|
||||
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
|
||||
// public static final double LEFT_FRONT_ENCODER_OFFSET = 180.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 RIGHT_BACK_ENCODER_OFFSET = 0.0;
|
||||
|
||||
// swerve PID constants
|
||||
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 double SHOOTER_TURRET_MIN = -1.0;
|
||||
|
||||
public static final double DEADZONE_LEFT = -20.0;
|
||||
public static final double DEADZONE_RIGHT = 0.0;
|
||||
public static final double DEADZONE_LEFT = 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_FOLLOWER_ID = 0; //"//
|
||||
|
||||
@@ -30,6 +30,7 @@ public class Robot extends TimedRobot {
|
||||
public void robotInit() {
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
//AimToCenterTest.RunAll();
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
|
||||
@@ -7,6 +7,7 @@ package frc4388.robot;
|
||||
import javax.swing.text.WrappedPlainView;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
@@ -37,124 +38,112 @@ public class RobotMap {
|
||||
}
|
||||
|
||||
/* Swerve Subsystem */
|
||||
public final WPI_TalonFX leftFrontSteerMotor = null;
|
||||
public final WPI_TalonFX leftFrontWheelMotor = null;
|
||||
public final WPI_TalonFX rightFrontSteerMotor = null;
|
||||
public final WPI_TalonFX rightFrontWheelMotor = null;
|
||||
public final WPI_TalonFX leftBackSteerMotor = null;
|
||||
public final WPI_TalonFX leftBackWheelMotor = null;
|
||||
public final WPI_TalonFX rightBackSteerMotor = null;
|
||||
public final WPI_TalonFX rightBackWheelMotor = null;
|
||||
public final CANCoder leftFrontEncoder = null;
|
||||
public final CANCoder rightFrontEncoder = null;
|
||||
public final CANCoder leftBackEncoder = 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);
|
||||
// public final WPI_TalonFX leftFrontSteerMotor = null;
|
||||
// public final WPI_TalonFX leftFrontWheelMotor = null;
|
||||
// public final WPI_TalonFX rightFrontSteerMotor = null;
|
||||
// public final WPI_TalonFX rightFrontWheelMotor = null;
|
||||
// public final WPI_TalonFX leftBackSteerMotor = null;
|
||||
// public final WPI_TalonFX leftBackWheelMotor = null;
|
||||
// public final WPI_TalonFX rightBackSteerMotor = null;
|
||||
// public final WPI_TalonFX rightBackWheelMotor = null;
|
||||
// public final CANCoder leftFrontEncoder = null;
|
||||
// public final CANCoder rightFrontEncoder = null;
|
||||
// public final CANCoder leftBackEncoder = null;
|
||||
// public final CANCoder rightBackEncoder = null;
|
||||
|
||||
// void configureSwerveMotorControllers() {
|
||||
// leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
// rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
// leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
// rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
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);
|
||||
|
||||
// leftFrontSteerMotor.configFactoryDefault();
|
||||
// leftFrontWheelMotor.configFactoryDefault();
|
||||
// rightFrontSteerMotor.configFactoryDefault();
|
||||
// rightFrontWheelMotor.configFactoryDefault();
|
||||
// leftBackSteerMotor.configFactoryDefault();
|
||||
// leftBackWheelMotor.configFactoryDefault();
|
||||
// rightBackSteerMotor.configFactoryDefault();
|
||||
// rightBackWheelMotor.configFactoryDefault();
|
||||
void configureSwerveMotorControllers() {
|
||||
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// 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);
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
rightFrontSteerMotor.configFactoryDefault();
|
||||
rightFrontWheelMotor.configFactoryDefault();
|
||||
leftBackSteerMotor.configFactoryDefault();
|
||||
leftBackWheelMotor.configFactoryDefault();
|
||||
rightBackSteerMotor.configFactoryDefault();
|
||||
rightBackWheelMotor.configFactoryDefault();
|
||||
|
||||
// leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
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.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
// SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
|
||||
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);
|
||||
// }
|
||||
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
|
||||
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
|
||||
/* Boom Boom Subsystem */
|
||||
|
||||
@@ -17,6 +17,7 @@ public class AimToCenter extends CommandBase {
|
||||
// use odometry to find x and y later
|
||||
double x = 0;
|
||||
double y = 0;
|
||||
double angle = 0;
|
||||
double m_targetAngle;
|
||||
|
||||
// public static Gains m_aimGains;
|
||||
@@ -31,34 +32,26 @@ public class AimToCenter extends CommandBase {
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
x = 0;
|
||||
y = 0;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if (x > 0) {
|
||||
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_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle());
|
||||
m_turret.runshooterRotatePID(m_targetAngle);
|
||||
|
||||
}
|
||||
|
||||
public boolean isDeadzone() {
|
||||
if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
public static double angleToCenter(double x, double y, double gyro) {
|
||||
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
|
||||
return(angle);
|
||||
//m_turret.runshooterRotatePID(m_targetAngle);
|
||||
}
|
||||
|
||||
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.
|
||||
|
||||
Reference in New Issue
Block a user