From 5f9bd988fbde1fdc48644ae67044c8c981e138ec Mon Sep 17 00:00:00 2001 From: minhnguyenbhs Date: Wed, 11 Sep 2024 19:25:03 -0400 Subject: [PATCH] added real robot stuff- ready to review? --- src/main/java/frc/robot/Constants.java | 33 ++++---- src/main/java/frc/robot/RobotContainer.java | 32 +++----- .../subsystems/intake/IntakeRoboRio.java | 77 +++++++++++++++++++ .../robot/subsystems/scoring/AimerIOSim.java | 6 +- 4 files changed, 109 insertions(+), 39 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeRoboRio.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b31ccc..ba5fb3c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,6 +44,7 @@ public static final class SensorConstants { //TODO: Find real values public static final int indexerSensorPort = 0; + public static final int uptakeSensorPort = 0; } public static final class ScoringConstants { @@ -110,8 +111,8 @@ public static final class ScoringConstants { public static final double hoodHomeAmps = 40.0; // TODO: Find this public static final double hoodHomeAngleRad = Math.PI - 0.23; - public static final double aimMaxAngleRadians = 1.65; // Math.PI / 2 - public static final double aimMinAngleRadians = -0.03; + public static final double aimMaxAngleRadians = 2*Math.PI; // Math.PI / 2 + public static final double aimMinAngleRadians = Math.PI; public static final double maxAimIntake = 0.0; public static final double minAimIntake = 0.0; @@ -129,21 +130,21 @@ public static final class ScoringConstants { // Value - Aimer angle in radians public static HashMap getAimerMap() { HashMap map = new HashMap(); - map.put(0.0, 0.8); - map.put(1.45, 0.8); - map.put(1.98, 0.62); - map.put(2.41, 0.53); - map.put(3.02, 0.45); - map.put(3.22, 0.45); - map.put(3.9, 0.36); - map.put(4.55, 0.35); - map.put(4.95, 0.32); - map.put(5.15, 0.295); - map.put(5.35, 0.295); - map.put(5.5, 0.295); - map.put(5.64, 0.29); + map.put(0.0, 2*Math.PI-0.8); + map.put(1.45, 2*Math.PI-0.8); + map.put(1.98, 2*Math.PI-0.62); + map.put(2.41, 2*Math.PI-0.53); + map.put(3.02, 2*Math.PI-0.45); + map.put(3.22, 2*Math.PI-0.45); + map.put(3.9, 2*Math.PI-0.36); + map.put(4.55, 2*Math.PI-0.35); + map.put(4.95, 2*Math.PI-0.32); + map.put(5.15, 2*Math.PI-0.295); + map.put(5.35, 2*Math.PI-0.295); + map.put(5.5, 2*Math.PI-0.295); + map.put(5.64, 2*Math.PI-0.29); // map.put(5.82, 0.275); - map.put(6.0, 0.29); + map.put(6.0, 2*Math.PI-0.29); return map; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a53ca34..2a61328 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.subsystems.drive.commands.DriveWithJoysticks; import frc.robot.Constants.AlignTarget; import frc.robot.Constants.ConversionConstants; +import frc.robot.Constants.Mode; import frc.robot.Constants.ScoringConstants; import frc.robot.commands.ShootWithGamepad; import frc.robot.subsystems.intake.IntakeIO; @@ -29,9 +30,11 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeAction; import frc.robot.subsystems.scoring.AimerIO; +import frc.robot.subsystems.scoring.AimerIORoboRio; import frc.robot.subsystems.scoring.AimerIOSim; import frc.robot.subsystems.scoring.ScoringSubsystem; import frc.robot.subsystems.scoring.ShooterIO; +import frc.robot.subsystems.scoring.ShooterIORoboRio; import frc.robot.subsystems.scoring.ShooterIOSim; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringAction; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -49,27 +52,19 @@ public class RobotContainer { CommandXboxController controller = new CommandXboxController(2); public RobotContainer() { - //configureSubsystems(); - - scoringSubsystem = new ScoringSubsystem(new ShooterIOSim(), new AimerIOSim()); - intakeSubsystem = new IntakeSubsystem(new IntakeIOSim()); - + configureSubsystems(); configureBindings(); - System.out.println("containerrunning"); - SmartDashboard.putString("AlignTarget", "NONE"); } - /*public void configureSubsystems() { - if (true) { - scoringSubsystem = - new ScoringSubsystem( - new ShooterIOSim(), new AimerIOSim()); - } - - if (FeatureFlags.runIntake) { + public void configureSubsystems() { + if (Constants.currentMode == Mode.REAL) { + scoringSubsystem = new ScoringSubsystem(new ShooterIORoboRio(), new AimerIORoboRio()); + //intakeSubsystem = new IntakeSubsystem(new IntakeIORobo) + } else if (Constants.currentMode == Mode.SIM) { + scoringSubsystem = new ScoringSubsystem(new ShooterIOSim(), new AimerIOSim()); intakeSubsystem = new IntakeSubsystem(new IntakeIOSim()); } - }*/ + } // spotless:off private void configureBindings() { @@ -110,7 +105,7 @@ private void configureBindings() { () -> controller.getRightTriggerAxis() > 0.5, controller.getHID()::getAButton, controller.getHID()::getBButton, scoringSubsystem, - () -> AlignTarget.AMP)); + () -> drive.getAlignTarget())); //FeatureFlags.runDrive ? drivetrain::getAlignTarget : () -> AlignTarget.NONE)); rightJoystick.button(11).onTrue(new InstantCommand(() -> scoringSubsystem.setArmDisabled(true))); @@ -160,9 +155,6 @@ private void configureBindings() { rightJoystick.povRight() .onTrue(new InstantCommand( () -> drive.setAlignTarget(AlignTarget.RIGHT))); - - SmartDashboard.putString("align target", drive.getAlignTarget().toString()); - } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRoboRio.java b/src/main/java/frc/robot/subsystems/intake/IntakeRoboRio.java new file mode 100644 index 0000000..d0fcdb4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRoboRio.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.SensorConstants; + +public class IntakeRoboRio implements IntakeIO { + + private TalonFX leftIntake = + new TalonFX(IntakeConstants.leftIntakeMotorID); + private TalonFX rightIntake = + new TalonFX(IntakeConstants.rightIntakeMotorID); + + private TalonFX belt = new TalonFX(IntakeConstants.indexTwoMotorID); + + DigitalInput bannerSensor = new DigitalInput(SensorConstants.uptakeSensorPort); + + public IntakeRoboRio() { + TalonFXConfigurator leftIntakeConfig = leftIntake.getConfigurator(); + leftIntakeConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(120) + .withStatorCurrentLimitEnable(true)); + + TalonFXConfigurator rightIntakeConfig = rightIntake.getConfigurator(); + rightIntakeConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(120) + .withStatorCurrentLimitEnable(true)); + + leftIntake.setInverted(true); + rightIntake.setInverted(true); + + belt.setInverted(true); + + TalonFXConfigurator beltConfig = belt.getConfigurator(); + beltConfig.apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)); + beltConfig.apply( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true)); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.leftIntakeVoltage = leftIntake.getMotorVoltage().getValueAsDouble(); + inputs.leftIntakeStatorCurrent = leftIntake.getStatorCurrent().getValueAsDouble(); + + inputs.rightIntakeVoltage = rightIntake.getMotorVoltage().getValueAsDouble(); + inputs.rightIntakeStatorCurrent = rightIntake.getStatorCurrent().getValueAsDouble(); + + inputs.beltVoltage = belt.getMotorVoltage().getValueAsDouble(); + inputs.beltStatorCurrent = belt.getStatorCurrent().getValueAsDouble(); + inputs.beltSupplyCurrent = belt.getSupplyCurrent().getValueAsDouble(); + + inputs.noteSensed = !bannerSensor.get(); + } + + @Override + public void setIntakeVoltage(double volts) { + leftIntake.set(volts / 12); + rightIntake.set(volts / 12); + } + + @Override + public void setBeltVoltage(double volts) { + belt.setControl(new VoltageOut(volts)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java index d1937f2..7585d3c 100644 --- a/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java @@ -17,11 +17,11 @@ public class AimerIOSim implements AimerIO { DCMotor.getKrakenX60(1), 80, SingleJointedArmSim.estimateMOI(0.3872, 8.61), - 0.4, + 0.3872, 0.0, - ScoringConstants.aimMaxAngleRadians+Math.PI/2, + 2*Math.PI, true, - 0.0); + 3/2*Math.PI); private final PIDController controller = new PIDController( ScoringConstants.aimerkP, ScoringConstants.aimerkI, ScoringConstants.aimerkD);