diff --git a/build.gradle b/build.gradle index 0243b91a..a3cab5c4 100644 --- a/build.gradle +++ b/build.gradle @@ -79,7 +79,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - implementation "com.github.deepbluerobotics:lib199:0e5932a4219f3439ef2be3981a9189baf777f61e" + implementation "com.github.deepbluerobotics:lib199:0ce129844b9b918dad0207c0e1020fc26588f461" } test { diff --git a/simgui-ds.json b/simgui-ds.json index afe7dbdd..56bd4817 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -52,7 +52,7 @@ "incKey": 75 } ], - "axisCount": 2, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 77, @@ -60,7 +60,7 @@ 46, 47 ], - "povCount": 0 + "povCount": 1 }, { "axisConfig": [ @@ -97,7 +97,8 @@ "useGamepad": true }, { - "guid": "Keyboard1" + "guid": "Keyboard1", + "name": "Manipulator" } ] } diff --git a/simgui.json b/simgui.json index 9b3270e4..e6f7c67b 100644 --- a/simgui.json +++ b/simgui.json @@ -6,6 +6,11 @@ } }, "Other Devices": { + "AbsoluteEncoder[13]": { + "header": { + "open": true + } + }, "CANCoder (v6)[2]": { "header": { "open": true @@ -63,6 +68,9 @@ "NetworkTables": { "transitory": { "SmartDashboard": { + "Arm": { + "open": true + }, "BL": { "open": true }, diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 451079cb..dd5f255c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -19,6 +19,7 @@ import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; +import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -30,9 +31,11 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; @@ -76,6 +79,9 @@ public class Arm extends SubsystemBase { private ShuffleboardTab sysIdTab = Shuffleboard.getTab("arm SysID"); private boolean setPIDOff; + + private SimDouble rotationsSim; + public Arm() { // weird math stuff armMotorMaster.setInverted(MOTOR_INVERTED_MASTER); @@ -145,6 +151,9 @@ public Arm() { armMotorFollower.setSmartCurrentLimit(80); SmartDashboard.putBoolean("arm is at pos", false); + if (RobotBase.isSimulation()) { + rotationsSim = new SimDeviceSim("AbsoluteEncoder", ARM_MOTOR_PORT_MASTER).getDouble("rotations"); + } } public void setBooleanDrive(boolean climb) { @@ -430,4 +439,10 @@ public void setDefaultCommand(TeleopArm teleopArm, Object object) { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'setDefaultCommand'"); } + + @Override + public void simulationPeriodic() { + // Fake goaling to the goal instantaneously + rotationsSim.set((goalState.position + armMasterEncoder.getZeroOffset()) / armMasterEncoder.getPositionConversionFactor()); + } }