diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 122ed51ec..844c8c7fd 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/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(simType: SimType, device: string, field: string): T | undefined + public static GetUnsafe(simType: SimType, device: string, field: string, defaultValue: T): T + public static GetUnsafe(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(simType: SimType, device: string, field: string): T | undefined public static Get(simType: SimType, device: string, field: string, defaultValue: T): T public static Get(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined { @@ -214,6 +232,10 @@ export class SimDriverStation { SimGeneric.Set(SimType.DriverStation, "", ">match_time", gameData) } + public static IsEnabled(): boolean { + return SimGeneric.GetUnsafe(SimType.DriverStation, "", ">enabled", false) + } + public static SetMode(mode: RobotSimMode) { SimGeneric.Set(SimType.DriverStation, "", ">enabled", mode != RobotSimMode.Disabled) SimGeneric.Set(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,7 @@ 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 { diff --git a/fission/vite.config.ts b/fission/vite.config.ts index 428d7d061..ead5ab543 100644 --- a/fission/vite.config.ts +++ b/fission/vite.config.ts @@ -9,7 +9,7 @@ const serverPort = 3000 const dockerServerPort = 80 const useLocal = false -const useSsl = false +const useSsl = true const plugins = [ react(), glsl({ diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/RelativeEncoder.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/RelativeEncoder.java index 7046cd7df..a25dddaec 100644 --- a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/RelativeEncoder.java +++ b/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; } diff --git a/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java index e8e8733c1..9452a5634 100644 --- a/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java +++ b/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. */