Skip to content

Commit 49249bf

Browse files
FIRSTTeam4909FIRSTTeam4909
committed
Add logging for Algea and Climber
1 parent ec166e8 commit 49249bf

File tree

7 files changed

+471
-464
lines changed

7 files changed

+471
-464
lines changed

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 117 additions & 107 deletions
Original file line numberDiff line numberDiff line change
@@ -15,111 +15,121 @@
1515
import edu.wpi.first.wpilibj2.command.CommandScheduler;
1616

1717
public class Robot extends LoggedRobot {
18-
private Command m_autonomousCommand;
19-
20-
private final RobotContainer m_robotContainer;
21-
22-
public Robot() {
23-
// Record metadata
24-
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
25-
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
26-
Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
27-
Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
28-
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
29-
switch (BuildConstants.DIRTY) {
30-
case 0:
31-
Logger.recordMetadata("GitDirty", "All changes committed");
32-
break;
33-
case 1:
34-
Logger.recordMetadata("GitDirty", "Uncomitted changes");
35-
break;
36-
default:
37-
Logger.recordMetadata("GitDirty", "Unknown");
38-
break;
39-
}
40-
41-
// Set up data receivers & replay source
42-
switch (Constants.currentMode) {
43-
case REAL:
44-
// Running on a real robot, log to a USB stick ("/U/logs")
45-
Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs"));
46-
Logger.addDataReceiver(new NT4Publisher());
47-
break;
48-
49-
case SIM:
50-
// Running a physics simulator, log to NT
51-
Logger.addDataReceiver(new NT4Publisher());
52-
break;
53-
54-
case REPLAY:
55-
// Replaying a log, set up replay source
56-
// setUseTiming(false); // Run as fast as possible
57-
// String logPath = LogFileUtil.findReplayLog();
58-
// Logger.setReplaySource(new WPILOGReader(logPath));
59-
// Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
60-
// break;
61-
}
62-
63-
// Start AdvantageKit logger
64-
Logger.start();
65-
m_robotContainer = new RobotContainer();
66-
67-
68-
}
69-
70-
@Override
71-
public void robotPeriodic() {
72-
CommandScheduler.getInstance().run();
73-
}
74-
75-
@Override
76-
public void disabledInit() {}
77-
78-
@Override
79-
public void disabledPeriodic() {}
80-
81-
@Override
82-
public void disabledExit() {}
83-
84-
@Override
85-
public void autonomousInit() {
86-
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
87-
88-
if (m_autonomousCommand != null) {
89-
m_autonomousCommand.schedule();
90-
}
91-
}
92-
93-
@Override
94-
public void autonomousPeriodic() {}
95-
96-
@Override
97-
public void autonomousExit() {}
98-
99-
@Override
100-
public void teleopInit() {
101-
if (m_autonomousCommand != null) {
102-
m_autonomousCommand.cancel();
103-
}
104-
}
105-
106-
@Override
107-
public void teleopPeriodic() {}
108-
109-
@Override
110-
public void teleopExit() {}
111-
112-
@Override
113-
public void testInit() {
114-
CommandScheduler.getInstance().cancelAll();
115-
}
116-
117-
@Override
118-
public void testPeriodic() {}
119-
120-
@Override
121-
public void testExit() {}
122-
123-
@Override
124-
public void simulationPeriodic() {}
18+
private Command m_autonomousCommand;
19+
20+
private final RobotContainer m_robotContainer;
21+
22+
public Robot() {
23+
// Record metadata
24+
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
25+
Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
26+
Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
27+
Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
28+
Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
29+
switch (BuildConstants.DIRTY) {
30+
case 0:
31+
Logger.recordMetadata("GitDirty", "All changes committed");
32+
break;
33+
case 1:
34+
Logger.recordMetadata("GitDirty", "Uncomitted changes");
35+
break;
36+
default:
37+
Logger.recordMetadata("GitDirty", "Unknown");
38+
break;
39+
}
40+
41+
// Set up data receivers & replay source
42+
switch (Constants.currentMode) {
43+
case REAL:
44+
// Running on a real robot, log to a USB stick ("/U/logs")
45+
Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs"));
46+
Logger.addDataReceiver(new NT4Publisher());
47+
break;
48+
49+
case SIM:
50+
// Running a physics simulator, log to NT
51+
Logger.addDataReceiver(new NT4Publisher());
52+
break;
53+
54+
case REPLAY:
55+
// Replaying a log, set up replay source
56+
// setUseTiming(false); // Run as fast as possible
57+
// String logPath = LogFileUtil.findReplayLog();
58+
// Logger.setReplaySource(new WPILOGReader(logPath));
59+
// Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath,
60+
// "_sim")));
61+
// break;
62+
}
63+
64+
// Start AdvantageKit logger
65+
Logger.start();
66+
m_robotContainer = new RobotContainer();
67+
68+
}
69+
70+
@Override
71+
public void robotPeriodic() {
72+
CommandScheduler.getInstance().run();
73+
}
74+
75+
@Override
76+
public void disabledInit() {
77+
}
78+
79+
@Override
80+
public void disabledPeriodic() {
81+
}
82+
83+
@Override
84+
public void disabledExit() {
85+
}
86+
87+
@Override
88+
public void autonomousInit() {
89+
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
90+
91+
if (m_autonomousCommand != null) {
92+
m_autonomousCommand.schedule();
93+
}
94+
}
95+
96+
@Override
97+
public void autonomousPeriodic() {
98+
}
99+
100+
@Override
101+
public void autonomousExit() {
102+
}
103+
104+
@Override
105+
public void teleopInit() {
106+
if (m_autonomousCommand != null) {
107+
m_autonomousCommand.cancel();
108+
}
109+
}
110+
111+
@Override
112+
public void teleopPeriodic() {
113+
}
114+
115+
@Override
116+
public void teleopExit() {
117+
}
118+
119+
@Override
120+
public void testInit() {
121+
CommandScheduler.getInstance().cancelAll();
122+
}
123+
124+
@Override
125+
public void testPeriodic() {
126+
}
127+
128+
@Override
129+
public void testExit() {
130+
}
131+
132+
@Override
133+
public void simulationPeriodic() {
134+
}
125135
}

