Skip to content

Commit de622a3

Browse files
tomtzooktomtzook
committed
Merge branch 'development'
2 parents 47b8d72 + c8dcc58 commit de622a3

10 files changed

Lines changed: 286 additions & 5 deletions

File tree

‎flashlib.frc.robot/build.gradle‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,5 +5,6 @@ dependencies {
55

66
api group: 'edu.wpi.first.wpilibj', name: 'wpilibj-java', version: WPILIB_VERSION
77
api group: 'edu.wpi.first.wpiutil', name: 'wpiutil-java', version: WPILIB_VERSION
8+
api group: 'edu.wpi.first.wpimath', name: 'wpimath-java', version: WPILIB_VERSION
89
api group: 'edu.wpi.first.hal', name: 'hal-java', version: WPILIB_VERSION
910
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/FrcRobotControl.java‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,4 +22,22 @@ public interface FrcRobotControl extends RobotControl {
2222
* @return {@link RobotFileSystem} component.
2323
*/
2424
RobotFileSystem getFileSystem();
25+
26+
/**
27+
* Gets the current type of runtime for the robot code.
28+
*
29+
* @return runtime type.
30+
*/
31+
RuntimeType getRuntimeType();
32+
33+
/**
34+
* Gets whether the current runtime type is simulation, meaning
35+
* that the robot code is executing in a simulated environment,
36+
*
37+
* @return <b>true</b> if in simulation, <b>false</b> otherwise.
38+
* @see #getRuntimeType()
39+
*/
40+
default boolean isRuntimeSimulation() {
41+
return getRuntimeType() == RuntimeType.SIMULATION;
42+
}
2543
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/FrcRobotControlImpl.java‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717
import com.flash3388.flashlib.time.Clock;
1818
import com.flash3388.flashlib.util.resources.ResourceHolder;
1919
import edu.wpi.first.wpilibj.DriverStation;
20+
import edu.wpi.first.wpilibj.RobotBase;
21+
import edu.wpi.first.wpilibj.RobotState;
2022
import org.slf4j.Logger;
2123

2224
import java.util.Collection;
@@ -102,6 +104,18 @@ public RobotFileSystem getFileSystem() {
102104
return mRobotFileSystem;
103105
}
104106

107+
@Override
108+
public RuntimeType getRuntimeType() {
109+
if (RobotBase.isReal()) {
110+
return RuntimeType.REAL;
111+
}
112+
if (RobotBase.isSimulation()) {
113+
return RuntimeType.SIMULATION;
114+
}
115+
116+
throw new AssertionError("unknown RuntimeType");
117+
}
118+
105119
@Override
106120
public void registerCloseables(Collection<? extends AutoCloseable> closeables) {
107121
mResourceHolder.add(closeables);

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/RobotMain.java‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22

33
import com.flash3388.flashlib.frc.robot.base.iterative.IterativeFrcRobot;
44
import com.flash3388.flashlib.frc.robot.base.iterative.LoopingRobotBase;
5-
import com.flash3388.flashlib.robot.RunningRobot;
65
import com.flash3388.flashlib.util.resources.ResourceHolder;
76
import edu.wpi.first.wpilibj.RobotBase;
87

@@ -19,15 +18,15 @@ public static void start(IterativeFrcRobot.Initializer initializer, RobotConfigu
1918
ResourceHolder resourceHolder = ResourceHolder.empty();
2019

2120
FrcRobotControl robotControl = new FrcRobotControlImpl(resourceHolder, configuration);
22-
RunningRobot.setControlInstance(robotControl);
21+
RunningFrcRobot.setControlInstance(robotControl);
2322
RobotBase.startRobot(()-> new LoopingRobotBase(initializer, robotControl, resourceHolder));
2423
}
2524

2625
public static void start(IterativeFrcRobot.Initializer initializer) {
2726
ResourceHolder resourceHolder = ResourceHolder.empty();
2827

2928
FrcRobotControl robotControl = new FrcRobotControlImpl(resourceHolder);
30-
RunningRobot.setControlInstance(robotControl);
29+
RunningFrcRobot.setControlInstance(robotControl);
3130
RobotBase.startRobot(()-> new LoopingRobotBase(initializer, robotControl, resourceHolder));
3231
}
3332
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/RunningFrcRobot.java‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
package com.flash3388.flashlib.frc.robot;
2+
3+
import com.flash3388.flashlib.global.GlobalDependencies;
4+
import com.flash3388.flashlib.robot.RobotControl;
5+
import com.flash3388.flashlib.robot.RunningRobot;
6+
7+
import java.util.concurrent.atomic.AtomicReference;
8+
9+
public class RunningFrcRobot {
10+
11+
private RunningFrcRobot() {}
12+
13+
private static final AtomicReference<FrcRobotControl> sControlInstance = new AtomicReference<>(null);
14+
15+
public static FrcRobotControl getControl() {
16+
FrcRobotControl control = sControlInstance.get();
17+
if (control == null) {
18+
throw new IllegalStateException("no robotcontrol was set");
19+
}
20+
21+
return control;
22+
}
23+
24+
public static void setControlInstance(FrcRobotControl instance) {
25+
RobotControl previousInstance = sControlInstance.getAndSet(instance);
26+
27+
RunningRobot.setControlInstance(instance);
28+
29+
if (previousInstance != null) {
30+
previousInstance.getLogger().warn("RunningRobot instance replaced");
31+
}
32+
}
33+
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/RuntimeType.java‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
package com.flash3388.flashlib.frc.robot;
2+
3+
public enum RuntimeType {
4+
/**
5+
* Robot code is executing on real hardware (i.e. RoboRIO)
6+
*/
7+
REAL,
8+
/**
9+
* Robot code is executing in a simulation.
10+
*/
11+
SIMULATION
12+
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/simulation/SimTankDriveSystem.java‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
package com.flash3388.flashlib.frc.robot.simulation;
2+
3+
import com.flash3388.flashlib.frc.robot.RunningFrcRobot;
4+
import com.flash3388.flashlib.frc.robot.simulation.TankDriveSim;
5+
import com.flash3388.flashlib.io.devices.SpeedController;
6+
import com.flash3388.flashlib.robot.systems.drive.TankDriveSystem;
7+
import com.flash3388.flashlib.scheduling.actions.ActionFlag;
8+
import com.flash3388.flashlib.scheduling.actions.Actions;
9+
import com.flash3388.flashlib.time.Time;
10+
import edu.wpi.first.math.geometry.Pose2d;
11+
import edu.wpi.first.wpilibj.RobotController;
12+
13+
import java.util.Objects;
14+
15+
public class SimTankDriveSystem extends TankDriveSystem {
16+
17+
private final TankDriveSim mDriveSim;
18+
19+
public SimTankDriveSystem(SpeedController rightController, SpeedController leftController, TankDriveSim driveSim) {
20+
super(rightController, leftController);
21+
mDriveSim = Objects.requireNonNull(driveSim);
22+
23+
if (RunningFrcRobot.getControl().isRuntimeSimulation()) {
24+
// this action will update our sim data
25+
Actions.fromRunnable(this::updateSim)
26+
.configure()
27+
.setName("SimTankDriveSystem-SimUpdater")
28+
.addFlags(ActionFlag.RUN_ON_DISABLED)
29+
.save()
30+
.start();
31+
}
32+
}
33+
34+
public Pose2d getSimPose() {
35+
assertInSimMode();
36+
return mDriveSim.getPose();
37+
}
38+
39+
public double getSimLeftPositionMeters() {
40+
assertInSimMode();
41+
return mDriveSim.getLeftPositionMeters();
42+
}
43+
44+
public double getSimRightPositionMeters() {
45+
assertInSimMode();
46+
return mDriveSim.getRightPositionMeters();
47+
}
48+
49+
public double getSimAngleDegrees() {
50+
assertInSimMode();
51+
return mDriveSim.getAngleDegrees();
52+
}
53+
54+
private void updateSim() {
55+
double rightVolts = getRightController().get() * RobotController.getBatteryVoltage();
56+
double leftVolts = getLeftController().get() * RobotController.getBatteryVoltage();
57+
58+
// expected loop time for actions
59+
mDriveSim.update(rightVolts, leftVolts, Time.milliseconds(20));
60+
}
61+
62+
private void assertInSimMode() {
63+
if (!RunningFrcRobot.getControl().isRuntimeSimulation()) {
64+
throw new IllegalStateException("cannot call sim-only methods when runtime is not simulation");
65+
}
66+
}
67+
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/simulation/TankDriveSim.java‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
package com.flash3388.flashlib.frc.robot.simulation;
2+
3+
import com.flash3388.flashlib.time.Time;
4+
import edu.wpi.first.math.geometry.Pose2d;
5+
import edu.wpi.first.math.system.plant.DCMotor;
6+
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
7+
8+
public class TankDriveSim {
9+
10+
private final DifferentialDrivetrainSim mDrivetrainSim;
11+
12+
public TankDriveSim(DCMotor motors,
13+
double driveMotorToWheelGearRatio,
14+
double driveMomentOfInertia,
15+
double weightKg,
16+
double driveWheelRadiusM,
17+
double driveTrackWidthM) {
18+
mDrivetrainSim = new DifferentialDrivetrainSim(
19+
motors,
20+
driveMotorToWheelGearRatio,
21+
driveMomentOfInertia,
22+
weightKg,
23+
driveWheelRadiusM,
24+
driveTrackWidthM,
25+
null
26+
);
27+
}
28+
29+
public Pose2d getPose() {
30+
return mDrivetrainSim.getPose();
31+
}
32+
33+
public double getLeftPositionMeters() {
34+
return mDrivetrainSim.getLeftPositionMeters();
35+
}
36+
37+
public double getRightPositionMeters() {
38+
return mDrivetrainSim.getRightPositionMeters();
39+
}
40+
41+
public double getAngleDegrees() {
42+
return getPose().getRotation().getDegrees();
43+
}
44+
45+
public void update(double rightVoltage, double leftVoltage, Time dt) {
46+
mDrivetrainSim.setInputs(rightVoltage, leftVoltage);
47+
mDrivetrainSim.update(dt.valueAsSeconds());
48+
}
49+
}

‎flashlib.frc.robot/src/main/java/com/flash3388/flashlib/frc/robot/systems/FrcTankDriveSystem.java‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
package com.flash3388.flashlib.frc.robot.systems;
2+
3+
import com.flash3388.flashlib.frc.robot.RunningFrcRobot;
4+
import com.flash3388.flashlib.frc.robot.simulation.SimTankDriveSystem;
5+
import com.flash3388.flashlib.frc.robot.simulation.TankDriveSim;
6+
import com.flash3388.flashlib.io.devices.Encoder;
7+
import com.flash3388.flashlib.io.devices.Gyro;
8+
import com.flash3388.flashlib.io.devices.SpeedController;
9+
import com.flash3388.flashlib.scheduling.actions.ActionFlag;
10+
import com.flash3388.flashlib.scheduling.actions.Actions;
11+
import edu.wpi.first.math.geometry.Pose2d;
12+
import edu.wpi.first.math.geometry.Rotation2d;
13+
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
14+
15+
public class FrcTankDriveSystem extends SimTankDriveSystem {
16+
17+
private final Encoder mRightEncoder;
18+
private final Encoder mLeftEncoder;
19+
private final Gyro mGyro;
20+
private final DifferentialDriveOdometry mOdometry;
21+
22+
public FrcTankDriveSystem(SpeedController rightController,
23+
SpeedController leftController,
24+
Encoder rightEncoder,
25+
Encoder leftEncoder,
26+
Gyro gyro,
27+
TankDriveSim driveSim) {
28+
super(rightController, leftController, driveSim);
29+
30+
mRightEncoder = rightEncoder;
31+
mLeftEncoder = leftEncoder;
32+
mGyro = gyro;
33+
34+
if (!RunningFrcRobot.getControl().isRuntimeSimulation()) {
35+
mOdometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(0), 0, 0);
36+
37+
// this action will update our odometry data
38+
Actions.fromRunnable(this::update)
39+
.configure()
40+
.setName("FrcTankDriveSystem-Updater")
41+
.addFlags(ActionFlag.RUN_ON_DISABLED)
42+
.save()
43+
.start();
44+
} else {
45+
mOdometry = null;
46+
}
47+
}
48+
49+
public Pose2d getPose() {
50+
if (RunningFrcRobot.getControl().isRuntimeSimulation()) {
51+
return getSimPose();
52+
}
53+
54+
return mOdometry.getPoseMeters();
55+
}
56+
57+
public double getLeftPositionMeters() {
58+
if (RunningFrcRobot.getControl().isRuntimeSimulation()) {
59+
return getSimLeftPositionMeters();
60+
}
61+
62+
return mLeftEncoder.getDistance();
63+
}
64+
65+
public double getRightPositionMeters() {
66+
if (RunningFrcRobot.getControl().isRuntimeSimulation()) {
67+
return getSimRightPositionMeters();
68+
}
69+
70+
return mRightEncoder.getDistance();
71+
}
72+
73+
public double getAngleDegrees() {
74+
if (RunningFrcRobot.getControl().isRuntimeSimulation()) {
75+
return getSimAngleDegrees();
76+
}
77+
78+
return mGyro.getAngle();
79+
}
80+
81+
private void update() {
82+
mOdometry.update(
83+
Rotation2d.fromDegrees(getAngleDegrees()),
84+
getLeftPositionMeters(),
85+
getRightPositionMeters()
86+
);
87+
}
88+
}

‎gradle.properties‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
GROUP=com.flash3388.flashlib
2-
VERSION=2023.1.0-beta.1
2+
VERSION=2023.1.0
33

4-
WPILIB_VERSION=2023.1.1-beta-4
4+
WPILIB_VERSION=2023.1.1
55
FLASHLIB_VERSION=3.1.0
66

77
NEXUS_SNAPSHOT_REPOSITORY_URL=https\://oss.sonatype.org/content/repositories/snapshots

0 commit comments

Comments
 (0)