Skip to content

Commit 675cff3

Browse files
blaze-developerblaze-developer
committed
Merge motor subsystem
2 parents 03b18a5 + a9449ba commit 675cff3

4 files changed

Lines changed: 202 additions & 2 deletions

File tree

‎src/main/java/first/robot/RobotContainer.java‎

Lines changed: 32 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,12 @@
77

88
package first.robot;
99

10-
import static org.wpilib.units.Units.*;
10+
import static org.wpilib.units.Units.FeetPerSecond;
11+
import static org.wpilib.units.Units.MetersPerSecond;
12+
import static org.wpilib.units.Units.RadiansPerSecond;
13+
import static org.wpilib.units.Units.Volts;
1114

15+
import com.ctre.phoenix6.CANBus;
1216
import first.generated.CompTunerConstants;
1317
import first.robot.Constants.DriveConstants;
1418
import first.robot.commands.drive.DriveCommands;
@@ -17,13 +21,22 @@
1721
import first.robot.subsystems.drive.GyroIOPigeon2;
1822
import first.robot.subsystems.drive.ModuleIOReplay;
1923
import first.robot.subsystems.drive.ModuleIOTalonFX;
24+
import first.robot.subsystems.motor.Motor;
25+
import first.robot.subsystems.motor.MotorIO;
26+
import first.robot.subsystems.motor.MotorIOHardware;
27+
import first.robot.subsystems.vision.AprilTagVision;
28+
import first.robot.subsystems.vision.CameraIO;
29+
import first.robot.subsystems.vision.CameraIOLimelight;
2030
import org.wpilib.command2.Command;
2131
import org.wpilib.command2.Commands;
2232
import org.wpilib.command2.button.CommandXboxController;
33+
import org.wpilib.hardware.bus.CANPort;
2334

2435
public class RobotContainer {
2536

2637
private Drive drive;
38+
private AprilTagVision vision;
39+
private Motor motor;
2740

2841
private final CommandXboxController gamepad = new CommandXboxController(0);
2942

@@ -41,6 +54,13 @@ public RobotContainer() {
4154
CompTunerConstants.BackRight,
4255
CompTunerConstants.kCANBus,
4356
CompTunerConstants.kSpeedAt12Volts);
57+
58+
motor = new Motor(new MotorIOHardware(4, new CANBus(CANPort.CAN_S4)));
59+
60+
vision =
61+
new AprilTagVision(
62+
drive::addVisionMeasurement,
63+
new CameraIOLimelight("limelight", drive::getRotation));
4464
break;
4565
case SIMBOT:
4666
drive =
@@ -69,6 +89,14 @@ public RobotContainer() {
6989
CompTunerConstants.kSpeedAt12Volts);
7090
}
7191

92+
if (motor == null) {
93+
motor = new Motor(new MotorIO() {});
94+
}
95+
96+
if (vision == null) {
97+
vision = new AprilTagVision(drive::addVisionMeasurement, new CameraIO() {});
98+
}
99+
72100
DriveCommands.configure(
73101
drive, () -> -gamepad.getLeftY(), () -> -gamepad.getLeftX(), () -> -gamepad.getRightX());
74102

@@ -77,7 +105,9 @@ public RobotContainer() {
77105
}
78106

79107
// Bind robot actions to commands here.
80-
private void configureBindings() {}
108+
private void configureBindings() {
109+
gamepad.rightTrigger().and(gamepad.leftTrigger()).whileTrue(motor.runVoltage(Volts.of(6.0)));
110+
}
81111