‎src/main/java/frc/robot/subsystems/Climber/Climber.java‎

Lines changed: 48 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -1,68 +1,57 @@
11
package frc.robot.subsystems.Climber;
22

33
import org.littletonrobotics.junction.Logger;
4-
5-
import edu.wpi.first.networktables.DoublePublisher;
6-
import edu.wpi.first.networktables.NetworkTable;
7-
import edu.wpi.first.networktables.NetworkTableInstance;
84
import edu.wpi.first.wpilibj2.command.Command;
95
import edu.wpi.first.wpilibj2.command.SubsystemBase;
106

117
public class Climber extends SubsystemBase {
12-
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
13-
private final NetworkTable climberTable = inst.getTable("Climber");
14-
private final DoublePublisher motorVelPub= climberTable.getDoubleTopic("Motor Vel").publish();
15-
private final DoublePublisher motorVolPub= climberTable.getDoubleTopic("Motor Vol").publish();
16-
private final DoublePublisher rotPub= climberTable.getDoubleTopic("Rotations").publish();
17-
private final DoublePublisher setPub= climberTable.getDoubleTopic("Setpoint").publish();
18-
19-
private ClimberIO m_io;
20-
private final double climb = -190.866;
21-
private final double winched = 22;
22-
23-
final double m_gearRatio = 1d;
24-
25-
public Climber(ClimberIO io) {
26-
m_io = io;
27-
}
28-
29-
public Command climb() {
30-
return this.run(() -> m_io.setSpeed(-1));
31-
}
32-
33-
public Command stop() {
34-
return this.run(() -> m_io.setSpeed(0));
35-
}
36-
37-
public Command lower() {
38-
return this.run(() -> m_io.setSpeed(1));
39-
}
40-
41-
public Command climbPosition() {
42-
return this.runOnce(() -> {
43-
m_io.gotosetpoint(climb, m_gearRatio);
44-
});
45-
}
46-
47-
public Command winchedPosition() {
48-
return this.runOnce(() -> {
49-
m_io.gotosetpoint(winched, m_gearRatio);
50-
});
51-
}
52-
53-
public Command reZero(){
54-
return this.runOnce(() -> {
55-
m_io.setPosition(0.0);
56-
}).withName("ReZero");
57-
}
8+
private ClimberIO m_io;
9+
private final ClimberIOinputsAutoLogged m_inputs = new ClimberIOinputsAutoLogged();
10+
11+
private final double climb = -190.866;
12+
private final double winched = 22;
13+
14+
final double m_gearRatio = 1d;
15+
16+
public Climber(ClimberIO io) {
17+
super("Climber");
18+
m_io = io;
19+
}
20+
21+
public Command climb() {
22+
return this.run(() -> m_io.setSpeed(-1));
23+
}
24+
25+
public Command stop() {
26+
return this.run(() -> m_io.setSpeed(0));
27+
}
28+
29+
public Command lower() {
30+
return this.run(() -> m_io.setSpeed(1));
31+
}
32+
33+
public Command climbPosition() {
34+
return this.runOnce(() -> {
35+
m_io.gotosetpoint(climb, m_gearRatio);
36+
});
37+
}
38+
39+
public Command winchedPosition() {
40+
return this.runOnce(() -> {
41+
m_io.gotosetpoint(winched, m_gearRatio);
42+
});
43+
}
44+
45+
public Command reZero() {
46+
return this.runOnce(() -> {
47+
m_io.setPosition(0.0);
48+
}).withName("ReZero");
49+
}
50+
51+
@Override
52+
public void periodic() {
53+
m_io.updateInputs(m_inputs);
54+
Logger.processInputs(getName() + "/", m_inputs);
55+
}
5856

59-
@Override
60-
public void periodic() {
61-
super.periodic();
62-
motorVelPub.set(m_io.getVelocity());
63-
motorVolPub.set(m_io.getVoltage());
64-
setPub.set(m_io.getSetpoint());
65-
rotPub.set(m_io.getPosition());
66-
}
67-
6857
}

