Skip to content

Commit

Permalink
set up DCmotorsim stuff even tho i prolly won't have time to actually…
Browse files Browse the repository at this point in the history
… use it
  • Loading branch information
amb2127 committed Apr 8, 2024
1 parent 7a2e6ef commit 5252f17
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 0 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
package org.codeorange.frc2024.utility.logging;

import org.codeorange.frc2024.utility.simulation.BetterDCMotorSim;

import static org.codeorange.frc2024.robot.Constants.NOMINAL_DT;

public class DCMotorSimAutoLogger implements MotorAutoLogger {
private final MotorInputs inputs;
private final BetterDCMotorSim motorSim;

public DCMotorSimAutoLogger(BetterDCMotorSim sim) {
inputs = new MotorInputs();
motorSim = sim;
}

public MotorInputs log() {
inputs.CANID = -1;
inputs.position = motorSim.getAngularPositionRotations();
inputs.velocity = motorSim.getAngularVelocityRPM();
inputs.supplyVoltage = 12.5;
inputs.motorVoltage = motorSim.getInputVoltage();
inputs.supplyCurrent = motorSim.getCurrentDrawAmps();
inputs.statorCurrent = motorSim.getCurrentDrawAmps() / (inputs.motorVoltage / 12.5);
inputs.temperature = 30;
inputs.energy += inputs.supplyVoltage * inputs.supplyCurrent * NOMINAL_DT;

return inputs;
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
package org.codeorange.frc2024.utility.simulation;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;

/**
* DCMotorSim but stores input voltage because i am lazy
*/
public class BetterDCMotorSim extends DCMotorSim {
private double inputVoltage = 0;

public BetterDCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double gearing) {
super(plant, gearbox, gearing);
}

public BetterDCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double gearing, Matrix<N2, N1> measurementStdDevs) {
super(plant, gearbox, gearing, measurementStdDevs);
}

public BetterDCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
super(gearbox, gearing, jKgMetersSquared);
}

public BetterDCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix<N2, N1> measurementStdDevs) {
super(gearbox, gearing, jKgMetersSquared, measurementStdDevs);
}

@Override
public void setInputVoltage(double volts) {
inputVoltage = volts;
setInput(volts);
}

public double getInputVoltage() {
return inputVoltage;
}
}

0 comments on commit 5252f17

Please sign in to comment.