From dbc1e070e0ec115311d7f48333b469525fd6027f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 16:37:59 -0600 Subject: [PATCH] AimToCenter works without fixing odometry issues --- src/main/java/frc4388/robot/Constants.java | 28 +++++++++---------- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 4 ++- .../commands/ShooterCommands/AimToCenter.java | 28 +++++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 22 +++++++-------- .../robot/subsystems/SwerveModule.java | 5 ++-- .../java/frc4388/robot/subsystems/Turret.java | 10 +++++++ 7 files changed, 58 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1b73301..26b599c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -37,26 +37,26 @@ public final class Constants { public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 3.0; - public static final double WIDTH = 23.5; - public static final double HEIGHT = 23.5; + public static final double WIDTH = 23.75; + public static final double HEIGHT = 23.75; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant? public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant? // IDs - public static final int LEFT_FRONT_STEER_CAN_ID = 2; // - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // - public static final int LEFT_BACK_STEER_CAN_ID = 6; // - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // - public static final int RIGHT_BACK_STEER_CAN_ID = 8; // - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;// - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // + public static final int LEFT_FRONT_STEER_CAN_ID = 2; // * + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // * + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // * + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // * + public static final int LEFT_BACK_STEER_CAN_ID = 6; // * + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // * + public static final int RIGHT_BACK_STEER_CAN_ID = 8; // * + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // * + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // * + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // * + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // * + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // * public static final int GYRO_ID = 14; // offsets are in degrees diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index da66be3..a28a7e7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -262,7 +262,7 @@ public class Robot extends TimedRobot { if (selectedOdo == null) { selectedOdo = m_robotContainer.getOdometry(); } - m_robotContainer.resetOdometry(selectedOdo); + // m_robotContainer.resetOdometry(selectedOdo); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 866fafb..15810a5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -273,8 +273,10 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kB.value) .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - + new JoystickButton(getDriverController(), XboxController.Button.kY.value) + .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-45), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index c881512..882e010 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -4,6 +4,7 @@ package frc4388.robot.commands.ShooterCommands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; @@ -35,15 +36,18 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - x = m_drive.getOdometry().getX(); - y = m_drive.getOdometry().getY(); + x = -m_drive.getOdometry().getY(); + y = -m_drive.getOdometry().getX(); + + SmartDashboard.putNumber("trans x", x); + SmartDashboard.putNumber("trans y", y); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getRegGyro().getDegrees())) % 360; - System.out.println("Target Angle" + m_targetAngle); + m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360; + SmartDashboard.putNumber("Target Angle", m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); // Check if limelight is within range (comment out to disable vision odo) @@ -62,15 +66,17 @@ public class AimToCenter extends CommandBase { } public static double aaravAngleToCenter(double x, double y, double gyro) { - double yes = 360 - gyro; - double exp = Math.toDegrees(Math.atan(y/x)) - yes; - if (x > 0) { return exp; } - if (x < 0) { return (180 + exp); } + + double actualGyro = -gyro + 90; + + double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro; + if (-x > 0) { return (180 + exp); } + if (-x < 0) { return (360 + exp); } - if (x == 0 && y > 0) { return (90 - yes); } - if (x == 0 && y < 0) { return (-90 - yes); } + if (x == 0 && y > 0) { return (270 - actualGyro); } + if (x == 0 && y < 0) { return (90 - actualGyro); } - System.out.println("Invalid case."); + System.err.println("Invalid case."); return 0; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 24727e7..6399fbe 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -36,10 +36,10 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth)); - Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); + Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(-halfHeight)); + Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(halfHeight)); + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(-halfHeight)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(halfHeight)); public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -48,7 +48,7 @@ public class SwerveDrive extends SubsystemBase { public WPI_Pigeon2 m_gyro; public SwerveDrivePoseEstimator m_poseEstimator; - public SwerveDriveOdometry m_odometry; + // public SwerveDriveOdometry m_odometry; public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; @@ -69,14 +69,14 @@ public class SwerveDrive extends SubsystemBase { modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( - m_gyro.getRotation2d(), + getRegGyro(),//m_gyro.getRotation2d(), new Pose2d(), m_kinematics, VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune - m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + // m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); m_gyro.reset(); SmartDashboard.putData("Field", m_field); @@ -160,7 +160,7 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); - m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); } @@ -172,9 +172,9 @@ public class SwerveDrive extends SubsystemBase { // chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds - // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); - // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); - // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 11e861e..32bb5ca 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -79,6 +79,7 @@ public class SwerveModule extends SubsystemBase { // driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); + canCoderConfiguration.sensorCoefficient = 0.087890625; canCoderConfiguration.magnetOffsetDegrees = offset; canCoderConfiguration.sensorDirection = true; canCoder.configAllSettings(canCoderConfiguration); @@ -90,9 +91,7 @@ public class SwerveModule extends SubsystemBase { } private Rotation2d getAngle() { - // Note: This assumes the CANCoders are setup with the default feedback - // coefficient - // and the sensor value reports degrees. + // ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 9d9e35c..95a41d0 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -195,6 +196,15 @@ public class Turret extends SubsystemBase { } public void runShooterRotatePID(double targetAngle) { + + double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2; + + targetAngle = (targetAngle % 360); + + if (targetAngle > softMid) { + targetAngle = targetAngle - 360; + } + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); }