Skip to content

Commit

Permalink
Merge branch 'staging' into intake
Browse files Browse the repository at this point in the history
  • Loading branch information
rutmanz authored Feb 3, 2024
2 parents 3195a97 + 3879f28 commit 5181091
Show file tree
Hide file tree
Showing 8 changed files with 195 additions and 6 deletions.
47 changes: 43 additions & 4 deletions src/main/java/org/team1540/robot2024/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -76,10 +76,6 @@ public static class Indexer {

}

public static class Elevator {
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(21.0);
}

public static class Shooter {
public static class Flywheels {
// TODO: determine ids
Expand Down Expand Up @@ -132,4 +128,47 @@ public static class Pivot {
public static final Rotation2d ERROR_TOLERANCE = Rotation2d.fromDegrees(0.2);
}
}

public static class Elevator {
public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25);
public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0);
public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.0);
public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0);
public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT;

public enum ElevatorState {
/**
* At max height :D
*/
TOP(ELEVATOR_MAX_HEIGHT),
/**
* At minimum height :D
*/
BOTTOM(ELEVATOR_MINIMUM_HEIGHT),
/**
* At height for top of initial climb :D
*/
CLIMB(254.0), //TODO: Find these values :D
/**
* At height for trap doing :D
*/
TRAP(254.0), //TODO: Find these values :D
/**
* At height for top of initial climb :D
*/
AMP(254.0); //TODO: Find these values :D

public final double heightMeters;
ElevatorState(double heightMeters) {
this.heightMeters = heightMeters;
}
}
}

public static class Tramp {
public static final double GEAR_RATIO = 3.0 / 1.0;
public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
public static final int TRAMP_MOTOR_ID = -1; //TODO: Configure this later
public static final int TRAMP_BEAM_BREAK_CHANNEL = -1; //TODO: Configure this later
}
}
13 changes: 11 additions & 2 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,10 @@
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.commands.indexer.IntakeCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.tramp.Tramp;
import org.team1540.robot2024.subsystems.tramp.TrampIO;
import org.team1540.robot2024.subsystems.tramp.TrampIOSim;
import org.team1540.robot2024.subsystems.tramp.TrampIOSparkMax;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.indexer.Indexer;
import org.team1540.robot2024.subsystems.indexer.IndexerIO;
Expand All @@ -33,6 +37,7 @@
public class RobotContainer {
// Subsystems
public final Drivetrain drivetrain;
public final Tramp tramp;
public final Shooter shooter;
public final Indexer indexer;

Expand Down Expand Up @@ -62,14 +67,13 @@ public RobotContainer() {
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT)),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT)));
tramp = new Tramp(new TrampIOSparkMax());
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());

indexer =
new Indexer(
new IndexerIOSparkMax()
);
break;

case SIM:
// Sim robot, instantiate physics sim IO implementations
drivetrain =
Expand All @@ -79,6 +83,7 @@ public RobotContainer() {
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim());
tramp = new Tramp(new TrampIOSim());
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
indexer =
new Indexer(
Expand All @@ -96,10 +101,14 @@ public RobotContainer() {
new ModuleIO() {},
new ModuleIO() {});
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});

indexer =
new Indexer(
new IndexerIO() {}
);

tramp = new Tramp(new TrampIO() {});

break;
}

Expand Down
33 changes: 33 additions & 0 deletions src/main/java/org/team1540/robot2024/commands/TrampCommand.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
package org.team1540.robot2024.commands;

import org.team1540.robot2024.subsystems.tramp.Tramp;

import edu.wpi.first.wpilibj2.command.Command;

public class TrampCommand extends Command {

private final Tramp tramp;

public TrampCommand(Tramp tramp) {
this.tramp = tramp;
addRequirements(tramp);
}

@Override
public void initialize() {
tramp.setPercent(0.5);
}

@Override
public void execute() {}

@Override
public boolean isFinished() {
return tramp.isBeamBreakBlocked();
}

@Override
public void end(boolean interrupted) {
tramp.stopTramp();
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ public ShooterPivotIOTalonFX() {
motorConfig.Slot0.kS = KS;
motorConfig.Slot0.kG = KG;
motorConfig.Slot0.kV = KV;
motorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;

motorConfig.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY_RPS;
motorConfig.MotionMagic.MotionMagicAcceleration = MAX_ACCEL_RPS2;
Expand Down
33 changes: 33 additions & 0 deletions src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
package org.team1540.robot2024.subsystems.tramp;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;

public class Tramp extends SubsystemBase {
private final TrampIO io;
private final TrampIOInputsAutoLogged inputs = new TrampIOInputsAutoLogged();

public Tramp(TrampIO io) {
this.io = io;
}

public void setPercent(double percentage) {
io.setVoltage(12.0 * percentage);
}

public boolean isBeamBreakBlocked() {
return inputs.breamBreakTripped;
}

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

public void stopTramp() {
io.setVoltage(0);
}

}
16 changes: 16 additions & 0 deletions src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
package org.team1540.robot2024.subsystems.tramp;

import org.littletonrobotics.junction.AutoLog;


public interface TrampIO {
@AutoLog
class TrampIOInputs {
public boolean breamBreakTripped = false;
public double velocityRPM = 0.0;
public double appliedVolts = 0.0;
public double currentAmps = 0.0;
}
default void setVoltage(double volts) {}
default void updateInputs(TrampIOInputs inputs) {}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
package org.team1540.robot2024.subsystems.tramp;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import org.team1540.robot2024.Constants;

import static org.team1540.robot2024.Constants.Tramp.GEAR_RATIO;

public class TrampIOSim implements TrampIO{
private final DCMotorSim sim = new DCMotorSim(DCMotor.getNEO(1), GEAR_RATIO,0.025); //TODO: Fix MOI its probably wrong :D

private double appliedVolts = 0.0;
@Override
public void updateInputs(TrampIOInputs inputs) {
sim.update(Constants.LOOP_PERIOD_SECS);
inputs.velocityRPM = sim.getAngularVelocityRPM();
inputs.appliedVolts = appliedVolts;
inputs.currentAmps = sim.getCurrentDrawAmps();
}

@Override
public void setVoltage(double volts) {
appliedVolts = MathUtil.clamp(volts, -12, 12);
sim.setInputVoltage(appliedVolts);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
package org.team1540.robot2024.subsystems.tramp;

import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.wpilibj.DigitalInput;
import org.team1540.robot2024.Constants.Tramp;

public class TrampIOSparkMax implements TrampIO {
private final DigitalInput beamBreak = new DigitalInput(0);
//TODO: Potentially change name :D
private final CANSparkMax motor = new CANSparkMax(Tramp.TRAMP_MOTOR_ID, MotorType.kBrushless);
private final RelativeEncoder motorEncoder = motor.getEncoder();
public TrampIOSparkMax() {
motor.setIdleMode(CANSparkMax.IdleMode.kBrake);
//Potentially invert motor
motor.setSmartCurrentLimit(40);
motor.enableVoltageCompensation(12);
}
@Override
public void setVoltage(double volts) {
motor.setVoltage(volts);
}

public void updateInputs(TrampIOInputs inputs) {
inputs.breamBreakTripped = !(beamBreak.get()); //I think returns false when broken... Returns true when broken now.
inputs.velocityRPM = motorEncoder.getVelocity();
inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage();
inputs.currentAmps = motor.getOutputCurrent();
}
}

0 comments on commit 5181091

Please sign in to comment.