Merge pull request #32 from Shenzhen-Robotics-Alliance/motor-sim-cont…

…rol-loops

Motor sim control loops

.gitignore

@@ -7,10 +7,14 @@ build/
 !/docs/maplesim/vendordep/
 
 ### IntelliJ IDEA ###
+idea/modules.xml
+.idea/compiler.xml
+.idea/jarRepositories.xml
+.idea/libraries/
 project/idea/modules.xml
 project/.idea/compiler.xml
 project/.idea/jarRepositories.xml
-project/idea/libraries/
+project/.idea/libraries/
 *.iws
 *.iml
 *.ipr
@@ -44,3 +48,4 @@ bin/
 .DS_Store
 
 /javadocs/**
+/.idea

docs/3.1_SWERVE_SIM_EZ_MODE.md

@@ -1,6 +1,12 @@
 # Swerve Simulation: Simplified Swerve Simulation
 
-## Comming soon
+> ⚠️ **Note**
+> 
+> You are reading the documentation for a Beta version of maple-sim. API references are subject to change in future versions.
+
+## Overview
+
+## 1. Creating a Simplified Swer
 
 <div style="display:flex">
     <h3 style="width:49%"><< Back to: <a href="https://shenzhen-robotics-alliance.github.io/maple-sim/3.0_SWERVE_SIMULATION_OVERVIEW.html">Swerve Simulation Overview</a></h3>

docs/3.2_SWERVE_SIM_HARDWARE_ABSTRACTION.md

@@ -1,9 +1,14 @@
 # Swerve Simulation: Hardware Abstractions
 
+> ⚠️ **Note**
+> 
+> You are reading the documentation for a Beta version of maple-sim. API references are subject to change in future versions.
+
+## Overview
 
 ## 0. Hardware Abstraction
 
-> ⚠️ **Before You Begin**
+> 💡 **To those using AKit**
 > 
 > If you're using AdvantageKit, your code is already hardware-abstracted. You do not need to restructure your code to use maple-sim.
 
@@ -44,12 +49,12 @@ The simulated gyro includes measurement errors and will drift if the robot colli
 
 Create `GyroIOSim.java`, which implements `GyroIO` by wrapping around the methods of `GyroSimulation`:
 
-### Gyro IO interface
+### IO interface
 ```java
 // Example gyro interface
 public interface GyroIO {
     Rotation2d getGyroRotation();
-    double getGyroAngularVelocity();
+    AngularVelocity getGyroAngularVelocity();
 }
 ```
 
@@ -69,7 +74,7 @@ public class GyroIOPigeon2 implements GyroIO {
     }
 
     @Override // specified by GroIOSim interface
-    public double getGyroAngularVelocity() {
+    public AngularVelocity getGyroAngularVelocity() {
         return pigeon2.getAngularVelocity();
     }
 }
@@ -108,20 +113,68 @@ Similar to the gyro, you also need to create hardware abstractions for the swerv
 To implement `ModuleIO` using the simulator, use the following API references in `ModuleIOSim`:
 
 ```java
+// This is only an example simulation IO implement, please change the code according to your ModuleIO interface
 public class ModuleIOSim implements ModuleIO {
     private final SwerveModuleSimulation moduleSimulation;
     public ModuleIOSim(SwerveModuleSimulation moduleSimulation) {
         this.moduleSimulation = moduleSimulation;
+
+        // configures the drive motor gains
+        this.moduleSimulation.getDriveMotorConfigs()
+            // configures a default feed-forward gains for the drive motor, with 0.1 volts of friction compensation
+            .withDefaultFeedForward(Volts.of(0.1))
+            // configures a velocity-voltage close loop controller
+            .withVelocityVoltageController(
+                // P gain is 12 volts / 3000 RPM (change it for your code)
+                // Meaning that: the correction voltage is 12 volts when error is 3000RPM
+                Volts.per(RPM).ofNative(12.0/3000.0), 
+                // true means the P gain is specified in un-geared motor speed
+                true
+            );
+
+        // configures the steer motor gains
+        this.moduleSimulation.getSteerMotorConfigs()
+            // configures a position-current close loop controller
+            .withPositionCurrentController(
+                // P gain is 12 amps / 60 degrees
+                // Meaning that: the correction current is 20 amps when the steer position error is 60 degrees
+                Amps.per(Degrees).ofNative(10.0/60.0), 
+                // D gain is zero
+                Amps.per(DegreesPerSecond).ofNative(0),
+                // false means the P and D gain are specifed in final mechanism position/velocity
+                false
+            );
     }
 
     @Override // specified by ModuleIO interface
-    public void setDriveOutputVoltage(double volts) {
-        this.moduleSimulation.requestDriveVoltageOut(volts);
+    public void setDriveOutputVoltage(Voltage voltage) {
+        this.moduleSimulation.requestDriveControl(new ControlRequest.VoltageOut(voltage));
     }
 
     @Override // specified by ModuleIO interface
-    public void setSteerOutputVoltage(double volts) {
-        this.moduleSimulation.requestSteerVoltageOut(volts);
+    public void setSteerOutputVoltage(Voltage voltage) {
+        this.moduleSimulation.requestSteerControl(new ControlRequest.VoltageOut(voltage));
+    }
+
+    @Override // specified by ModuleIO interface
+    public void requestDriveVelocity(LinearVelocity driveVelocitySetpoint) {
+        // calculates the amount of wheel speed needed to achive the linear speed
+        final AngularVelocity wheelVelocitySetpoint = RadiansPerSecond.of(
+            driveVelocitySetpoint.in(MetersPerSecond) / moduleSimulation.WHEEL_RADIUS.in(Meters)
+        );
+
+        // request the drive motor controller to run a closed-loop on velocity
+        moduleSimulation.requestDriveControl(
+            new ControlRequest.VelocityVoltage(wheelVelocitySetpoint)
+        );
+    }
+
+    @Override // specified by ModuleIO interface
+    public void requestSteerPosition(Rotation2d steerFacingSetpoint) {
+        // request the steer motor controller to run a close
+        moduleSimulation.requestSteerControl(
+            new ControlRequest.PositionCurrent(steerFacingSetpoint.getMeasure())
+        );
     }
 
     @Override // specified by ModuleIO interface
@@ -130,13 +183,13 @@ public class ModuleIOSim implements ModuleIO {
     }
 
     @Override // specified by ModuleIO interface
-    public double getSteerRelativePositionRad() {
-        return this.moduleSimulation.getSteerRelativeEncoderPositionRad();
+    public Angle getSteerRelativePosition() {
+        return moduleSimulation.getSteerRelativeEncoderPosition().divide(moduleSimulation.STEER_GEAR_RATIO));
     }
 
     @Override // specified by ModuleIO interface
-    public double getDriveEncoderPositionRad() {
-        return this.moduleSimulation.getDriveEncoderUnGearedPositionRad();
+    public Angle getDriveWheelrPositiond() {
+        return moduleSimulation.getDrieWheelFinalPosition()
     }
 }
 ```
@@ -148,11 +201,13 @@ An example of interacting with module simulations and simulating high-frequency
 When running the simulator, you can instantiate the above simulation IO implementations to allow the robot to run within the simulation environment.
 
 ```java
-this.gyroSimulation = new GyroSimulation(...);
+// creation the swerve simulation (please refer to previous documents)
 this.swerveDriveSimulation = new SwerveDriveSimulation(...);
 
+// 
+
 this.drive = new Drive(
-        new GyroIOSim(this.gyroSimulation),
+        new GyroIOSim(this.swerveDriveSimulation.getGyroSimulation()),
         new ModuleIOSim(this.swerveDriveSimulation.getModules()[0]),
         new ModuleIOSim(this.swerveDriveSimulation.getModules()[1]),
         new ModuleIOSim(this.swerveDriveSimulation.getModules()[2]),