Skip to content
This repository has been archived by the owner on Jan 16, 2024. It is now read-only.

Commit

Permalink
feat: long night
Browse files Browse the repository at this point in the history
  • Loading branch information
WeilSimon committed Dec 15, 2023
1 parent 0fb798e commit 4c04279
Show file tree
Hide file tree
Showing 14 changed files with 346 additions and 26 deletions.
6 changes: 0 additions & 6 deletions build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -65,12 +65,6 @@ configurations.all {
exclude group: "edu.wpi.first.wpilibj"
}

//task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
// mainClass = "org.littletonrobotics.junction.CheckInstall"
// classpath = sourceSets.main.runtimeClasspath
//}
//compileJava.finalizedBy checkAkitInstall

// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
// Also defines JUnit 4.
dependencies {
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -62,4 +62,10 @@ public static class Drivetrain{


}

public static class Intake {
public static int rollerID = 18;
public static int leadPivotID = 17;
public static int followerPivotID = 16;
}
}
16 changes: 14 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,11 @@
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.DriveCommands;
import frc.robot.commands.FeedForwardCharacterization;
import frc.robot.commands.IntakeCommands;
import frc.robot.commands.ShooterCommands;
import frc.robot.subsystems.drive.*;
import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.intake.IntakeIOReal;
import frc.robot.subsystems.limelight.Limelight;
import frc.robot.subsystems.limelight.LimelightIOReal;
import frc.robot.subsystems.shooter.*;
Expand All @@ -45,6 +48,7 @@ public class RobotContainer {
private final Shooter shooter;

private final Limelight limelight;
private final Intake intake;

// Controller
private final CommandXboxController controller = new CommandXboxController(0);
Expand All @@ -69,8 +73,10 @@ public RobotContainer() {
new Shooter(
new FlywheelIOTalonFX(),
new FlywheelIOSparkMaxSingle(1, 13, false, 20),
new FlywheelIOSparkMaxSingle(10, 14, true, 40));
new FlywheelIOSparkMaxSingle(1.0/10.0, 14, true, 40));
limelight = new Limelight(new LimelightIOReal());
// limelight = null;
intake = new Intake(new IntakeIOReal());
break;
}

Expand All @@ -87,6 +93,7 @@ public RobotContainer() {
new ModuleIOSim());
shooter = null;
limelight = null;
intake = null;
break;
}

