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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bd518e2..4fc150f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc4388.robot; +import com.revrobotics.CANSparkMax; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -49,9 +50,9 @@ public class RobotContainer { ); */ private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final BoomBoom m_robotBoomBoom = new BoomBoom(); + private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); - private final Turret m_robotTurret = new Turret(); + private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -77,8 +78,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())); } @@ -114,6 +114,7 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) .whenPressed(() -> m_robotHood.runHood(0.5d)) .whenReleased(() -> m_robotHood.runHood(0.d)); + // new JoystickButton(getOperatorJoystick()); } /** * Use this to pass the autonomous command to the main {@link Robot} class. 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/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java new file mode 100644 index 0000000..b63c5c5 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -0,0 +1,64 @@ +// 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.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; + +public class AimToCenter extends CommandBase { + /** Creates a new 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() { + } + + // 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(); + } + 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); + } + + // 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 5dfb993..01c850c 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,39 +40,25 @@ 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 public void periodic() { // This method will be called once per scheduler run - // Abhi was here + try { // SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); @@ -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(calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(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..530de78 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,7 +7,9 @@ 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.MotorFeedbackSensor; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; @@ -40,16 +42,15 @@ public class Turret extends SubsystemBase { SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); - //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,22 +58,20 @@ 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); - + + 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); } - private Gyro getGyroInterface() { - return null; - } @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,21 +80,13 @@ public class Turret extends SubsystemBase { m_sDriveSubsystem = subsystem1; } - public void runShooterWithInput(double input) { + public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } public void runshooterRotatePID(double targetAngle) { - 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; - - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); } public void resetGyroShooterRotate() @@ -108,19 +99,8 @@ 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; } - - - //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;