001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj.simulation; 006 007import edu.wpi.first.math.VecBuilder; 008import edu.wpi.first.math.numbers.N1; 009import edu.wpi.first.math.system.LinearSystem; 010import edu.wpi.first.math.system.plant.DCMotor; 011import edu.wpi.first.math.system.plant.LinearSystemId; 012import edu.wpi.first.math.util.Units; 013import edu.wpi.first.wpilibj.RobotController; 014 015/** Represents a simulated flywheel mechanism. */ 016public class FlywheelSim extends LinearSystemSim<N1, N1, N1> { 017 // Gearbox for the flywheel. 018 private final DCMotor m_gearbox; 019 020 // The gearing from the motors to the output. 021 private final double m_gearing; 022 023 /** 024 * Creates a simulated flywheel mechanism. 025 * 026 * @param plant The linear system that represents the flywheel. 027 * @param gearbox The type of and number of motors in the flywheel gearbox. 028 * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). 029 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 030 * noise is desired. If present must have 1 element for velocity. 031 */ 032 public FlywheelSim( 033 LinearSystem<N1, N1, N1> plant, 034 DCMotor gearbox, 035 double gearing, 036 double... measurementStdDevs) { 037 super(plant, measurementStdDevs); 038 m_gearbox = gearbox; 039 m_gearing = gearing; 040 } 041 042 /** 043 * Creates a simulated flywheel mechanism. 044 * 045 * @param gearbox The type of and number of motors in the flywheel gearbox. 046 * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). 047 * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the 048 * {@link #FlywheelSim(LinearSystem, DCMotor, double, double...)} constructor. 049 * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no 050 * noise is desired. If present must have 1 element for velocity. 051 */ 052 public FlywheelSim( 053 DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) { 054 super( 055 LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing), 056 measurementStdDevs); 057 m_gearbox = gearbox; 058 m_gearing = gearing; 059 } 060 061 /** 062 * Sets the flywheel's state. 063 * 064 * @param velocityRadPerSec The new velocity in radians per second. 065 */ 066 public void setState(double velocityRadPerSec) { 067 setState(VecBuilder.fill(velocityRadPerSec)); 068 } 069 070 /** 071 * Returns the flywheel velocity. 072 * 073 * @return The flywheel velocity. 074 */ 075 public double getAngularVelocityRadPerSec() { 076 return getOutput(0); 077 } 078 079 /** 080 * Returns the flywheel velocity in RPM. 081 * 082 * @return The flywheel velocity in RPM. 083 */ 084 public double getAngularVelocityRPM() { 085 return Units.radiansPerSecondToRotationsPerMinute(getOutput(0)); 086 } 087 088 /** 089 * Returns the flywheel current draw. 090 * 091 * @return The flywheel current draw. 092 */ 093 public double getCurrentDrawAmps() { 094 // I = V / R - omega / (Kv * R) 095 // Reductions are output over input, so a reduction of 2:1 means the motor is spinning 096 // 2x faster than the flywheel 097 return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) 098 * Math.signum(m_u.get(0, 0)); 099 } 100 101 /** 102 * Sets the input voltage for the flywheel. 103 * 104 * @param volts The input voltage. 105 */ 106 public void setInputVoltage(double volts) { 107 setInput(volts); 108 clampInput(RobotController.getBatteryVoltage()); 109 } 110}