Skip to content

Commit

Permalink
added real robot stuff- ready to review?
Browse files Browse the repository at this point in the history
  • Loading branch information
@minhnguyenbhs
minhnguyenbhs committed Sep 11, 2024
1 parent 6939298 commit 5f9bd98
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 39 deletions.
33 changes: 17 additions & 16 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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;
Expand All @@ -129,21 +130,21 @@ public static final class ScoringConstants {
// Value - Aimer angle in radians
public static HashMap<Double, Double> getAimerMap() {
HashMap<Double, Double> map = new HashMap<Double, Double>();
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;
}
Expand Down
32 changes: 12 additions & 20 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,19 @@
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;
import frc.robot.subsystems.intake.IntakeIOSim;
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;
Expand All @@ -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() {
Expand Down Expand Up @@ -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)));
Expand Down Expand Up @@ -160,9 +155,6 @@ private void configureBindings() {
rightJoystick.povRight()
.onTrue(new InstantCommand(
() -> drive.setAlignTarget(AlignTarget.RIGHT)));

SmartDashboard.putString("align target", drive.getAlignTarget().toString());

}


Expand Down
77 changes: 77 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/IntakeRoboRio.java
View file
Original file line number Diff line number Diff line change
@@ -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));
}
}
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/subsystems/scoring/AimerIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit 5f9bd98

Please sign in to comment.