Here are the two .java files. The first is the subsystem, and the second is the command that uses the subsystem. My apologies that these files are not formatted more easily to read. I couldn’t find info on how to format them without being a link from github. I’m happy to change them if someone can point me in the direction of how to do this. (That is why I originally only included a snippet of the code.)
package frc.robot.subsystems;
import com.fasterxml.jackson.annotation.JsonTypeInfo.Id;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import frc.robot.ShuffleboardInfo;
public class Tower extends SubsystemBase {
private final NetworkTableEntry towerUpperMotorEntry, towerLowerMotorEntry, towerLowerSensorEntry, towerUpperSensorEntry;
private final CANSparkMax lowerMotor = new CANSparkMax(Constants.LOWER_TOWER_MOTOR,MotorType.kBrushless);
private final CANSparkMax upperMotor = new CANSparkMax(Constants.UPPER_TOWER_MOTOR,MotorType.kBrushless);
private DigitalInput lowerSensor = new DigitalInput(Constants.LOWER_SENSOR_PORT_INPUT);
private DigitalInput upperSensor = new DigitalInput(Constants.UPPER_SENSOR_PORT_INPUT);
public Tower() {
lowerMotor.restoreFactoryDefaults();
upperMotor.restoreFactoryDefaults();
lowerMotor.setInverted(true);
upperMotor.setInverted(true);
lowerMotor.setIdleMode(IdleMode.kBrake);
upperMotor.setIdleMode(IdleMode.kBrake);
towerUpperMotorEntry = ShuffleboardInfo.getInstance().getTowerUpperMotorEntry();
towerLowerMotorEntry = ShuffleboardInfo.getInstance().getTowerLowerMotorEntry();
towerUpperSensorEntry = ShuffleboardInfo.getInstance().getTowerUpperMotorEntry();
towerLowerSensorEntry = ShuffleboardInfo.getInstance().getTowerLowerMotorEntry();
}
/** Sets the upper motor speed for the tower. */
public void setSpeedUpper(double speed) {
upperMotor.set(speed);
}
/** Returns the set speed of the upper motor. */
public double getSpeedUpper() {
return upperMotor.get();
}
/** Sets the speed of the lower tower motor. */
public void setSpeedLower(double speed) {
lowerMotor.set(speed);
}
/** Returns the set speed of the lower motor */
public double getSpeedLower() {
return lowerMotor.get();
}
public boolean isBallHereUpper() {
return upperSensor.get();
}
public boolean isBallHereLower() {
return lowerSensor.get();
}
public void stop() {
lowerMotor.set(0);
upperMotor.set(0);
}
@Override
public void periodic() {
towerUpperMotorEntry.setDouble(getSpeedUpper());
towerLowerMotorEntry.setDouble(getSpeedLower());
// This does not correctly show the status of the beam break
towerUpperSensorEntry.setBoolean(isBallHereUpper());
towerLowerSensorEntry.setBoolean(isBallHereLower());
}
}
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.Constants;
import frc.robot.subsystems.Tower;
public class LoadTower extends CommandBase {
private Tower tower;
//checks if command should end
private boolean finished;
/** Creates a new ToggleTower. /
/
Both tower motors will spin until sensor is set to false(the laser is tripped)
subsequent presses will turn the motors on and off, respectively
*/
public LoadTower(Tower tower) {
addRequirements(tower);
this.tower = tower;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
tower.setSpeedLower(Constants.TOWER_SPEED);
tower.setSpeedUpper(Constants.TOWER_SPEED);
finished = false;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//sensor to check if balls are in stored position
if(tower.isBallHereUpper()){
tower.setSpeedUpper(0);
}
if(tower.isBallHereLower() && tower.isBallHereUpper()){
tower.setSpeedLower(0);
finished = true;
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
tower.setSpeedUpper(0);
tower.setSpeedLower(0);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
//ends the command if the corresponding main is finished
return finished;
}
}