Skip to content

Commit

Permalink
feat: Intake & Indexer IO and subsystem
Browse files Browse the repository at this point in the history
  • Loading branch information
AdleyJ committed Jan 27, 2024
1 parent a491418 commit 61e7def
Show file tree
Hide file tree
Showing 6 changed files with 142 additions and 43 deletions.
7 changes: 7 additions & 0 deletions src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,13 @@ public static class Drivetrain {
}
public static class Indexer {
public static final int INTAKE_ID = 0;
public static final int FEEDER_ID = 0;
public static final double FEEDER_KP = 0.0;
public static final double FEEDER_KI = 0.0;
public static final double FEEDER_KD = 0.0;
public static final double FEEDER_KS = 0.0;
public static final double FEEDER_KV = 0.0;
public static final double GEAR_RATIO = 0.0;

}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
package org.team1540.robot2024.commands.indexer;

import edu.wpi.first.wpilibj2.command.Command;
import org.team1540.robot2024.subsystems.indexer.Indexer;

public class IntakeCommand extends Command {

private final Indexer indexer;

public IntakeCommand(Indexer indexer) {
this.indexer = indexer;
addRequirements(indexer);
}

@Override
public void initialize() {
indexer.setIntakePercent(0.4);
}


@Override
public boolean isFinished() {
return indexer.noteInIndexer();
}

@Override
public void end(boolean interrupted) {
indexer.setIntakePercent(0);
}
}
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
package org.team1540.robot2024.subsystems.indexer;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;

Expand All @@ -17,14 +20,29 @@ public void periodic() {
Logger.processInputs("Indexer", indexerInputs);
}

public void setIntakeSpeed(double speed) {
indexerIO.setIntakeSpeed(speed);
public void setIntakePercent(double percent) {
indexerIO.setIntakeVoltage(percent * 12.0);
}

public void setIntakeVoltage(double volts) {
indexerIO.setIntakeVoltage(volts);
public boolean noteInIndexer() {
return indexerInputs.noteInIntake;
}

public Command feedToAmp() {
return Commands.sequence(
Commands.runOnce(() -> indexerIO.setFeederVelocity(-600), this),
Commands.waitSeconds(5),
Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this)
);
}

public Command feedToShooter() {
return Commands.sequence(
Commands.runOnce(() -> indexerIO.setFeederVelocity(1200), this),
Commands.waitSeconds(1),
Commands.runOnce(() -> indexerIO.setFeederVelocity(0), this)
);
}


}
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,13 @@ public interface IndexerIO {

@AutoLog
class IndexerIOInputs {
public double intakeVoltage = 0.0;
public double intakeCurrent = 0.0;
public double intakeVelocityRPM = 0.0;
public double intakeVoltage;
public double intakeCurrent;
public double intakeVelocityRPM;
public double feederVoltage;
public double feederCurrentAmps;
public double feederVelocityRPM;
public boolean noteInIntake;
}

/**
Expand All @@ -18,8 +22,8 @@ default void updateInputs(IndexerIOInputs inputs) {}

default void setIntakeVoltage(double volts) {}

default void setIntakeSpeed(double speed) {}

default void setFeederVoltage(double volts) {}

default void setFeederVelocity(double velocity) {}

}

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
package org.team1540.robot2024.subsystems.indexer;

import com.revrobotics.*;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DigitalInput;

import static org.team1540.robot2024.Constants.Indexer.*;


public class IndexerIOSparkMax implements IndexerIO {
private final CANSparkMax intakeMotor = new CANSparkMax(INTAKE_ID, CANSparkLowLevel.MotorType.kBrushless);
private final CANSparkMax feederMotor = new CANSparkMax(FEEDER_ID, CANSparkLowLevel.MotorType.kBrushless);
DigitalInput indexerBeamBreak = new DigitalInput(0);
private final SparkPIDController feederPID;
private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);


public IndexerIOSparkMax() {
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
intakeMotor.enableVoltageCompensation(12.0);
intakeMotor.setSmartCurrentLimit(30);

feederMotor.setIdleMode(CANSparkBase.IdleMode.kBrake);
feederMotor.enableVoltageCompensation(12.0);
feederMotor.setSmartCurrentLimit(30);

feederPID = feederMotor.getPIDController();
feederPID.setP(FEEDER_KP, 0);
feederPID.setI(FEEDER_KI, 0);
feederPID.setD(FEEDER_KD, 0);



}
// 2 jobs,
// 1. be able to route to tramp
// 2. feeding the shooter (by default, be still, spin up fast 4 for shooter.
// make cmd factory spin
// slower in other direction for tramp

@Override
public void updateInputs(IndexerIOInputs inputs) {
inputs.intakeCurrent = intakeMotor.getOutputCurrent();
inputs.intakeVoltage = intakeMotor.getBusVoltage() * intakeMotor.getAppliedOutput();
inputs.intakeVelocityRPM = intakeMotor.getEncoder().getVelocity();
inputs.feederVoltage = feederMotor.getOutputCurrent();
inputs.feederCurrentAmps = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
inputs.noteInIntake = indexerBeamBreak.get();
}

@Override
public void setIntakeVoltage(double volts) {
intakeMotor.setVoltage(volts);
}

@Override
public void setFeederVoltage(double volts) {
feederMotor.setVoltage(volts);
}

@Override
public void setFeederVelocity(double velocity) {
feederPID.setReference(
Units.radiansPerSecondToRotationsPerMinute(velocity) * GEAR_RATIO,
CANSparkBase.ControlType.kVelocity,
0,
feederFF.calculate(velocity),
SparkPIDController.ArbFFUnits.kVoltage
);
}
}
// for elevator sim, app. voltage

0 comments on commit 61e7def

Please sign in to comment.