aimtocenter

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/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

@@ -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) {}

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