Skip to content

Commit

Permalink
Merge pull request #1 from HunterBarclay/ktvuwhs/2025-sim-ver
Browse files Browse the repository at this point in the history
2025 Minor Simulation Tweaks
  • Loading branch information
ktvuwhs authored Jan 11, 2025
2 parents cf2d7eb + f593016 commit 0e3d43c
Show file tree
Hide file tree
Showing 31 changed files with 2,190 additions and 511 deletions.
7 changes: 4 additions & 3 deletions simulation/SyntheSimJava/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,14 @@ repositories {

// KAUAI
maven {
url "https://dev.studica.com/maven/release/2024/"
url "https://dev.studica.com/maven/release/2025/"
}
}

def WPI_Version = '2025.1.1'
def REV_Version = '2025.0.0'
def CTRE_Version = '25.1.0'
def KAUAI_Version = '2024.1.0'
def KAUAI_Version = '2025.1.1-beta-1'

dependencies {
// This dependency is exported to consumers, that is to say found on their compile classpath.
Expand All @@ -52,6 +52,7 @@ dependencies {
// WPILib
implementation "edu.wpi.first.wpilibj:wpilibj-java:$WPI_Version"
implementation "edu.wpi.first.wpiutil:wpiutil-java:$WPI_Version"
implementation "edu.wpi.first.wpiunits:wpiunits-java:$WPI_Version"
implementation "edu.wpi.first.hal:hal-java:$WPI_Version"

// REVRobotics
Expand All @@ -61,7 +62,7 @@ dependencies {
implementation "com.ctre.phoenix6:wpiapi-java:$CTRE_Version"

// KAUAI
implementation "com.kauailabs.navx.frc:navx-frc-java:$KAUAI_Version"
implementation "com.kauailabs.navx.frc:navx_frc-java:$KAUAI_Version"
}

java {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,89 +1,56 @@
// package com.autodesk.synthesis.ctre;

// import com.autodesk.synthesis.CANEncoder;
// import com.autodesk.synthesis.CANMotor;
// import com.ctre.phoenix6.signals.NeutralModeValue;
// import com.ctre.phoenix6.StatusSignal;
// import com.ctre.phoenix6.configs.TalonFXConfigurator;
// import com.ctre.phoenix6.hardware.DeviceIdentifier;

// public class TalonFX extends com.ctre.phoenix6.hardware.TalonFX {
// private CANMotor m_motor;
// private CANEncoder m_encoder;

// /**
// * Creates a new TalonFX, wrapped with simulation support.
// *
// * @param deviceNumber CAN Device ID.
// */
// public TalonFX(int deviceNumber) {
// super(deviceNumber);

// this.m_motor = new CANMotor("SYN TalonFX", deviceNumber, 0.0, false, 0.3);
// this.m_encoder = new CANEncoder("SYN TalonFX", deviceNumber);
// }

// /**
// * Sets the torque of the real and simulated motors
// *
// * @param percentOutput The torque
// */
// @Override
// public void set(double percentOutput) {
// super.set(percentOutput);
// this.m_motor.setPercentOutput(percentOutput);
// }

// /**
// * Sets both the real and simulated motors to neutral mode
// *
// * @param mode The neutral mode value
// *
// */
// @Override
// public void setNeutralMode(NeutralModeValue mode) {
// super.setNeutralMode(mode);

// this.m_motor.setBrakeMode(mode == NeutralModeValue.Brake);
// }

// /**
// * Gets and internal configurator for both the simulated and real motors
// *
// * @return The internal configurator for this Talon motor
// */
// @Override
// public TalonFXConfigurator getConfigurator() {
// DeviceIdentifier id = this.deviceIdentifier;
// return new com.autodesk.synthesis.ctre.TalonFXConfigurator(id, this);
// }

// // called internally by the configurator to set the deadband, not for user use
// public void setNeutralDeadband(double deadband) {
// this.m_motor.setNeutralDeadband(deadband);
// }

// /**
// * Gets the position of the simulated encoder
// *
// * @return The motor position in revolutions
// */
// @Override
// public StatusSignal<Double> getPosition() {
// Double pos = this.m_encoder.getPosition();
// super.setPosition(pos);
// return super.getPosition();
// }

// /**
// * Gets the velocity of the simulated motor according to the simulated encoder
// *
// * @return The motor velocity in revolutions per second
// */
// @Override
// public StatusSignal<Double> getVelocity() {
// Double velocity = this.m_encoder.getVelocity();
// super.set(velocity);
// return super.getVelocity();
// }
// }
package com.autodesk.synthesis.ctre;

import com.autodesk.synthesis.CANEncoder;
import com.autodesk.synthesis.CANMotor;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.hardware.DeviceIdentifier;

public class TalonFX extends com.ctre.phoenix6.hardware.TalonFX {
private CANMotor m_motor;
private CANEncoder m_encoder;

/**
* Creates a new TalonFX, wrapped with simulation support.
*
* @param deviceNumber CAN Device ID.
*/
public TalonFX(int deviceNumber) {
super(deviceNumber);

this.m_motor = new CANMotor("SYN TalonFX", deviceNumber, 0.0, false, 0.3);
this.m_encoder = new CANEncoder("SYN TalonFX", deviceNumber);
}

@Override
public void set(double percentOutput) {
super.set(percentOutput);
this.m_motor.setPercentOutput(percentOutput);
}

@Override
public StatusCode setNeutralMode(NeutralModeValue mode) {
this.m_motor.setBrakeMode(mode == NeutralModeValue.Brake);
return super.setNeutralMode(mode);
}

@Override
public TalonFXConfigurator getConfigurator() {
DeviceIdentifier id = this.deviceIdentifier;
return new com.autodesk.synthesis.ctre.TalonFXConfigurator(id, this);
}

// called internally by the configurator to set the deadband, not for user use
public void setNeutralDeadband(double deadband) {
this.m_motor.setNeutralDeadband(deadband);
}

public double getPositionSim() {
return this.m_encoder.getPosition();
}

public double getVelocitySim() {
return this.m_encoder.getVelocity();
}
}
Original file line number Diff line number Diff line change
@@ -1,38 +1,38 @@
// package com.autodesk.synthesis.ctre;
package com.autodesk.synthesis.ctre;

// import com.ctre.phoenix6.hardware.DeviceIdentifier;
// import com.ctre.phoenix6.configs.TorqueCurrentConfigs;
// import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.hardware.DeviceIdentifier;
import com.ctre.phoenix6.configs.TorqueCurrentConfigs;
import com.ctre.phoenix6.StatusCode;

// /**
// * TalonFXConfigurator wrapper to add proper WPILib HALSim support.
// */
// public class TalonFXConfigurator extends com.ctre.phoenix6.configs.TalonFXConfigurator {
// private TalonFX devicePtr;
/**
* TalonFXConfigurator wrapper to add proper WPILib HALSim support.
*/
public class TalonFXConfigurator extends com.ctre.phoenix6.configs.TalonFXConfigurator {
private TalonFX devicePtr;

// /**
// * Creates a new TalonFXConfigurator, wrapped with simulation support.
// *
// * @param id Device ID
// * @param device The motor to configure
// */
// public TalonFXConfigurator(DeviceIdentifier id, TalonFX device) {
// super(id);
// // awful, jank solution, please help
// // if you know how to get a device from an id, let me know
// this.devicePtr = device;
// }
/**
* Creates a new TalonFXConfigurator, wrapped with simulation support.
*
* @param id Device ID
* @param device The motor to configure
*/
public TalonFXConfigurator(DeviceIdentifier id, TalonFX device) {
super(id);
// awful, jank solution, please help
// if you know how to get a device from an id, let me know
this.devicePtr = device;
}

// /**
// * Applies a torque configuration to a TalonFX motor and passes the new neutral deadband to the simulated motor in fission if applicable
// *
// * @param newTorqueCurrent The new torque configuration for this motor
// */
// @Override
// public StatusCode apply(TorqueCurrentConfigs newTorqueCurrent) {
// StatusCode code = super.apply(newTorqueCurrent);
// double newDeadband = newTorqueCurrent.TorqueNeutralDeadband;
// this.devicePtr.setNeutralDeadband(newDeadband);
// return code;
// }
// }
/**
* Applies a torque configuration to a TalonFX motor and passes the new neutral deadband to the simulated motor in fission if applicable
*
* @param newTorqueCurrent The new torque configuration for this motor
*/
@Override
public StatusCode apply(TorqueCurrentConfigs newTorqueCurrent) {
StatusCode code = super.apply(newTorqueCurrent);
double newDeadband = newTorqueCurrent.TorqueNeutralDeadband;
this.devicePtr.setNeutralDeadband(newDeadband);
return code;
}
}
Original file line number Diff line number Diff line change
@@ -1,31 +1,35 @@
package com.autodesk.synthesis.revrobotics;

import com.autodesk.synthesis.CANEncoder;
import com.autodesk.synthesis.revrobotics.SparkMax;
import com.revrobotics.spark.config.SparkMaxConfigAccessor;
import com.revrobotics.REVLibError;

public class RelativeEncoder implements com.revrobotics.RelativeEncoder {
private CANEncoder m_encoder;
private double m_zero = 0.0;

public RelativeEncoder(CANEncoder encoder) {
private com.revrobotics.RelativeEncoder m_original;
private double m_zero = 0.0;
private CANEncoder m_encoder;
private SparkMaxConfigAccessor m_accessor;

public RelativeEncoder(com.revrobotics.RelativeEncoder original, CANEncoder encoder, SparkMaxConfigAccessor accessor) {
m_original = original;
m_encoder = encoder;
m_accessor = accessor;
}

@Override
public double getPosition() {
return m_encoder.getPosition() - m_zero;
return m_encoder.getPosition() * m_accessor.encoder.getPositionConversionFactor() * (m_accessor.encoder.getInverted() ? -1.0 : 1.0) - m_zero;
}

@Override
public double getVelocity() {
return m_encoder.getVelocity();
return m_encoder.getVelocity() * m_accessor.encoder.getVelocityConversionFactor() * (m_accessor.encoder.getInverted() ? -1.0 : 1.0);
}

@Override
public REVLibError setPosition(double position) {
m_zero = m_encoder.getPosition() - position;
m_zero = this.getPosition() - position;
return REVLibError.kOk;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
import com.autodesk.synthesis.CANEncoder;

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.REVLibError;

/**
* SparkAbsoluteEncoder wrapper to add proper WPILib HALSim support.
Expand Down
Loading

0 comments on commit 0e3d43c

Please sign in to comment.