Add Motion Magic Talon config

Resolves #20

Author: jhh

core/src/main/java/org/strykeforce/thirdcoast/talon/MotionMagicTalonConfiguration.java

@@ -0,0 +1,69 @@
+package org.strykeforce.thirdcoast.talon;
+
+import com.ctre.CANTalon;
+import com.ctre.CANTalon.TalonControlMode;
+import com.ctre.CANTalon.VelocityMeasurementPeriod;
+import org.jetbrains.annotations.NotNull;
+
+public class MotionMagicTalonConfiguration extends PIDTalonConfiguration {
+
+  private final Double motionMagicAcceleration;
+  private final Double motionMagicCruiseVelocity;
+
+  MotionMagicTalonConfiguration(@NotNull String name,
+      double setpointMax,
+      Encoder encoder,
+      Boolean isBrakeInNeutral,
+      Boolean isOutputReversed,
+      VelocityMeasurementPeriod velocityMeasurementPeriod,
+      Integer velocityMeasurementWindow,
+      LimitSwitch forwardLimitSwitch,
+      LimitSwitch reverseLimitSwitch,
+      SoftLimit forwardSoftLimit,
+      SoftLimit reverseSoftLimit,
+      Integer currentLimit,
+      Double voltageRampRate,
+      Double outputVoltageMax,
+      Double closedLoopRampRate,
+      Double forwardOutputVoltagePeak,
+      Double reverseOutputVoltagePeak,
+      Double forwardOutputVoltageNominal,
+      Double reverseOutputVoltageNominal,
+      Integer allowableClosedLoopError,
+      Double nominalClosedLoopVoltage,
+      Double pGain, Double iGain, Double dGain, Double fGain, Integer iZone,
+      Double motionMagicAcceleration, Double motionMagicCruiseVelocity) {
+    super(name, TalonControlMode.MotionMagic, setpointMax, encoder, isBrakeInNeutral, isOutputReversed,
+        velocityMeasurementPeriod, velocityMeasurementWindow, forwardLimitSwitch,
+        reverseLimitSwitch, forwardSoftLimit, reverseSoftLimit, currentLimit, voltageRampRate,
+        outputVoltageMax, closedLoopRampRate, forwardOutputVoltagePeak, reverseOutputVoltagePeak,
+        forwardOutputVoltageNominal, reverseOutputVoltageNominal, allowableClosedLoopError,
+        nominalClosedLoopVoltage, pGain, iGain, dGain, fGain, iZone);
+    this.motionMagicAcceleration = motionMagicAcceleration;
+    this.motionMagicCruiseVelocity = motionMagicCruiseVelocity;
+  }
+
+  @Override
+  public void configure(@NotNull CANTalon talon) {
+    super.configure(talon);
+    talon.changeControlMode(TalonControlMode.MotionMagic);
+    talon.setMotionMagicAcceleration(valueOrElse(motionMagicAcceleration, 0));
+    talon.setMotionMagicCruiseVelocity(valueOrElse(motionMagicCruiseVelocity, 0));
+  }
+
+  public Double getMotionMagicAcceleration() {
+    return motionMagicAcceleration;
+  }
+
+  public Double getMotionMagicCruiseVelocity() {
+    return motionMagicCruiseVelocity;
+  }
+
+  @Override
+  public String toString() {
+    return "MotionMagicTalonConfiguration{" +
+        "motionMagicAcceleration=" + motionMagicAcceleration +
+        ", motionMagicCruiseVelocity=" + motionMagicCruiseVelocity +
+        "} " + super.toString();
+  }
+}

core/src/main/java/org/strykeforce/thirdcoast/talon/PIDTalonConfiguration.java

@@ -87,14 +87,14 @@ public void configure(@NotNull CANTalon talon) {
     super.configure(talon);
   }
 
