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

turret soft + hard limits + zero WORKS

Browse Source
This commit is contained in:
aarav18
2022-03-17 11:54:05 -06:00
parent fc3a61f75c
commit 25426d7f33
3 changed files with 28 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -5
View File
@@ -204,18 +204,18 @@ public final class Constants {
// ID
public static final int TURRET_MOTOR_CAN_ID = 19;
//Gains for turret
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, TURRET_SPEED_MULTIPLIER);
public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER;
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.3/*TURRET_SPEED_MULTIPLIER*/);
public static final double SHOOTER_TURRET_MIN = -0.3;//-TURRET_SPEED_MULTIPLIER;
//Gains for hood
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7);
//#region test start
//#endregion test end
public static final double TURRET_FORWARD_HARD_LIMIT = 18.4;
public static final double TURRET_REVERSE_HARD_LIMIT = -104.3;
public static final double TURRET_FORWARD_HARD_LIMIT = 18.429;
public static final double TURRET_REVERSE_HARD_LIMIT = -106.454;
public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2;
public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 4;
public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2;
//Shooter gains for actual Drum
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0);
src/main/java/frc4388/robot/RobotContainer.java
+3 -1
View File
@@ -303,7 +303,9 @@ public class RobotContainer {
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));
// .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));\
.whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
.whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom));
src/main/java/frc4388/robot/subsystems/Turret.java
+20 -2
View File
@@ -45,6 +45,8 @@ public class Turret extends SubsystemBase {
long leftCurrentTime;
long leftElapsedTime;
double speedLimiter;
public Turret(CANSparkMax boomBoomRotateMotor) {
m_boomBoomRotateMotor = boomBoomRotateMotor;
@@ -59,9 +61,10 @@ public class Turret extends SubsystemBase {
m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// setTurretLimitSwitches(true);
m_boomBoomRightLimit.enableLimitSwitch(true);
m_boomBoomLeftLimit.enableLimitSwitch(true);
this.speedLimiter = 1.0;
setTurretPIDGains();
}
@@ -99,6 +102,7 @@ public class Turret extends SubsystemBase {
m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
// TODO: add 0.1
}
@Override
@@ -140,6 +144,20 @@ public class Turret extends SubsystemBase {
}
leftPrevState = leftState; // * Update the state of the left limit switch.
// speed limiting near hard limits
double currentPos = this.getEncoderPosition();
double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT);
double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT);
if (forwardDistance < 20.0) {
this.speedLimiter = 0.2 + (forwardDistance * 0.05);
}
if (reverseDistance < 20.0) {
this.speedLimiter = 0.2 + (reverseDistance * 0.05);
}
}
/**
@@ -169,7 +187,7 @@ public class Turret extends SubsystemBase {
* @param input from -1.0 to 1.0, positive is clockwise
*/
public void runTurretWithInput(double input) {
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER);
m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter);
}
public void runShooterRotatePID(double targetAngle) {