From 289baefd21e402c76d3a5c18608610cc3d975471 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Tue, 13 Feb 2024 22:21:26 -0600 Subject: [PATCH] more motor tuning, added absolute encoder setting to smartDashboard --- .../java/frc/robot/settings/Constants.java | 12 ++--- .../subsystems/AngleShooterSubsystem.java | 45 ++++++++++++------- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 1 + 4 files changed, 36 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 555f2538..4cbd72b9 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -272,7 +272,7 @@ public static final class ShooterConstants{ public static final double SHOOTER_ANGLE_TOLERANCE = 0.5; public static final double SHOOTER_HEIGHT = 0.1; public static final double ANGLE_TICKS_PER_DEGREE = 2491; - public static final double DEGREES_PER_ROTATION = 2491; + public static final double DEGREES_PER_ROTATION = 360; public static final double DISTANCE_MULTIPLIER = 0; public static final double OFFSET_MULTIPLIER = 1; @@ -314,11 +314,11 @@ public static final class IndexerConstants{ public static final double INDEXER_SPEED = 2491; } public static final class IntakeConstants{ - public static final int INTAKE_1_MOTOR = 2491; - public static final int INTAKE_2_MOTOR = 2491; - public static final int BRUSH_1_MOTOR = 2491; - public static final int BRUSH_2_MOTOR = 2491; - public static final int BRUSH_3_MOTOR = 2491; + public static final int INTAKE_1_MOTOR = 20; + public static final int INTAKE_2_MOTOR = 21; + public static final int BRUSH_1_MOTOR = 22; + public static final int BRUSH_2_MOTOR = 23; + public static final int BRUSH_3_MOTOR = 24; public static final double INTAKE_SPEED = 0.75; } public static final class CTREConfigs { diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index 430676e3..d744d56f 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java @@ -13,8 +13,10 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkAbsoluteEncoder.Type; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; @@ -22,40 +24,45 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class AngleShooterSubsystem extends SubsystemBase { CANSparkMax pitchMotor; SparkPIDController pitchPID; - CANcoder angleEncoder; + SparkAbsoluteEncoder absoluteEncoder; double shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; public static Pose2d dtvalues; public static ChassisSpeeds DTChassisSpeeds; public AngleShooterSubsystem() { pitchMotor = new CANSparkMax(ShooterConstants.PITCH_MOTOR_ID, MotorType.kBrushless); - + pitchMotor.setInverted(true); pitchPID = pitchMotor.getPIDController(); - pitchPID.setFF(ShooterConstants.pitchFeedForward); - } + absoluteEncoder = pitchMotor.getAbsoluteEncoder(Type.kDutyCycle); + absoluteEncoder.setPositionConversionFactor(1); + absoluteEncoder.setInverted(false); + SmartDashboard.putData("set zero offset", new InstantCommand(()->absoluteEncoder.setZeroOffset(SmartDashboard.getNumber("shooter encoder zero offset", absoluteEncoder.getZeroOffset())))); + } + public void pitchShooter(double pitchSpeed) { pitchMotor.set(pitchSpeed); } - + public double getShooterAngle() { - return angleEncoder.getPosition().getValueAsDouble() * ShooterConstants.DEGREES_PER_ROTATION; + return absoluteEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROTATION; } - + public static void setDTPose(Pose2d pose) { dtvalues = pose; } - + public static void setDTChassisSpeeds(ChassisSpeeds speeds) { DTChassisSpeeds = speeds; } - + public double calculateSpeakerAngleDifference() { double deltaX; shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS; @@ -69,19 +76,19 @@ public double calculateSpeakerAngleDifference() { double deltaY = Math.abs(dtvalues.getY() - Field.SPEAKER_Y); double speakerDist = Math.sqrt(Math.pow(deltaY, 2) + Math.pow(deltaX, 2)); SmartDashboard.putNumber("dist to speakre", speakerDist); - + double shootingTime = speakerDist / shootingSpeed; // calculates how long the note will take to reach the target double currentXSpeed = DTChassisSpeeds.vxMetersPerSecond; double currentYSpeed = DTChassisSpeeds.vyMetersPerSecond; Translation2d targetOffset = new Translation2d(currentXSpeed * shootingTime*OFFSET_MULTIPLIER, currentYSpeed * shootingTime*OFFSET_MULTIPLIER); // line above calculates how much our current speed will affect the ending // location of the note if it's in the air for ShootingTime - + // next 3 lines set where we actually want to aim, given the offset our shooting // will have based on our speed double offsetSpeakerY = Field.SPEAKER_Y + targetOffset.getY(); double offsetSpeakerX; - + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { offsetSpeakerX = Field.RED_SPEAKER_X + targetOffset.getX(); } else { @@ -92,20 +99,24 @@ public double calculateSpeakerAngleDifference() { double offsetSpeakerdist = Math.sqrt(Math.pow(offsetDeltaX, 2) + Math.pow(offsetDeltaY, 2)); SmartDashboard.putString("offset amount", targetOffset.toString()); SmartDashboard.putString("offset speaker location", - new Translation2d(offsetSpeakerX, offsetSpeakerY).toString()); + new Translation2d(offsetSpeakerX, offsetSpeakerY).toString()); // getting desired robot angle double totalDistToSpeaker = Math .sqrt(Math.pow(offsetSpeakerdist, 2) + Math.pow(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT, 2)); double desiredShooterAngle = Math - .toDegrees(Math.asin(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT / totalDistToSpeaker)); + .toDegrees(Math.asin(Field.SPEAKER_Z - ShooterConstants.SHOOTER_HEIGHT / totalDistToSpeaker)); desiredShooterAngle = desiredShooterAngle+(speakerDist*DISTANCE_MULTIPLIER); SmartDashboard.putNumber("desired shooter angle", desiredShooterAngle); - + double differenceAngle = (desiredShooterAngle - this.getShooterAngle()); SmartDashboard.putNumber("differenceAngleShooter", differenceAngle); - + return differenceAngle; } - + @Override + public void periodic() { + SmartDashboard.putNumber("shooter angle encoder position", absoluteEncoder.getPosition()); + SmartDashboard.putNumber("shooter encoder zero offset", absoluteEncoder.getZeroOffset()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 46306749..ef4b9c44 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -26,7 +26,7 @@ public class IntakeSubsystem extends SubsystemBase { double intakeRunSpeed; public IntakeSubsystem() { intake1 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); - intake2 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless); + intake2 = new CANSparkMax(IntakeConstants.INTAKE_2_MOTOR, MotorType.kBrushless); brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless); brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless); brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e91e1b66..41a2f35e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -63,6 +63,7 @@ public ShooterSubsystem(double runSpeed) { shooterR = new TalonFX(ShooterConstants.SHOOTER_R_MOTORID); shooterL = new TalonFX(ShooterConstants.SHOOTER_L_MOTORID); shooterL.setInverted(true); + shooterR.setInverted(false); shooterL.setControl(shooterR.getAppliedControl()); shooterL.setNeutralMode(NeutralModeValue.Coast); shooterR.setNeutralMode(NeutralModeValue.Coast);