mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 08:48:03 -06:00
Merge branch 'shooter-test' into AutoAlign
This commit is contained in:
Abhishrek05
@@ -6,6 +6,20 @@ package frc4388.robot.subsystems;
|
||||
|
||||
import java.util.function.BooleanSupplier;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration;
|
||||
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
|
||||
import com.ctre.phoenix6.signals.ForwardLimitValue;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
|
||||
import com.ctre.phoenix6.signals.ReverseLimitValue;
|
||||
import com.revrobotics.CANSparkBase;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.SparkLimitSwitch;
|
||||
@@ -13,6 +27,7 @@ import com.revrobotics.SparkPIDController;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import edu.wpi.first.wpilibj.CAN;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Talon;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants;
|
||||
@@ -22,24 +37,34 @@ import frc4388.utility.Gains;
|
||||
|
||||
public class Intake extends SubsystemBase {
|
||||
|
||||
//NEO
|
||||
private CANSparkMax intakeMotor;
|
||||
private CANSparkMax pivot;
|
||||
private SparkPIDController m_spedController;
|
||||
private static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
|
||||
private SparkLimitSwitch forwardLimit;
|
||||
private SparkLimitSwitch reverseLimit;
|
||||
private SparkLimitSwitch intakeforwardLimit;
|
||||
private SparkLimitSwitch intakereverseLimit;
|
||||
|
||||
private Shooter shooter;
|
||||
//Talon
|
||||
private TalonFX talonIntake;
|
||||
private TalonFX talonPivot;
|
||||
private CANcoder encoder;
|
||||
|
||||
private HardwareLimitSwitchConfigs l;
|
||||
|
||||
TalonFXConfiguration doodooController = new TalonFXConfiguration();
|
||||
|
||||
|
||||
public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
|
||||
|
||||
private BooleanSupplier sup = () -> true;
|
||||
private BooleanSupplier dup = () -> false;
|
||||
|
||||
|
||||
|
||||
private double smartDashboardOuttakeValue;
|
||||
|
||||
/** Creates a new Intake. */
|
||||
//For NEO
|
||||
public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
|
||||
this.intakeMotor = intakeMotor;
|
||||
this.pivot = pivot;
|
||||
@@ -53,9 +78,6 @@ public class Intake extends SubsystemBase {
|
||||
reverseLimit.enableLimitSwitch(true);
|
||||
|
||||
intakeMotor.restoreFactoryDefaults();
|
||||
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
|
||||
|
||||
|
||||
|
||||
intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
|
||||
@@ -67,8 +89,79 @@ public class Intake extends SubsystemBase {
|
||||
m_spedController.setP(armGains.kP);
|
||||
m_spedController.setI(armGains.kI);
|
||||
m_spedController.setD(armGains.kD);
|
||||
|
||||
SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
}
|
||||
|
||||
//For Talon
|
||||
public Intake(TalonFX talonIntake, TalonFX talonPivot) {
|
||||
this.talonIntake = talonIntake;
|
||||
this.talonPivot = talonPivot;
|
||||
|
||||
talonIntake.getConfigurator().apply(new TalonFXConfiguration());
|
||||
talonPivot.getConfigurator().apply(new TalonFXConfiguration());
|
||||
|
||||
talonIntake.setNeutralMode(NeutralModeValue.Brake);
|
||||
talonPivot.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
// talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs());
|
||||
// talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs());
|
||||
|
||||
|
||||
|
||||
// doodooController.Slot0.kP = armGains.kP;
|
||||
// doodooController.Slot1.kI = armGains.kI;
|
||||
// doodooController.Slot2.kD = armGains.kD;
|
||||
|
||||
// in init function, set slot 0 gains
|
||||
var slot0Configs = new Slot0Configs();
|
||||
slot0Configs.kP = 0.05; // An error of 0.5 rotations results in 12 V output
|
||||
slot0Configs.kI = 0.0; // no output for integrated error
|
||||
slot0Configs.kD = 0.0; // A velocity of 1 rps results in 0.1 V output
|
||||
|
||||
talonPivot.getConfigurator().apply(slot0Configs);
|
||||
|
||||
|
||||
}
|
||||
|
||||
// ! Talon Methods
|
||||
public void talonPIDIn() {
|
||||
PositionVoltage request = new PositionVoltage(0).withSlot(0);
|
||||
talonPivot.setControl(request.withPosition(53)); //TODO: Find actual value
|
||||
}
|
||||
|
||||
public void talonPIDOut() {
|
||||
PositionVoltage request = new PositionVoltage(53).withSlot(53);
|
||||
talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value
|
||||
}
|
||||
|
||||
public void talonHandoff() {
|
||||
talonIntake.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
}
|
||||
|
||||
public void talonSpinIntakeMotor() {
|
||||
talonIntake.set(IntakeConstants.INTAKE_SPEED);
|
||||
}
|
||||
|
||||
public boolean getTalonIntakeLimitSwitchState() {
|
||||
var r = talonIntake.getForwardLimit().getValue().value == 1;
|
||||
return r;
|
||||
}
|
||||
|
||||
public void talonSetPivotEncoderPosition(int val) {
|
||||
talonPivot.setPosition(val);
|
||||
}
|
||||
|
||||
public void talonStopIntakeMotors() {
|
||||
talonIntake.set(0);
|
||||
}
|
||||
|
||||
public void talonStopArmMotor() {
|
||||
talonPivot.set(0);
|
||||
}
|
||||
|
||||
|
||||
// ! NEO Methods
|
||||
//hanoff
|
||||
public void spinIntakeMotor() {
|
||||
intakeMotor.set(IntakeConstants.INTAKE_SPEED);
|
||||
@@ -76,7 +169,6 @@ public class Intake extends SubsystemBase {
|
||||
|
||||
//Rotate robot in for handoff
|
||||
public void rotateArmIn() {
|
||||
//pivot.set(IntakeConstants.PIVOT_SPEED);
|
||||
pivot.set(IntakeConstants.PIVOT_SPEED);
|
||||
}
|
||||
|
||||
@@ -90,6 +182,10 @@ public class Intake extends SubsystemBase {
|
||||
m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
|
||||
//SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
|
||||
}
|
||||
|
||||
public void pidOut() {
|
||||
m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
|
||||
}
|
||||
|
||||
public void limitNote() {
|
||||
if (intakeforwardLimit.isPressed()) {
|
||||
@@ -99,10 +195,6 @@ public class Intake extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
public void pidOut() {
|
||||
m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
|
||||
}
|
||||
|
||||
public void rotateArmOut2() {
|
||||
if(reverseLimit.isPressed()){
|
||||
stopArmMotor();
|
||||
@@ -120,7 +212,15 @@ public class Intake extends SubsystemBase {
|
||||
}
|
||||
|
||||
public void handoff() {
|
||||
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED);
|
||||
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
}
|
||||
|
||||
public void handoff2() {
|
||||
if(intakeforwardLimit.isPressed()) {
|
||||
intakeMotor.set(-smartDashboardOuttakeValue);
|
||||
} else {
|
||||
intakeMotor.set(-smartDashboardOuttakeValue);
|
||||
}
|
||||
}
|
||||
|
||||
public void stopIntakeMotors() {
|
||||
@@ -155,13 +255,13 @@ public class Intake extends SubsystemBase {
|
||||
return pivot.getEncoder().getVelocity();
|
||||
}
|
||||
|
||||
public void resetPostion() {
|
||||
pivot.getEncoder().setPosition(0);
|
||||
public void setPivotEncoderPosition(int val) {
|
||||
pivot.getEncoder().setPosition(val);
|
||||
}
|
||||
|
||||
public void resetPosition1() {
|
||||
public void resetPosition() {
|
||||
if(forwardLimit.isPressed()) {
|
||||
resetPostion();
|
||||
setPivotEncoderPosition(0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -169,6 +269,10 @@ public class Intake extends SubsystemBase {
|
||||
return pivot.getEncoder().getPosition();
|
||||
}
|
||||
|
||||
public double getIntakeVelocity() {
|
||||
return intakeMotor.getEncoder().getVelocity();
|
||||
}
|
||||
|
||||
public void rotateArm() {
|
||||
|
||||
}
|
||||
@@ -181,11 +285,20 @@ public class Intake extends SubsystemBase {
|
||||
}
|
||||
}
|
||||
|
||||
public void changeIntakeNeutralState() {
|
||||
if(forwardLimit.isPressed()) {
|
||||
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
SmartDashboard.putNumber("Vel Output", getVelocity());
|
||||
SmartDashboard.putNumber("Position", getPos());
|
||||
resetPosition1();
|
||||
resetPosition();
|
||||
changeIntakeNeutralState();
|
||||
|
||||
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -33,7 +33,7 @@ public class LED extends SubsystemBase {
|
||||
if(m_self != null)
|
||||
return;
|
||||
m_led = new AddressableLED(9);
|
||||
m_ledBuffer = new AddressableLEDBuffer(130);
|
||||
m_ledBuffer = new AddressableLEDBuffer(10);
|
||||
m_led.setLength(m_ledBuffer.getLength());
|
||||
m_led.setData(m_ledBuffer);
|
||||
m_led.start();
|
||||
|
||||
@@ -5,8 +5,10 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Talon;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.IntakeConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
@@ -20,15 +22,6 @@ public class Shooter extends SubsystemBase {
|
||||
private TalonFX leftShooter;
|
||||
private TalonFX rightShooter;
|
||||
|
||||
private Limelight limelight;
|
||||
|
||||
// 0 = Stop
|
||||
// 1 = Idle, no limelight
|
||||
// 2 = limelight
|
||||
// 3 = Shooting
|
||||
private int shooterMode;
|
||||
|
||||
|
||||
/** Creates a new Shooter. */
|
||||
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight limelight) {
|
||||
leftShooter = leftTalonFX;
|
||||
@@ -38,16 +31,35 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
leftShooter.setNeutralMode(NeutralModeValue.Coast);
|
||||
rightShooter.setNeutralMode(NeutralModeValue.Coast);
|
||||
SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
|
||||
|
||||
}
|
||||
|
||||
public Shooter(TalonFX leftShooter) {
|
||||
this.leftShooter = leftShooter;
|
||||
leftShooter.setNeutralMode(NeutralModeValue.Coast);
|
||||
}
|
||||
|
||||
public void singleSpin() {
|
||||
leftShooter.set(1.0);
|
||||
}
|
||||
|
||||
public void singleSpin(double speed) {
|
||||
leftShooter.set(speed);
|
||||
}
|
||||
|
||||
public void spin() {
|
||||
shooterMode = 3;
|
||||
spin(ShooterConstants.SHOOTER_SPEED);
|
||||
spin(smartDashboardShooterSpeed);
|
||||
}
|
||||
|
||||
public void spin(double speed) {
|
||||
leftShooter.set(-speed);
|
||||
rightShooter.set(speed);
|
||||
rightShooter.set(-speed);
|
||||
}
|
||||
|
||||
public void spin(double leftSpeed, double rightSpeed) {
|
||||
leftShooter.set(leftSpeed);
|
||||
rightShooter.set(-rightSpeed);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
@@ -68,15 +80,10 @@ public class Shooter extends SubsystemBase {
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
|
||||
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
|
||||
smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
|
||||
|
||||
if(limelight.isNearSpeaker() && shooterMode == 0){
|
||||
shooterMode = 1;
|
||||
spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
|
||||
}
|
||||
|
||||
SmartDashboard.putNumber("Shooter Speed mode", shooterMode);
|
||||
SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
|
||||
SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
@@ -35,6 +36,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
@@ -55,10 +57,11 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
SmartDashboard.putBoolean("drift correction", false);
|
||||
// SmartDashboard.putBoolean("drift correction", false);
|
||||
stopped = false;
|
||||
} else if(leftStick.getNorm() > 0.05) {
|
||||
if (!stopped) {
|
||||
@@ -66,7 +69,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
stopped = true;
|
||||
}
|
||||
|
||||
SmartDashboard.putBoolean("drift correction", true);
|
||||
// SmartDashboard.putBoolean("drift correction", true);
|
||||
|
||||
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
|
||||
|
||||
@@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase {
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
if(fieldRelative) {
|
||||
double rot = 0;
|
||||
if(rightStick.getNorm() > 0.5) {
|
||||
orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1));
|
||||
rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians();
|
||||
}
|
||||
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
|
||||
Reference in New Issue
Block a user