From b2b14517615c1f20c365f55c5c256c615b418adb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 6 Mar 2022 18:02:04 -0700 Subject: [PATCH] encoder reset thingies --- src/main/java/frc4388/robot/Constants.java | 10 +- src/main/java/frc4388/robot/Robot.java | 3 + .../java/frc4388/robot/RobotContainer.java | 19 ++-- src/main/java/frc4388/robot/RobotMap.java | 6 +- .../java/frc4388/robot/commands/Shoot.java | 98 +++++++++++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 26 +++-- 6 files changed, 117 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e4672af..7801197 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -115,7 +115,6 @@ public final class Constants { // misc public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - // TODO: put in real numbers for the hub public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0)); } @@ -172,8 +171,7 @@ public final class Constants { public static final double ENCODER_TICKS_PER_REV = 2048; // Shoot Command Constants - public static final Gains SHOOT_DRIVE_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains SHOOT_TURRET_GAINS = new Gains(2.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID @@ -193,7 +191,7 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final float HOOD_FORWARD_LIMIT = 0; // TODO: find + public static final float HOOD_FORWARD_LIMIT = 200; // TODO: find public static final float HOOD_REVERSE_LIMIT = 0; // TODO: find } @@ -209,8 +207,8 @@ public final class Constants { public static final String NAME = "photonCamera"; - public static final double TARGET_HEIGHT = 8*12 + 8; // Convert to metric - public static final double TARGET_RADIUS = 4*12; // Convert to metric + public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?) + public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; public static final double V_FOV = 49.7; public static final double LIME_VIXELS = 960; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index bc9e910..0faf4a2 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -136,6 +136,9 @@ public class Robot extends TimedRobot { // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + + SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); + SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); // odo chooser stuff addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 49c27b9..be300b8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -95,10 +95,10 @@ public class RobotContainer { private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); // private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - /*private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); - private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);*/ + private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -214,6 +214,13 @@ public class RobotContainer { // Y > Retract Intake new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(() -> m_robotIntake.runExtender(false)); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); + // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) @@ -402,8 +409,8 @@ public class RobotContainer { Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); // FIXME: Chassis speeds are created from joystick inputs and do not reflect // actual robot velocity. - Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, - m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); + Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0], + m_robotSwerveDrive.getChassisSpeeds()[1]); Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); pathPoints.add(waypoint); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4f3ec54..94255ca 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -35,7 +35,7 @@ public class RobotMap { public RobotMap() { // configureLEDMotorControllers(); configureSwerveMotorControllers(); - // configureShooterMotorControllers(); + configureShooterMotorControllers(); } /* LED Subsystem */ @@ -213,8 +213,8 @@ public class RobotMap { // numbers out of our ass anymore // hood subsystem - // angleAdjusterMotor.restoreFactoryDefaults(); - // angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + angleAdjusterMotor.restoreFactoryDefaults(); + angleAdjusterMotor.setIdleMode(IdleMode.kBrake); } diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index bc6a2b9..b2a574f 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -4,9 +4,12 @@ package frc4388.robot.commands; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; @@ -32,13 +35,12 @@ public class Shoot extends CommandBase { public double m_targetVel; public double m_targetHood; public double m_targetAngle; - public double m_driveTargetAngle; + public Pose2d m_targetPoint; // pid public double error; public double prevError; - public Gains driveGains = ShooterConstants.SHOOT_DRIVE_GAINS; - public Gains turretGains = ShooterConstants.SHOOT_TURRET_GAINS; + public Gains gains = ShooterConstants.SHOOT_GAINS; public double kP, kI, kD; public double proportional, integral, derivative; public double time; @@ -49,6 +51,7 @@ public class Shoot extends CommandBase { public int inverted; // testing + public boolean simMode = true; public DummySensor driveDummy; public DummySensor turretDummy; @@ -69,9 +72,9 @@ public class Shoot extends CommandBase { addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); - kP = driveGains.kP; - kI = driveGains.kI; - kD = driveGains.kD; + kP = gains.kP; + kI = gains.kI; + kD = gains.kD; proportional = 0; integral = 0; @@ -81,24 +84,30 @@ public class Shoot extends CommandBase { tolerance = 5.0; isAimedInTolerance = false; - driveDummy = new DummySensor(180); - turretDummy = new DummySensor(180); + if (simMode) { + driveDummy = new DummySensor(180); + turretDummy = new DummySensor(180); - DummySensor.resetAll(); + DummySensor.resetAll(); + } } /** * Updates error for custom PID. */ public void updateError() { + m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation()); m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); error = (m_targetAngle - turretDummy.get() + 360) % 360; // error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); - SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); - System.out.println("Target Angle: " + m_targetAngle); - System.out.println("Error: " + error); + + if (simMode) { + SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); + System.out.println("Target Angle: " + m_targetAngle); + System.out.println("Error: " + error); + } } // Called when the command is initially scheduled. @@ -143,24 +152,68 @@ public class Shoot extends CommandBase { output = kP * proportional + kI * integral + kD * derivative; normOutput = output/360 * inverted; } - + // TODO: horizontal velocity correction + public double hTargetDistanceFromHub() { + + double hVel = m_swerve.getChassisSpeeds()[0]; + double velBeforeCorrection = m_boomBoom.getVelocity(m_distance); + double vDistanceFromHub = m_odoY; + double approxTravelTime = vDistanceFromHub / velBeforeCorrection; + double hTargetDistanceFromHub = hVel * approxTravelTime; + + // return hTargetDistanceFromHub; + return 0.0; // this is for no velocity correction + } + + public Pose2d findTargetPoint() { + + // position vector and radius + Translation2d position = new Translation2d(m_odoX, m_odoY); + double radius = position.getNorm(); + + // equation of circle = x^2 + y^2 = m_distance^2 + // derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y + + // velocity vector (x, y) + Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]); + + // unit tangential vector + Translation2d tangential = new Translation2d(0, 0); + // velocity vector (tangential, radial) + Translation2d polarVelocity = new Translation2d(0, 0); + + return SwerveDriveConstants.HUB_POSE; + } + + // TODO: vertical velocity correction + public double vTargetDistanceFromHub() { + return 0.0; // this is for no velocity correction + } + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.out.println("Normalized Output: " + normOutput); - // custom pid + if (simMode) { + System.out.println("Normalized Output: " + normOutput); + } + // custom pid runPID(); - driveDummy.apply(normOutput); - System.out.println("Drive Dummy: " + driveDummy.get()); + + if (simMode) { + driveDummy.apply(normOutput); + System.out.println("Drive Dummy: " + driveDummy.get()); + } m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the // entire swerve drive or its the line below // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); m_hood.runAngleAdjustPID(m_targetHood); m_boomBoom.runDrumShooterVelocityPID(m_targetVel); - - turretDummy.apply(normOutput); - System.out.println("Turret Dummy: " + turretDummy.get()); + + if (simMode) { + turretDummy.apply(normOutput); + System.out.println("Turret Dummy: " + turretDummy.get()); + } m_turret.m_boomBoomRotateMotor.set(normOutput); } @@ -171,6 +224,9 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return isAimedInTolerance; + if (simMode) { + return isAimedInTolerance; + } + return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 09fe7a0..44cd6e0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -63,7 +64,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; public Rotation2d rotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); private final Field2d m_field = new Field2d(); @@ -113,12 +114,12 @@ public class SwerveDrive extends SubsystemBase { double xSpeedMetersPerSecond = -speed.getX(); double ySpeedMetersPerSecond = speed.getY(); - SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED)); + chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, + rot * SwerveDriveConstants.ROTATION_SPEED); + SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -175,13 +176,20 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees()); // chassis speeds - // TODO: find the actual max velocity in m/s of the robot in fast mode to have - // accurate 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); } + /** + * Gets the current chassis speeds in m/s and rad/s. + * @return Current chassis speeds (vx, vy, ω) + */ + public double[] getChassisSpeeds() { + return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond}; + } + /** * Gets the current pose of the robot. *