Expand All @@ -106,6 +113,7 @@ public RobotContainer() {
});
shooter = null;
limelight = null;
intake = null;
break;
}
}
Expand Down Expand Up @@ -134,6 +142,7 @@ private void configureButtonBindings() {

// shooter.setDefaultCommand(new ShooterCommands.ShooterIdle(shooter));
shooter.setDefaultCommand(new ShooterCommands.ShooterTesting(shooter));
intake.setDefaultCommand(new IntakeCommands.IntakeTesting(intake));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller.y().onTrue(new InstantCommand(() -> drive.resetPose()));
// controller
Expand All @@ -146,7 +155,10 @@ private void configureButtonBindings() {
// drive)
// .ignoringDisable(true));

controller.a()
controller.a().onTrue(new IntakeCommands.IntakeDown(intake));
controller.b().onTrue(new IntakeCommands.IntakeUp(intake));

controller.rightTrigger(0.7)
.whileTrue(DriveCommands.LimelightRotDrive(
drive,
() -> -controller.getLeftX(),
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/DriveCommands.java
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@

public class DriveCommands {
private static final double DEADBAND = 0.1;
private static PIDController pid = new PIDController(0,0,0);
private static PIDController pid = new PIDController(1,0,0);

private DriveCommands() {}

Expand Down Expand Up @@ -88,7 +88,7 @@ public static Command LimelightRotDrive(
double omega = MathUtil.applyDeadband(limelight.getTx() * limelight.getHorizontalFov()/(2*100), 0.01);
// Square values
linearMagnitude = linearMagnitude * linearMagnitude;
omega = Math.copySign(omega * omega, omega);
// omega = Math.copySign(omega * omega, omega);

// Calcaulate new linear velocity
Translation2d linearVelocity =
Expand Down
58 changes: 58 additions & 0 deletions src/main/java/frc/robot/commands/IntakeCommands.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
package frc.robot.commands;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.intake.Intake;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;

public class IntakeCommands {
public static class IntakeTesting extends CommandBase{
LoggedDashboardNumber intakeAngle = new LoggedDashboardNumber("IntakeAngle", 0);
LoggedDashboardNumber rollerVolts = new LoggedDashboardNumber("RollerVolts", 0);
Intake intake;
public IntakeTesting(Intake intake){
this.intake = intake;
addRequirements(intake);
}

@Override
public void execute() {
intake.setPivotAngle(Rotation2d.fromRotations(intakeAngle.get()));
intake.rollerVoltage(rollerVolts.get());
}

}

public static class IntakeUp extends CommandBase{
Intake intake;
public IntakeUp(Intake intake){
this.intake = intake;
addRequirements(intake);
}

@Override
public void initialize() {
intake.setPivotAngle(Rotation2d.fromRotations(5));
intake.rollerVoltage(0);
}
}

public static class IntakeDown extends CommandBase{
Intake intake;
public IntakeDown(Intake intake){
this.intake = intake;
addRequirements(intake);
}

@Override
public void initialize() {
intake.setPivotAngle(Rotation2d.fromRotations(24));
intake.rollerVoltage(8);
}
}





}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/commands/ShooterCommands.java
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,7 @@ public ShooterTesting(Shooter shooter) {

@Override
public void execute() {
// shooter.setMainVelocity(mainRPM.get());
shooter.setMainVelocity(mainRPM.get());
shooter.setSecondaryVelocity(secondaryRPM.get());
shooter.setLoadingVoltage(loadingVoltage.get());
Expand Down
64 changes: 64 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/Intake.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
package frc.robot.subsystems.intake;

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.util.LoggedTunableNumber;
import org.littletonrobotics.junction.Logger;


public class Intake extends SubsystemBase {
private static final boolean TUNING_MODE = true;
private final IntakeIO io;

private final IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged();

private final LoggedTunableNumber kPLead = new LoggedTunableNumber("Intake/kPLead", 0.058);
private final LoggedTunableNumber kILead = new LoggedTunableNumber("Intake/kILead", 0);
private final LoggedTunableNumber kDLead = new LoggedTunableNumber("Intake/kDLead", 0.7);

private final LoggedTunableNumber kPFollow = new LoggedTunableNumber("Intake/kPFollow", 0.045);
private final LoggedTunableNumber kIFollow = new LoggedTunableNumber("Intake/kIFollow", 0);
private final LoggedTunableNumber kDFollow = new LoggedTunableNumber("Intake/kDFollow", 3);

// private final ArmFeedforward ff = new ArmFeedforward()

private Rotation2d setpoint = new Rotation2d();

public Intake(IntakeIO io) {
this.io = io;
io.setPivotBreakMode(true);
}

@Override
public void periodic() {
io.updateInputs(inputs);
Logger.getInstance().processInputs("Intake", inputs);

io.configureLeadPID(0.05, 0, .7);
io.configureSecondPID(0.045, 0, 4);


if (TUNING_MODE && (kPLead.hasChanged() || kILead.hasChanged() || kDLead.hasChanged())) {
io.configureLeadPID(kPLead.get(), kILead.get(), kDLead.get());
}
if (TUNING_MODE && (kPFollow.hasChanged() || kIFollow.hasChanged() || kDFollow.hasChanged())) {
}

Logger.getInstance().recordOutput("Intake/setpoint", setpoint.getRotations());
}


public void rollerVoltage(double volts){
io.setRollerVoltage(volts);
}

public void setPivotAngle(Rotation2d angle){
System.out.println("Setting Angle Subsystem");
setpoint = angle;
io.setPivotPosition(angle);
}



}
29 changes: 29 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/IntakeIO.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
package frc.robot.subsystems.intake;

import edu.wpi.first.math.geometry.Rotation2d;
import org.littletonrobotics.junction.AutoLog;

public interface IntakeIO {
@AutoLog
class IntakeInputs {
public double pivotVoltage = 0.0;
public double[] pivotCurrent = new double[] {};
public double[] pivotPosition = new double[]{};
public double pivotVelocity = 0.0;
public double rollerVoltage = 0.0;
public double rollerCurrent = 0.0;
}

public default void updateInputs(IntakeInputs inputs) {}

public default void setPivotVoltage(double volts) {}

public default void setRollerVoltage(double volts) {}

public default void setPivotBreakMode(boolean breakMode) {}

public default void setPivotPosition(Rotation2d position){}

public default void configureLeadPID(double kP, double kI, double kD){}
public default void configureSecondPID(double kP, double kI, double kD){}
}
114 changes: 114 additions & 0 deletions src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
package frc.robot.subsystems.intake;

import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
import com.revrobotics.SparkMaxPIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import frc.robot.Constants;

public class IntakeIOReal implements IntakeIO {
private final TalonFX roller = new TalonFX(Constants.Intake.rollerID);
private final CANSparkMax leadSpark =
new CANSparkMax(Constants.Intake.leadPivotID, CANSparkMaxLowLevel.MotorType.kBrushless);
private final CANSparkMax followSpark =
new CANSparkMax(Constants.Intake.followerPivotID, CANSparkMaxLowLevel.MotorType.kBrushless);
private final SparkMaxPIDController leadSparkPID = leadSpark.getPIDController();
private final SparkMaxPIDController followSparkPID = followSpark.getPIDController();

private final StatusSignal<Double> rollerDutyCycle = roller.getDutyCycle();
private final StatusSignal<Double> rollerSupplyVoltage = roller.getSupplyVoltage();
private final StatusSignal<Double> rollerCurrent = roller.getStatorCurrent();

public IntakeIOReal() {
// roller.

leadSpark.restoreFactoryDefaults();
followSpark.restoreFactoryDefaults();

leadSpark.getEncoder().setPosition(0);
followSpark.getEncoder().setPosition(0);
leadSpark.setInverted(false);
followSpark.setInverted(true);

leadSpark.enableVoltageCompensation(12.0);
leadSpark.setSmartCurrentLimit(30);
followSpark.enableVoltageCompensation(12.0);
followSpark.setSmartCurrentLimit(30);

leadSpark.burnFlash();
followSpark.burnFlash();

var config = new TalonFXConfiguration();
config.CurrentLimits.StatorCurrentLimit = 30.0;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
roller.getConfigurator().apply(config);
roller.setInverted(true);

rollerDutyCycle.setUpdateFrequency(50.0);
rollerSupplyVoltage.setUpdateFrequency(50.0);
rollerCurrent.setUpdateFrequency(50.0);
}

@Override
public void updateInputs(IntakeInputs inputs) {
inputs.pivotCurrent =
new double[] {leadSpark.getOutputCurrent(), followSpark.getOutputCurrent()};
inputs.pivotVoltage = leadSpark.getBusVoltage() * leadSpark.getAppliedOutput();
inputs.pivotPosition = new double[]{leadSpark.getEncoder().getPosition(), followSpark.getEncoder().getPosition()};
inputs.pivotVelocity = leadSpark.getEncoder().getVelocity();

rollerDutyCycle.refresh();
rollerSupplyVoltage.refresh();
rollerCurrent.refresh();

inputs.rollerVoltage = rollerDutyCycle.getValue() * rollerSupplyVoltage.getValue();
inputs.rollerCurrent = rollerCurrent.getValue();
}

@Override
public void setPivotVoltage(double volts) {
leadSpark.setVoltage(volts);
}

@Override
public void setRollerVoltage(double volts) {
roller.setControl(new VoltageOut(volts));
// roller.setVoltage(volts);
}

@Override
public void setPivotBreakMode(boolean breakMode) {
CANSparkMax.IdleMode idleMode = CANSparkMax.IdleMode.fromId(breakMode ? 1 : 0);
leadSpark.setIdleMode(idleMode);
followSpark.setIdleMode(idleMode);
}

@Override
public void configureLeadPID(double kP, double kI, double kD) {
leadSparkPID.setP(kP, 0);
leadSparkPID.setI(kI, 0);
leadSparkPID.setD(kD, 0);
leadSparkPID.setFF(0, 0);
}

@Override
public void configureSecondPID(double kP, double kI, double kD){
followSparkPID.setP(kP, 0);
followSparkPID.setI(kI, 0);
followSparkPID.setD(kD, 0);
followSparkPID.setFF(0, 0);
}

@Override
public void setPivotPosition(Rotation2d position) {
System.out.println("Set Pivot Position IO");
leadSparkPID.setReference(position.getRotations(), CANSparkMax.ControlType.kPosition);
followSparkPID.setReference(position.getRotations(), CANSparkMax.ControlType.kPosition);;
}
}
Loading

0 comments on commit 4c04279

Please sign in to comment.