Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

updated aim to center

Browse Source
This commit is contained in:
Abhishrek05
2022-02-19 15:05:49 -07:00
parent 64481cb1cc
commit 92777c105d
2 changed files with 22 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/AimToCenter.java
+13 -1
View File
@@ -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);
}
src/main/java/frc4388/robot/subsystems/Turret.java
+9 -19
View File
@@ -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);
}