From be850585ec7c87c44e7e1ab12c5b7ff097fc3ddf Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 19:58:21 -0600 Subject: [PATCH] locked motors + shoot kinda works --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 8 +-- .../java/frc4388/robot/RobotContainer.java | 65 ++++++++++--------- src/main/java/frc4388/robot/RobotMap.java | 12 ++-- .../robot/commands/ShooterCommands/Shoot.java | 38 +++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +++ .../robot/subsystems/SwerveModule.java | 8 +++ 7 files changed, 87 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 26b599c..e5351ea 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -277,7 +277,7 @@ public final class Constants { public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a28a7e7..2232b4c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -142,9 +142,9 @@ public class Robot extends TimedRobot { m_robotTime.updateTimes(); // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); - current = + // current = // m_robotContainer.m_robotBoomBoom.getCurrent() + - m_robotContainer.m_robotClimber.getCurrent(); //+ + // m_robotContainer.m_robotClimber.getCurrent(); //+ // m_robotContainer.m_robotHood.getCurrent() + // m_robotContainer.m_robotIntake.getCurrent() + // m_robotContainer.m_robotExtender.getCurrent() + @@ -152,8 +152,8 @@ public class Robot extends TimedRobot { // m_robotContainer.m_robotStorage.getCurrent() + // m_robotContainer.m_robotSwerveDrive.getCurrent(); // m_robotContainer.m_robotTurret.getCurrent(); - SmartDashboard.putNumber("Total Robot Current Draw", current); - SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); + // SmartDashboard.putNumber("Total Robot Current Draw", current); + // SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 15810a5..1a2ed8c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,7 +112,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + // public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -172,9 +172,9 @@ public class RobotContainer { /* Default Commands */ // IK command - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + // getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( @@ -208,17 +208,17 @@ public class RobotContainer { new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotTurret.setDefaultCommand( - new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - }, m_robotTurret)); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + // if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + // } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + // }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { @@ -226,12 +226,12 @@ public class RobotContainer { if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); - m_robotClimber.setDefaultCommand( - new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - getOperatorController().getRightY() * 10000); }} - , m_robotClimber)); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + // getOperatorController().getRightY() * 10000); }} + // , m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -269,15 +269,22 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); - + 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)); + + new JoystickButton(getDriverController(), XboxController.Button.kY.value) + .whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + .whenReleased(() -> m_robotSwerveDrive.stopModules()); + new JoystickButton(getDriverController(), XboxController.Button.kX.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); + /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -335,18 +342,18 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) + // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) + // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); + // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9638c45..3a92ee8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -140,13 +140,13 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); NeutralMode mode = NeutralMode.Coast; - leftFrontSteerMotor.setNeutralMode(mode); + leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); leftFrontWheelMotor.setNeutralMode(mode);// Coast - rightFrontSteerMotor.setNeutralMode(mode); + rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); rightFrontWheelMotor.setNeutralMode(mode);// Coast - leftBackSteerMotor.setNeutralMode(mode); + leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); leftBackWheelMotor.setNeutralMode(mode);// Coast - rightBackSteerMotor.setNeutralMode(mode); + rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); rightBackWheelMotor.setNeutralMode(mode);// Coast // current limits @@ -196,8 +196,8 @@ public class RobotMap { } /* Climb Subsystem */ - public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO - public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO +// public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO +// public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO /* Hooks Subsystem */ // public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 5f4ae4c..339785d 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -53,7 +53,7 @@ public class Shoot extends CommandBase { private double initialSwerveRotation; // testing - private boolean simMode = true; + private boolean simMode = false; private DummySensor driveDummy; private DummySensor turretDummy; @@ -99,10 +99,10 @@ public class Shoot extends CommandBase { */ public void updateError() { m_targetPoint = SwerveDriveConstants.HUB_POSE; - 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; + // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); + m_targetAngle = AimToCenter.aaravAngleToCenter(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); if (simMode) { @@ -115,8 +115,11 @@ public class Shoot extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_odoX = 0;//m_swerve.getOdometry().getX(); - m_odoY = -1;//m_swerve.getOdometry().getY(); + + m_turret.gotoMidpoint(); + + m_odoX = 0;//-m_swerve.getOdometry().getY(); + m_odoY = -8;//-m_swerve.getOdometry().getX(); m_gyroAngle = m_swerve.getRegGyro().getDegrees(); initialSwerveRotation = m_gyroAngle; @@ -125,14 +128,15 @@ public class Shoot extends CommandBase { m_targetVel = m_boomBoom.getVelocity(m_distance); m_targetHood = m_boomBoom.getHood(m_distance); - m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; + m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; // deadzone processing if (AimToCenter.isDeadzone(m_targetAngle)) {} // initial error updateError(); - System.out.println("Error: " + error); + // System.out.println("Error: " + error); prevError = error; } /** @@ -159,22 +163,28 @@ public class Shoot extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + + if (simMode) { System.out.println("Normalized Output: " + normOutput); } - // custom pid - runPID(); - + // custom pid + if (simMode) { driveDummy.apply(normOutput); System.out.println("Drive Dummy: " + driveDummy.get()); } + + runPID(); + SmartDashboard.putNumber("Error", this.error); + SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle); + SmartDashboard.putNumber("Normalized Output", normOutput); 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); + // m_hood.runAngleAdjustPID(m_targetHood); + // m_boomBoom.runDrumShooterVelocityPID(m_targetVel); if (simMode) { turretDummy.apply(normOutput); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6399fbe..2c5600b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -127,6 +127,14 @@ public class SwerveDrive extends SubsystemBase { : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); + if (ignoreAngles) { + SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; + for (int i = 0; i < states.length; i ++) { + SwerveModuleState state = states[i]; + lockedStates[i]= new SwerveModuleState(0, state.angle); + } + setModuleStates(lockedStates); + } setModuleStates(states); // SmartDashboard.putNumber("rot", rot); // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 32bb5ca..d4905af 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -38,6 +38,9 @@ public class SwerveModule extends SubsystemBase { public double m_currentPos; public double m_lastPos; + public SwerveModuleState lastState = new SwerveModuleState(); + public SwerveModuleState currentState; + /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; @@ -181,10 +184,15 @@ public class SwerveModule extends SubsystemBase { @Override public void periodic() { + + currentState = this.getState(); + Rotation2d currentRotation = getAngle(); SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360); + + lastState = currentState; } public void reset() {