‎src/main/java/frc/robot/subsystems/Climber/ClimberIO.java‎

Lines changed: 15 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,31 +1,26 @@
11
package frc.robot.subsystems.Climber;
22

3+
import org.littletonrobotics.junction.AutoLog;
34

45
public interface ClimberIO {
5-
6-
public static class ClimberIOinputs{
7-
public double velocity = 0.0;
86

9-
public double motorPosition = 0.0;
10-
11-
public double motorVoltage = 0.0;
12-
}
7+
@AutoLog
8+
public static class ClimberIOinputs {
9+
public double velocity = 0.0;
10+
public double motorPosition = 0.0;
11+
public double motorSetpoint = 0.0;
12+
public double motorSupplyCurrent = 0.0;
13+
public double motorVoltage = 0.0;
14+
public boolean motorConnected = false;
15+
}
1316

14-
public default void setSpeed(double speed) {}
17+
public void setSpeed(double speed);
1518

16-
public default void setBrakeMode(boolean enableBrakeMode) {}
19+
public void setBrakeMode(boolean enableBrakeMode);
1720

18-
public default void updateInputs(ClimberIOinputs inputs) {}
21+
public void updateInputs(ClimberIOinputs inputs);
1922

20-
public default void gotosetpoint(double setpoint, double gearRatio) {}
23+
public void gotosetpoint(double setpoint, double gearRatio);
2124

22-
public default double getVelocity() { return 0; }
23-
24-
public default double getVoltage() { return 0; }
25-
26-
public default double getPosition() { return 0; }
27-
28-
public default double getSetpoint() { return 0; }
29-
30-
public default void setPosition(double position) {}
25+
public void setPosition(double position);
3126
}

0 commit comments

Comments
 (0)