001/*
002 * Copyright (c) 2022 REV Robotics
003 *
004 * Redistribution and use in source and binary forms, with or without
005 * modification, are permitted provided that the following conditions are met:
006 *
007 * 1. Redistributions of source code must retain the above copyright notice,
008 *    this list of conditions and the following disclaimer.
009 * 2. Redistributions in binary form must reproduce the above copyright
010 *    notice, this list of conditions and the following disclaimer in the
011 *    documentation and/or other materials provided with the distribution.
012 * 3. Neither the name of REV Robotics nor the names of its
013 *    contributors may be used to endorse or promote products derived from
014 *    this software without specific prior written permission.
015 *
016 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
017 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
018 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
019 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
020 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
021 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
022 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
023 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
024 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
025 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
026 * POSSIBILITY OF SUCH DAMAGE.
027 */
028
029package com.revrobotics;
030
031import edu.wpi.first.math.system.plant.DCMotor;
032import java.util.*;
033
034/** Manages physics simulation for REV Robotics products. */
035public class REVPhysicsSim {
036  private static final REVPhysicsSim sim = new REVPhysicsSim();
037
038  /** Gets the robot simulator instance. */
039  public static REVPhysicsSim getInstance() {
040    return sim;
041  }
042
043  /**
044   * Adds a SparkMAX controller to the simulator.
045   *
046   * @param spark The SparkMAX device
047   * @param stallTorque The stall Torque of the motor connected to SparkMAX units are N m
048   * @param freeSpeed The maximum freeSpeed in RPM
049   */
050  public void addSparkMax(CANSparkMax spark, final float stallTorque, final float freeSpeed) {
051    if (spark != null) {
052      SparkMaxSimProfile simSpark = new SparkMaxSimProfile(spark, stallTorque, freeSpeed);
053      _simProfiles.add(simSpark);
054    }
055  }
056
057  /**
058   * Adds a SparkMAX controller to the simulator.
059   *
060   * @param spark The SparkMAX device
061   * @param motor the motor connected to the spark max
062   */
063  public void addSparkMax(CANSparkMax spark, DCMotor motor) {
064    if (spark != null) {
065      SparkMaxSimProfile simSpark = new SparkMaxSimProfile(spark, motor);
066      _simProfiles.add(simSpark);
067    }
068  }
069
070  /** Runs the simulator: - enable the robot - runs all SparkMax devices connected */
071  public void run() {
072    // Simulate devices
073    for (SimProfile simProfile : _simProfiles) {
074      simProfile.run();
075    }
076  }
077
078  private final ArrayList<SimProfile> _simProfiles = new ArrayList<SimProfile>();
079
080  /** Holds information about a simulated device. */
081  static class SimProfile {
082    private long _lastTime;
083    private boolean _running = false;
084
085    /** Runs the simulation profile. Implemented by device-specific profiles. */
086    public void run() {}
087
088    /** Returns the time since last call, in milliseconds. */
089    protected double getPeriod() {
090      // set the start time if not yet running
091      if (!_running) {
092        _lastTime = System.nanoTime();
093        _running = true;
094      }
095
096      long now = System.nanoTime();
097      final double period = (now - _lastTime) / 1000000.;
098      _lastTime = now;
099
100      return period;
101    }
102  }
103}