-  private double valueOrElse(@Nullable Double value, double def) {
+  protected double valueOrElse(@Nullable Double value, double def) {
     if (value != null) {
       return value;
     }
     return def;
   }
 
-  private int valueOrElse(@Nullable Integer value, int def) {
+  protected int valueOrElse(@Nullable Integer value, int def) {
     if (value != null) {
       return value;
     }

core/src/main/java/org/strykeforce/thirdcoast/talon/TalonConfigurationBuilder.java

@@ -82,6 +82,12 @@ public class TalonConfigurationBuilder {
   @Nullable
   private Integer iZone;
 
+  // MotionMagicTalonConfiguration
+  @Nullable
+  private Double motionMagicAcceleration;
+  @Nullable
+  private Double motionMagicCruiseVelocity;
+
   /**
    * Create a builder with defaults.
    */
@@ -111,6 +117,11 @@ public TalonConfigurationBuilder(final TalonConfiguration config) {
       mode = TalonControlMode.Speed;
     } else if (config instanceof PositionTalonConfiguration) {
       mode = TalonControlMode.Position;
+    } else if (config instanceof MotionMagicTalonConfiguration) {
+      mode = TalonControlMode.MotionMagic;
+      MotionMagicTalonConfiguration mm = (MotionMagicTalonConfiguration) config;
+      motionMagicAcceleration = mm.getMotionMagicAcceleration();
+      motionMagicCruiseVelocity = mm.getMotionMagicCruiseVelocity();
     } else {
       throw new AssertionError(config.getClass().getCanonicalName());
     }
@@ -150,11 +161,13 @@ public static TalonConfiguration create(Toml config) {
       case Speed:
         talonConfiguration = config.to(SpeedTalonConfiguration.class);
         break;
+      case MotionMagic:
+        talonConfiguration = config.to(MotionMagicTalonConfiguration.class);
+        break;
       case PercentVbus:
       case Current:
       case Follower:
       case MotionProfile:
-      case MotionMagic:
       case Disabled:
         throw new UnsupportedOperationException(mode.name());
     }
@@ -214,10 +227,19 @@ public TalonConfiguration build() {
             reverseOutputVoltagePeak, forwardOutputVoltageNominal, reverseOutputVoltageNominal,
             allowableClosedLoopError, nominalClosedLoopVoltage, pGain, iGain, dGain, fGain, iZone);
         break;
-      case Follower:
       case MotionMagic:
-      case Current:
+        tc = new MotionMagicTalonConfiguration(name, setpointMax, encoder, brakeInNeutral,
+            outputReversed, velocityMeasurementPeriod, velocityMeasurementWindow,
+            forwardLimitSwitch, reverseLimitSwitch, forwardSoftLimit, reverseSoftLimit,
+            currentLimit, voltageRampRate, outputVoltageMax, closedLoopRampRate,
+            forwardOutputVoltagePeak,
+            reverseOutputVoltagePeak, forwardOutputVoltageNominal, reverseOutputVoltageNominal,
+            allowableClosedLoopError, nominalClosedLoopVoltage, pGain, iGain, dGain, fGain, iZone,
+            motionMagicAcceleration, motionMagicCruiseVelocity);
+        break;
+      case Follower:
       case MotionProfile:
+      case Current:
       case Disabled:
       case PercentVbus:
         throw new UnsupportedOperationException(mode.name());
@@ -578,6 +600,30 @@ public TalonConfigurationBuilder iZone(int iZone) {
     return this;
   }
 
+  /**
+   * Set the motion-magic acceleration.
+   *
+   * @param accel value for acceleration.
+   * @return this builder.
+   */
+  @NotNull
+  public TalonConfigurationBuilder motionMagicAcceleration(double accel) {
+    this.motionMagicAcceleration = accel;
+    return this;
+  }
+
+  /**
+   * Set the motion-magic cruise velocity.
+   *
+   * @param vel value for cruise velocity.
+   * @return this builder.
+   */
+  @NotNull
+  public TalonConfigurationBuilder motionMagicCruiseVelocity(double vel) {
+    this.motionMagicCruiseVelocity = vel;
+    return this;
+  }
+
   /**
    * Get the forward limit switch.
    *

core/src/test/groovy/org/strykeforce/thirdcoast/talon/PIDTalonConfigurationTest.groovy

@@ -38,6 +38,11 @@ class PIDTalonConfigurationTest extends Specification {
     mode = "Speed"
     setpointMax = 1.0
 
+    [[TALON]]
+    name = "motion_magic"
+    mode = "MotionMagic"
+    motionMagicAcceleration = 2.7
+    motionMagicCruiseVelocity = 6.7
 '''
 
     def talon = Mock(CANTalon)
@@ -152,4 +157,18 @@ class PIDTalonConfigurationTest extends Specification {
         t.class == SpeedTalonConfiguration
     }
 
+    def "configures motion magic mode talon"() {
+        when:
+        def t = provisioner.configurationFor("motion_magic")
+        t.configure(talon)
+
+        then:
+        t.name == "motion_magic"
+        t.class == MotionMagicTalonConfiguration
+
+        and:
+        1 * talon.setMotionMagicAcceleration(2.7)
+        1 * talon.setMotionMagicCruiseVelocity(6.7)
+    }
+
 }

core/src/test/groovy/org/strykeforce/thirdcoast/talon/TalonConfigurationBuilderTest.groovy

@@ -3,7 +3,8 @@ package org.strykeforce.thirdcoast.talon
 import com.moandjiezana.toml.Toml
 import spock.lang.Specification
 
-import static com.ctre.CANTalon.FeedbackDevice.*
+import static com.ctre.CANTalon.FeedbackDevice.EncRising
+import static com.ctre.CANTalon.FeedbackDevice.QuadEncoder
 import static com.ctre.CANTalon.TalonControlMode.*
 import static com.ctre.CANTalon.VelocityMeasurementPeriod.Period_25Ms
 import static com.ctre.CANTalon.VelocityMeasurementPeriod.Period_5Ms
@@ -328,6 +329,22 @@ pGain = 1.2
         tc.IZone == 2767
     }
 
+    def "configure motionMagicAcceleration"() {
+        when:
+        MotionMagicTalonConfiguration tc = tcb.mode(MotionMagic).motionMagicAcceleration(27.67).build()
+
+        then:
+        tc.motionMagicAcceleration == 27.67
+    }
+
+    def "configure motionMagicCruiseVelocity"() {
+        when:
+        MotionMagicTalonConfiguration tc = tcb.mode(MotionMagic).motionMagicCruiseVelocity(27.67).build()
+
+        then:
+        tc.motionMagicCruiseVelocity == 27.67
+    }
+
     def "checks config for proper operating mode"() {
         given:
         def toml = new Toml().read("mode = \"Disabled\"\nsetpointMax = 0.0")
@@ -371,5 +388,24 @@ voltageRampRate = 4.0
         vtc.voltageRampRate == 4.0
     }
 
+    def "reads motion magic params from MotionMagicTalonConfiguration"() {
+        def input = '''
+name = "foo"
+mode = "MotionMagic"
+setpointMax = 0.0
+motionMagicAcceleration = 2.7
+motionMagicCruiseVelocity = 6.7
+'''
+        when:
+        def mmtc = (MotionMagicTalonConfiguration) new TalonConfigurationBuilder(TalonConfigurationBuilder.create(new Toml().read(input))).build()
+
+        then:
+        mmtc instanceof MotionMagicTalonConfiguration
+        mmtc.name == 'foo'
+        mmtc.setpointMax == 0.0
+        mmtc.motionMagicAcceleration == 2.7
+        mmtc.motionMagicCruiseVelocity == 6.7
+    }
+
 }
 

tct/src/main/java/org/strykeforce/thirdcoast/telemetry/tct/talon/InspectCommand.java

@@ -12,6 +12,7 @@
 import org.slf4j.LoggerFactory;
 import org.strykeforce.thirdcoast.talon.Encoder;
 import org.strykeforce.thirdcoast.talon.LimitSwitch;
+import org.strykeforce.thirdcoast.talon.MotionMagicTalonConfiguration;
 import org.strykeforce.thirdcoast.talon.PIDTalonConfiguration;
 import org.strykeforce.thirdcoast.talon.SoftLimit;
 import org.strykeforce.thirdcoast.talon.TalonConfiguration;
@@ -112,6 +113,12 @@ public void perform() {
     doubleLine("D:", pid.getDGain());
     doubleLine("F:", pid.getFGain());
     intLine("I-zone:", pid.getIZone());
+    if (config.getMode() == TalonControlMode.MotionMagic) {
+      writer.println();
+      MotionMagicTalonConfiguration mmtc = (MotionMagicTalonConfiguration) pid;
+      doubleLine("MM Cruise Velocity:", mmtc.getMotionMagicCruiseVelocity());
+      doubleLine("MM Acceleration:", mmtc.getMotionMagicAcceleration());
+    }
     writer.println();
     intLine("Allowable CL Error:", pid.getAllowableClosedLoopError());
     doubleLine("Nominal CL Voltage:", pid.getNominalClosedLoopVoltage());

tct/src/main/java/org/strykeforce/thirdcoast/telemetry/tct/talon/config/cl/ClosedLoopMenuModule.java

@@ -61,4 +61,14 @@ public static Menu configMenu(@Named("TALON_CONFIG_CL") CommandAdapter commandAd
   @IntoSet
   @Named("TALON_CONFIG_CL")
   public abstract Command allowableClosedLoopErrorCommand(AllowableClosedLoopErrorCommand command);
+
+  @Binds
+  @IntoSet
+  @Named("TALON_CONFIG_CL")
+  public abstract Command motionMagicAccelerationCommand(MotionMagicAccelerationCommand command);
+
+  @Binds
+  @IntoSet
+  @Named("TALON_CONFIG_CL")
+  public abstract Command motionMagicCruiseVelocityCommand(MotionMagicCruiseVelocityCommand command);
 }

tct/src/main/java/org/strykeforce/thirdcoast/telemetry/tct/talon/config/cl/MotionMagicAccelerationCommand.java

@@ -0,0 +1,30 @@
+package org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl;
+
+import com.ctre.CANTalon;
+import javax.inject.Inject;
+import org.jline.reader.LineReader;
+import org.strykeforce.thirdcoast.telemetry.tct.talon.TalonSet;
+import org.strykeforce.thirdcoast.telemetry.tct.talon.config.AbstractDoubleConfigCommand;
+
+/**
+ * Configure Motion Magic Acceleration.
+ */
+public class MotionMagicAccelerationCommand extends AbstractDoubleConfigCommand {
+
+  public final static String NAME = "Motion Magic Acceleration";
+
+  @Inject
+  public MotionMagicAccelerationCommand(LineReader reader, TalonSet talonSet) {
+    super(NAME, reader, talonSet);
+  }
+
+  @Override
+  protected void saveConfig(double value) {
+    talonSet.talonConfigurationBuilder().motionMagicAcceleration(value);
+  }
+
+  @Override
+  protected void config(CANTalon talon, double value) {
+    talon.setMotionMagicAcceleration(value);
+  }
+}

tct/src/main/java/org/strykeforce/thirdcoast/telemetry/tct/talon/config/cl/MotionMagicCruiseVelocityCommand.java

@@ -0,0 +1,30 @@
+package org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl;
+
+import com.ctre.CANTalon;
+import javax.inject.Inject;
+import org.jline.reader.LineReader;
+import org.strykeforce.thirdcoast.telemetry.tct.talon.TalonSet;
+import org.strykeforce.thirdcoast.telemetry.tct.talon.config.AbstractDoubleConfigCommand;
+
+/**
+ * Configure Motion Magic Cruise Velocity.
+ */
+public class MotionMagicCruiseVelocityCommand extends AbstractDoubleConfigCommand {
+
+  public final static String NAME = "Motion Magic Cruise Velocity";
+
+  @Inject
+  public MotionMagicCruiseVelocityCommand(LineReader reader, TalonSet talonSet) {
+    super(NAME, reader, talonSet);
+  }
+
+  @Override
+  protected void saveConfig(double value) {
+    talonSet.talonConfigurationBuilder().motionMagicCruiseVelocity(value);
+  }
+
+  @Override
+  protected void config(CANTalon talon, double value) {
+    talon.setMotionMagicCruiseVelocity(value);
+  }
+}

tct/src/main/resources/org/strykeforce/thirdcoast/telemetry/tct/menu.toml

@@ -32,6 +32,8 @@ TALON_CONFIG_CL = [
   "org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl.FCommand",
   "org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl.IZoneCommand",
   "org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl.AllowableClosedLoopErrorCommand",
+  "org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl.MotionMagicCruiseVelocityCommand",
+  "org.strykeforce.thirdcoast.telemetry.tct.talon.config.cl.MotionMagicAccelerationCommand",
 ]
 
 TALON_CONFIG_ENC = [