Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 11 additions & 11 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,22 +7,22 @@

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.Transporter.Transporter;


public class RobotContainer {
public static final Transporter TRANSPORTER = new Transporter();

public class RobotContainer
{
public RobotContainer()
{
public RobotContainer() {
configureBindings();
}
private void configureBindings() {}

public Command getAutonomousCommand()
{


private void configureBindings() {
}


public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
}
}
20 changes: 20 additions & 0 deletions src/main/java/frc/robot/subsystems/Transporter/Transporter.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
package frc.robot.subsystems.Transporter;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Transporter extends SubsystemBase {
private final TalonSRX rightMotor = TransporterConstants.RIGHT_MOTOR;
private final TalonSRX leftMotor = TransporterConstants.LEFT_MOTOR;

void stopMotors() {
setTargetPercentageVoltageOutput(0);
}

void setTargetPercentageVoltageOutput(double percentageMotorOutput) {
rightMotor.set(ControlMode.PercentOutput, percentageMotorOutput);
leftMotor.set(ControlMode.PercentOutput, percentageMotorOutput);
}
}

Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package frc.robot.subsystems.Transporter;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import frc.robot.RobotContainer;

public class TransporterCommands {
public static Command getSetTargetStateCommand(TransporterConstants.TransporterState targetState) {
return getSetMotorOutputCommand(targetState.targetMotorOutput);
}

private static Command getSetMotorOutputCommand(double motorOutput) {
return new StartEndCommand(
() -> RobotContainer.TRANSPORTER.setTargetPercentageVoltageOutput(motorOutput),
RobotContainer.TRANSPORTER::stopMotors,
RobotContainer.TRANSPORTER
);
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
package frc.robot.subsystems.Transporter;

import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

public class TransporterConstants {
private static final int
RIGHT_MOTOR_ID = 1,
LEFT_MOTOR_ID = 2;

static final WPI_TalonSRX
RIGHT_MOTOR = new WPI_TalonSRX(RIGHT_MOTOR_ID),
LEFT_MOTOR = new WPI_TalonSRX(LEFT_MOTOR_ID);

private static final InvertType RIGHT_MOTOR_INVERTED_VALUE = InvertType.None;
private static final InvertType LEFT_MOTOR_INVERTED_VALUE = InvertType.InvertMotorOutput;
private static final NeutralMode NEUTRAL_MODE = NeutralMode.Coast;
private static final double VOLTAGE_COMPENSATION_VALUE = 12;

static {
configureRightMotor();
configureLeftMotor();
}

private static final void configureRightMotor() {
RIGHT_MOTOR.configFactoryDefault();

RIGHT_MOTOR.setInverted(RIGHT_MOTOR_INVERTED_VALUE);
RIGHT_MOTOR.setNeutralMode(NEUTRAL_MODE);
RIGHT_MOTOR.enableVoltageCompensation(true);
RIGHT_MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_VALUE);
}

private static final void configureLeftMotor() {
LEFT_MOTOR.configFactoryDefault();

LEFT_MOTOR.setInverted(LEFT_MOTOR_INVERTED_VALUE);
LEFT_MOTOR.setNeutralMode(NEUTRAL_MODE);
LEFT_MOTOR.enableVoltageCompensation(true);
LEFT_MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_VALUE);
}

public enum TransporterState {
COLLECT(0.5),
EJECT(-0.5),
REST(0);

final double targetMotorOutput;

TransporterState(double setTransporterState) {
this.targetMotorOutput = setTransporterState;
}
}
}
Loading