Merge branch 'Shooter' of https://github.com/Team4388/2022NoWayHome into Shooter

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);

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.

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() {
 

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;
+  }
+}

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));
   }
 }

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
-**/
+}

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;