From ed8cf1443cd7093c9844a918d149974ccf7ceee1 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 17 Feb 2022 19:54:24 -0700 Subject: [PATCH 1/4] :)) --- .../java/frc4388/robot/RobotContainer.java | 6 ++- .../robot/commands/AimWithOdometry.java | 37 +++++++++++++++++++ .../frc4388/robot/subsystems/BoomBoom.java | 33 ++++++----------- .../java/frc4388/robot/subsystems/Turret.java | 30 +++++---------- .../java/frc4388/robot/subsystems/Vision.java | 2 + 5 files changed, 64 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/AimWithOdometry.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 964e0d9..907cdb5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc4388.robot; +import com.revrobotics.CANSparkMax; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -72,7 +74,7 @@ public class RobotContainer { //Turret default command m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } @@ -96,7 +98,7 @@ public class RobotContainer { .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); - + /* Driver Buttons */ } /** diff --git a/src/main/java/frc4388/robot/commands/AimWithOdometry.java b/src/main/java/frc4388/robot/commands/AimWithOdometry.java new file mode 100644 index 0000000..cb882a4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimWithOdometry.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class AimWithOdometry extends CommandBase { + /** Creates a new AimWithOdometry. */ + public AimWithOdometry() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Pose2d odo = new Pose2d(new Translation2d(0,0), new Rotation2d(0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 0a235e3..60caaf3 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.simulation.JoystickSim; import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -28,7 +29,7 @@ public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 -BangBangController m_controller = new BangBangController(); +// BangBangController m_controller = new BangBangController(); double velP; double input; @@ -39,33 +40,19 @@ public double m_fireVel; public Hood m_hoodSubsystem; public Turret m_turretSubsystem; -SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later +// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ -public BoomBoom(){ -//Testing purposes resetting gyros -//resetGryoAngleADj(); -//shooterTrims = new Trims(0,0); -feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); - - -} /** Creates a new BoomBoom. */ - public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { +public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - - - - - m_shooterTable = new ShooterTables(); - - - } + m_shooterTable = new ShooterTables(); +} @Override @@ -92,7 +79,10 @@ try { m_hoodSubsystem = subsystem0; m_turretSubsystem = subsystem1; } - +/** + * Runs the Drum motor at a given speed + * @param speed percent output form -1.0 to 1.0 + */ public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); @@ -114,7 +104,8 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2f70146..2890df7 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -42,14 +42,14 @@ public class Turret extends SubsystemBase { //Variables - public Turret() { + public Turret(CANSparkMax BoomBoomRotateMotor) { //Take in rotate motor as an argument - m_boomBoomRotateMotor = new CANSparkMax(30, MotorType.kBrushless); + m_boomBoomRotateMotor = BoomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - m_turretGyro = getGyroInterface(); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); @@ -57,14 +57,11 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit m_boomBoomRotateMotor.setInverted(false); } - private Gyro getGyroInterface() { - return null; - } @Override public void periodic() { @@ -81,16 +78,18 @@ public class Turret extends SubsystemBase { m_sDriveSubsystem = subsystem1; } - public void runShooterWithInput(double input) { + public void runTurretWithInput(double input) { //Rename to turret m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } - public void runshooterRotatePID(double targetAngle) { + public void runshooterRotatePID(double targetAngle) { //Split into configure and run m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; @@ -111,16 +110,5 @@ public class Turret extends SubsystemBase { public double getAnglePositionDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; } - - - //function turnWithJoystick(double input) - // motor.set(input) -} - - - -/** TODO -* setPosition function -* Limit switches -**/ \ No newline at end of file +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 310d70f..0b6ca09 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -37,6 +37,8 @@ public static double fireAngle; public double m_hoodTrim; public double m_turretTrim; public double m_fireAngle; + + public Vision(Turret aimSubsystem, BoomBoom boomBoom) { m_turret = aimSubsystem; m_boomBoom = boomBoom; From b5215b7a09d242aba5b79853be3d1d992d922390 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 19 Feb 2022 11:05:44 -0700 Subject: [PATCH 2/4] aimtocenter --- src/main/java/frc4388/robot/Constants.java | 11 ++++--- src/main/java/frc4388/robot/RobotMap.java | 2 +- ...{AimWithOdometry.java => AimToCenter.java} | 33 +++++++++++++++---- .../java/frc4388/robot/subsystems/Turret.java | 24 +++++++------- 4 files changed, 46 insertions(+), 24 deletions(-) rename src/main/java/frc4388/robot/commands/{AimWithOdometry.java => AimToCenter.java} (50%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 76c7b44..b23b51e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -83,16 +83,17 @@ public final class Constants { public static final int XBOX_OPERATOR_ID = 1; } + public static final class OdoAimConstants { + public static final double X_VECTOR_DISTANCE = 12.3; + // public static final Gains AIM_GAINS = new Gains(0, 0, 0, 0, 0, 0, 1); - + } public static final class ShooterConstants { /* PID Constants Shooter */ -<<<<<<< Updated upstream public static final int CLOSED_LOOP_TIME_MS = 1; -======= ->>>>>>> Stashed changes - public static final int SHOOTER_TIMEOUT_MS = 32; + + public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index fb93b88..1e2b648 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -102,7 +102,7 @@ public class RobotMap { public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // Create motor CANSparkMax void ConfigureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/commands/AimWithOdometry.java b/src/main/java/frc4388/robot/commands/AimToCenter.java similarity index 50% rename from src/main/java/frc4388/robot/commands/AimWithOdometry.java rename to src/main/java/frc4388/robot/commands/AimToCenter.java index cb882a4..734b9c2 100644 --- a/src/main/java/frc4388/robot/commands/AimWithOdometry.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -4,27 +4,46 @@ package frc4388.robot.commands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; -public class AimWithOdometry extends CommandBase { +public class AimToCenter extends CommandBase { /** Creates a new AimWithOdometry. */ - public AimWithOdometry() { + Turret m_turret; + SwerveDrive m_drive; + + // use odometry to find x and y later + double x = 0; + double y = 0; + double z = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); + double m_targetAngle; + + // public static Gains m_aimGains; + + public AimToCenter(Turret turret, SwerveDrive drive) { // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_drive = drive; + addRequirements(m_turret, m_drive); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Pose2d odo = new Pose2d(new Translation2d(0,0), new Rotation2d(0)); + aimTurret(m_targetAngle); } + public void aimTurret(double targetAngle) { //Split into configure and run + m_turret.runshooterRotatePID(targetAngle); + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) {} diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2890df7..1abcfa1 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -41,10 +42,11 @@ public class Turret extends SubsystemBase { public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + //Variables - public Turret(CANSparkMax BoomBoomRotateMotor) { //Take in rotate motor as an argument + public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument - m_boomBoomRotateMotor = BoomBoomRotateMotor; + m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); @@ -83,18 +85,18 @@ public class Turret extends SubsystemBase { } public void runshooterRotatePID(double targetAngle) { //Split into configure and run - m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); - m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); - m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); - m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); - m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); - targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); + m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); } public void resetGyroShooterRotate() @@ -107,7 +109,7 @@ public class Turret extends SubsystemBase { return m_boomBoomRotateEncoder.getPosition(); } - public double getAnglePositionDegrees() { + public double getBoomBoomAngleDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; } From 64481cb1cc22b497e90b3460fc988d3dad37a5e0 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 19 Feb 2022 13:32:39 -0700 Subject: [PATCH 3/4] edited aimtocenter --- src/main/java/frc4388/robot/commands/AimToCenter.java | 8 ++------ src/main/java/frc4388/robot/subsystems/Turret.java | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 734b9c2..a284d8a 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -31,17 +31,13 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - aimTurret(m_targetAngle); - } - - public void aimTurret(double targetAngle) { //Split into configure and run - m_turret.runshooterRotatePID(targetAngle); + m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + m_turret.runshooterRotatePID(m_targetAngle); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1abcfa1..5f785c5 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -9,6 +9,7 @@ import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; @@ -95,7 +96,6 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); } From 92777c105d3bc9c555ddbd48abeed13813b3a734 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 19 Feb 2022 15:05:49 -0700 Subject: [PATCH 4/4] updated aim to center --- .../frc4388/robot/commands/AimToCenter.java | 14 +++++++++- .../java/frc4388/robot/subsystems/Turret.java | 28 ++++++------------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index a284d8a..b63c5c5 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -36,7 +36,19 @@ public class AimToCenter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + if (x > 0) { + m_targetAngle = 180 + Math.atan(y/x) - m_drive.gyro.getAngle(); + } + if (x < 0) { + m_targetAngle = 360 + Math.atan(y/x) - m_drive.gyro.getAngle(); + } + if (x == 0 && y > 0) { + m_targetAngle = 270 - m_drive.gyro.getAngle(); + } + if (x == 0 && y < 0) { + m_targetAngle = 90 - m_drive.gyro.getAngle(); + } + m_turret.runshooterRotatePID(m_targetAngle); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 5f785c5..530de78 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -42,8 +42,6 @@ public class Turret extends SubsystemBase { SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); - - //Variables public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument @@ -62,17 +60,18 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit m_boomBoomRotateMotor.setInverted(false); - + + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); } @Override public void periodic() { - // SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); - - // SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); - - // SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } @@ -81,20 +80,11 @@ public class Turret extends SubsystemBase { m_sDriveSubsystem = subsystem1; } - public void runTurretWithInput(double input) { //Rename to turret + public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } - public void runshooterRotatePID(double targetAngle) { //Split into configure and run - m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); - m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); - m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); - m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); - - - m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); - + public void runshooterRotatePID(double targetAngle) { targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); }