82112
private void configureDriveBindings() {
83113
// Default command, normal field-relative drive

‎src/main/java/first/robot/subsystems/motor/Motor.java‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
// Copyright (c) 2026 FRC 1425 Error Code Xero
2+
// http://github.com/errorcodexero
3+
//
4+
// Use of this source code is governed by an MIT-style
5+
// license that can be found in the LICENSE file at
6+
// the root directory of this project.
7+
8+
package first.robot.subsystems.motor;
9+
10+
import org.littletonrobotics.junction.Logger;
11+
import org.wpilib.command2.Command;
12+
import org.wpilib.command2.SubsystemBase;
13+
import org.wpilib.units.measure.AngularVelocity;
14+
import org.wpilib.units.measure.Voltage;
15+
16+
public class Motor extends SubsystemBase {
17+
18+
private final MotorIO io;
19+
private final MotorInputsAutoLogged inputs = new MotorInputsAutoLogged();
20+
21+
public Motor(MotorIO io) {
22+
this.io = io;
23+
}
24+
25+
@Override
26+
public void periodic() {
27+
io.updateInputs(inputs);
28+
Logger.processInputs("Motor", inputs);
29+
}
30+
31+
/**
32+
* A command that runs the motor at a certain voltage and stops when done
33+
*
34+
* @param volts
35+
* @return
36+
*/
37+
public Command runVoltage(Voltage volts) {
38+
return startEnd(() -> io.setVoltage(volts), io::stop);
39+
}
40+
41+
/**
42+
* A command that run the motor at a certain velocity and stops when done
43+
*
44+
* @param velocity
45+
* @return
46+
*/
47+
public Command runVelocity(AngularVelocity velocity) {
48+
return startEnd(() -> io.setVelocity(velocity), io::stop);
49+
}
50+
}

‎src/main/java/first/robot/subsystems/motor/MotorIO.java‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
// Copyright (c) 2026 FRC 1425 Error Code Xero
2+
// http://github.com/errorcodexero
3+
//
4+
// Use of this source code is governed by an MIT-style
5+
// license that can be found in the LICENSE file at
6+
// the root directory of this project.
7+
8+
package first.robot.subsystems.motor;
9+
10+
import static org.wpilib.units.Units.Amps;
11+
import static org.wpilib.units.Units.Rotations;
12+
import static org.wpilib.units.Units.RotationsPerSecond;
13+
import static org.wpilib.units.Units.Volts;
14+
15+
import org.littletonrobotics.junction.AutoLog;
16+
import org.wpilib.units.measure.Angle;
17+
import org.wpilib.units.measure.AngularVelocity;
18+
import org.wpilib.units.measure.Current;
19+
import org.wpilib.units.measure.Voltage;
20+
21+
public interface MotorIO {
22+
@AutoLog
23+
public static class MotorInputs {
24+
public Angle position = Rotations.zero();
25+
public AngularVelocity velocity = RotationsPerSecond.zero();
26+
27+
public Voltage voltage = Volts.zero();
28+
public Current current = Amps.zero();
29+
30+
public boolean connected = false;
31+
}
32+
33+
public default void updateInputs(MotorInputs inputs) {}
34+
35+
public default void setVoltage(Voltage volts) {}
36+
37+
public default void setVelocity(AngularVelocity vel) {}
38+
39+
public default void stop() {}
40+
}

‎src/main/java/first/robot/subsystems/motor/MotorIOHardware.java‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
// Copyright (c) 2026 FRC 1425 Error Code Xero
2+
// http://github.com/errorcodexero
3+
//
4+
// Use of this source code is governed by an MIT-style
5+
// license that can be found in the LICENSE file at
6+
// the root directory of this project.
7+
8+
package first.robot.subsystems.motor;
9+
10+
import static org.wpilib.units.Units.Hertz;
11+
12+
import com.ctre.phoenix6.BaseStatusSignal;
13+
import com.ctre.phoenix6.CANBus;
14+
import com.ctre.phoenix6.StatusSignal;
15+
import com.ctre.phoenix6.configs.Slot0Configs;
16+
import com.ctre.phoenix6.controls.VelocityVoltage;
17+
import com.ctre.phoenix6.controls.VoltageOut;
18+
import com.ctre.phoenix6.hardware.TalonFX;
19+
import first.robot.util.PhoenixUtil;
20+
import org.wpilib.units.measure.Angle;
21+
import org.wpilib.units.measure.AngularVelocity;
22+
import org.wpilib.units.measure.Current;
23+
import org.wpilib.units.measure.Voltage;
24+
25+
public class MotorIOHardware implements MotorIO {
26+
private final TalonFX motor;
27+
28+
private final StatusSignal<Angle> position;
29+
private final StatusSignal<AngularVelocity> velocity;
30+
private final StatusSignal<Voltage> voltage;
31+
private final StatusSignal<Current> current;
32+
33+
public MotorIOHardware(int canid, CANBus bus) {
34+
motor = new TalonFX(canid, bus);
35+
36+
position = motor.getPosition();
37+
velocity = motor.getVelocity();
38+
voltage = motor.getMotorVoltage();
39+
current = motor.getSupplyCurrent();
40+
41+
BaseStatusSignal.setUpdateFrequencyForAll(Hertz.of(50), position, velocity, voltage, current);
42+
43+
motor.optimizeBusUtilization();
44+
45+
Slot0Configs config = new Slot0Configs();
46+
47+
config.kP = 2.0;
48+
config.kI = 0.0;
49+
config.kD = 0.0;
50+
51+
PhoenixUtil.tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25));
52+
}
53+
54+
@Override
55+
public void updateInputs(MotorInputs inputs) {
56+
var status = BaseStatusSignal.refreshAll(position, velocity, voltage, current);
57+
58+
inputs.connected = status.isOK();
59+
60+
inputs.position = position.getValue();
61+
inputs.velocity = velocity.getValue();
62+
inputs.voltage = voltage.getValue();
63+
inputs.current = current.getValue();
64+
}
65+
66+
@Override
67+
public void setVoltage(Voltage volts) {
68+
motor.setControl(new VoltageOut(volts));
69+
}
70+
71+
@Override
72+
public void setVelocity(AngularVelocity vel) {
73+
motor.setControl(new VelocityVoltage(vel));
74+
}
75+
76+
@Override
77+
public void stop() {
78+
motor.stopMotor();
79+
}
80+
}

0 commit comments

Comments
 (0)