[AARD-1867] Minor fixes for code simulation (#1113)

* Minor fixes for code simulation

* cleanup: Format

* fix: Disable SSL

Author: HunterBarclay

fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

@@ -137,6 +137,24 @@ export function getSimMap(): SimMap | undefined {
 export class SimGeneric {
     private constructor() {}
 
+    public static GetUnsafe<T>(simType: SimType, device: string, field: string): T | undefined
+    public static GetUnsafe<T>(simType: SimType, device: string, field: string, defaultValue: T): T
+    public static GetUnsafe<T>(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined {
+        const map = getSimMap()?.get(simType)
+        if (!map) {
+            // console.warn(`No '${simType}' devices found`)
+            return undefined
+        }
+
+        const data = map.get(device)
+        if (!data) {
+            // console.warn(`No '${simType}' device '${device}' found`)
+            return undefined
+        }
+
+        return (data.get(field) as T | undefined) ?? defaultValue
+    }
+
     public static Get<T>(simType: SimType, device: string, field: string): T | undefined
     public static Get<T>(simType: SimType, device: string, field: string, defaultValue: T): T
     public static Get<T>(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined {
@@ -214,6 +232,10 @@ export class SimDriverStation {
         SimGeneric.Set<string>(SimType.DriverStation, "", ">match_time", gameData)
     }
 
+    public static IsEnabled(): boolean {
+        return SimGeneric.GetUnsafe<boolean>(SimType.DriverStation, "", ">enabled", false)
+    }
+
     public static SetMode(mode: RobotSimMode) {
         SimGeneric.Set<boolean>(SimType.DriverStation, "", ">enabled", mode != RobotSimMode.Disabled)
         SimGeneric.Set<boolean>(SimType.DriverStation, "", ">autonomous", mode == RobotSimMode.Auto)
@@ -228,7 +250,7 @@ export class SimPWM {
     private constructor() {}
 
     public static GetSpeed(device: string): number | undefined {
-        return SimGeneric.Get(SimType.PWM, device, PWM_SPEED, 0.0)
+        return SimDriverStation.IsEnabled() ? SimGeneric.Get(SimType.PWM, device, PWM_SPEED, 0.0) : 0.0
     }
 
     public static GetPosition(device: string): number | undefined {
@@ -268,7 +290,9 @@ export class SimCANMotor {
     private constructor() {}
 
     public static GetPercentOutput(device: string): number | undefined {
-        return SimGeneric.Get(SimType.CANMotor, device, CANMOTOR_PERCENT_OUTPUT, 0.0)
+        return SimDriverStation.IsEnabled()
+            ? SimGeneric.Get(SimType.CANMotor, device, CANMOTOR_PERCENT_OUTPUT, 0.0)
+            : 0.0
     }
 
     public static GetBrakeMode(device: string): number | undefined {

simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/RelativeEncoder.java

@@ -23,7 +23,7 @@ public RelativeEncoder(com.revrobotics.RelativeEncoder original, CANEncoder enco
 
     @Override
     public double getPosition() {
-        return (m_encoder.getPosition() - m_zero) * m_positionConversionFactor * m_invertedFactor;
+        return m_encoder.getPosition() * m_positionConversionFactor * m_invertedFactor - m_zero;
     }
 
     @Override
@@ -33,22 +33,19 @@ public double getVelocity() {
 
     @Override
     public REVLibError setPosition(double position) {
-        m_zero = position;
-        m_original.setPosition(position);
+        m_zero = m_encoder.getPosition() * m_positionConversionFactor * m_invertedFactor - position;
         return REVLibError.kOk;
     }
 
     @Override
     public REVLibError setPositionConversionFactor(double factor) {
         m_positionConversionFactor = factor;
-        m_original.setPositionConversionFactor(factor);
         return REVLibError.kOk;
     }
 
     @Override
     public REVLibError setVelocityConversionFactor(double factor) {
         m_velocityConversionFactor = factor;
-        m_original.setVelocityConversionFactor(factor);
         return REVLibError.kOk;
     }
 
@@ -90,7 +87,6 @@ public int getCountsPerRevolution() {
     @Override
     public REVLibError setInverted(boolean inverted) {
         m_invertedFactor = inverted ? -1.0 : 1.0;
-        m_original.setInverted(inverted);
         return REVLibError.kOk;
     }
 

simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java

@@ -18,7 +18,10 @@
 import edu.wpi.first.wpilibj.XboxController;
 
 import com.autodesk.synthesis.revrobotics.CANSparkMax;
+import com.autodesk.synthesis.revrobotics.RelativeEncoder;
+import com.autodesk.synthesis.revrobotics.SparkAbsoluteEncoder;
 import com.kauailabs.navx.frc.AHRS;
+import com.autodesk.synthesis.CANEncoder;
 import com.autodesk.synthesis.ctre.TalonFX;
 
 /**
@@ -42,6 +45,7 @@ public class Robot extends TimedRobot {
     private CANSparkMax m_sparkLeft = new CANSparkMax(1, MotorType.kBrushless);
     private CANSparkMax m_sparkRight = new CANSparkMax(2, MotorType.kBrushless);
     private CANSparkMax m_sparkArm = new CANSparkMax(3, MotorType.kBrushless);
+    private RelativeEncoder m_encoder;
 
     /**
      * This function is run when the robot is first started up and should be used
@@ -53,6 +57,11 @@ public void robotInit() {
         m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
         m_chooser.addOption("My Auto", kCustomAuto);
         SmartDashboard.putData("Auto choices", m_chooser);
+
+        m_encoder = m_sparkLeft.getEncoderSim();
+        // 4 inch diameter wheels, default is 1 unit = 1 radian.
+        // Following conversion factor is 1 unit = 1 inch travelled.
+        m_encoder.setPositionConversionFactor(2.0);
     }
 
     /**
@@ -91,32 +100,34 @@ public void autonomousInit() {
         m_autoSelected = m_chooser.getSelected();
         // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
         System.out.println("Auto selected: " + m_autoSelected);
-        m_sparkLeft.getAbsoluteEncoderSim()
+        m_encoder.setPosition(0.0);
+        m_autoState = AutoState.Stage1;
     }
 
     enum AutoState {
         Stage1, Stage2
     }
-
     private AutoState m_autoState = AutoState.Stage1;
 
     /** This function is called periodically during autonomous. */
     @Override
     public void autonomousPeriodic() {
         switch (m_autoState) {
-            case Stage1: {
+            case Stage1:
                 m_sparkLeft.set(0.5);
                 m_sparkRight.set(0.5);
-                if (m_sparkLeft.getEncoder())
+                if (m_encoder.getPosition() > 36.0) {
+                    m_autoState = AutoState.Stage2;
+                    System.out.println("--- Transitioning to Stage 2 ---");
+                }
                 break;
-            } case Stage2: {
+            case Stage2:
+                m_sparkLeft.set(0.5);
+                m_sparkRight.set(-0.5);
                 break;
-            } default: {
+            default:
                 break;
-            }
         }
-        m_SparkMax1.set(0.2);
-        m_SparkMax2.set(-0.2);
     }
 
     /** This function is called once when teleop is enabled. */