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