merge in main

packages/core/src/ClosedLoopControlAlgorithm.ts

@@ -0,0 +1,4 @@
+export enum ClosedLoopControlAlgorithm {
+    Pid,
+    Pidf,
+}

packages/core/src/ExpansionHub.ts

@@ -14,6 +14,8 @@ import { I2CReadStatus } from "./I2CReadStatus.js";
 import { PidCoefficients } from "./PidCoefficients.js";
 import { DigitalChannelDirection } from "./DigitalChannelDirection.js";
 import { MotorMode } from "./MotorMode.js";
+import { PidfCoefficients } from "./PidfCoefficients.js";
+import { ClosedLoopControlAlgorithm } from "./ClosedLoopControlAlgorithm.js";
 
 export type ParentExpansionHub = ParentRevHub & ExpansionHub;
 
@@ -322,26 +324,62 @@ export interface ExpansionHub extends RevHub {
     getMotorEncoderPosition(motorChannel: number): Promise<number>;
 
     /**
-     * Set the current PID coefficients for a motor
+     * Set the Closed Loop Control Coefficients for PID mode.
+     *
      * @param motorChannel
      * @param motorMode
-     * @param pid
+     * @param algorithm PID algorithm
+     * @param pid PID coefficients
      */
-    setMotorPIDCoefficients(
+    setMotorClosedLoopControlCoefficients(
         motorChannel: number,
         motorMode: MotorMode,
+        algorithm: ClosedLoopControlAlgorithm.Pid,
         pid: PidCoefficients,
     ): Promise<void>;
 
     /**
-     * Get the current PID coefficients for a motor
+     * Set the Closed Loop Control Coefficients for PIDF mode
+     *
+     * @param motorChannel
+     * @param motorMode
+     * @param algorithm PIDF algorithm
+     * @param pidf PIDF coefficients
+     */
+    setMotorClosedLoopControlCoefficients(
+        motorChannel: number,
+        motorMode: MotorMode,
+        algorithm: ClosedLoopControlAlgorithm.Pidf,
+        pidf: PidfCoefficients,
+    ): Promise<void>;
+
+    /**
+     * Set Motor Closed Loop Coefficients using either PID algorithm or
+     * PIDF algorithm.
+     *
+     * @param motorChannel
+     * @param motorMode
+     * @param algorithm either PID or PIDF algorithm.
+     * @param pid either PidCoefficients or PidfCoefficients depending on algorithm.
+     */
+    setMotorClosedLoopControlCoefficients(
+        motorChannel: number,
+        motorMode: MotorMode,
+        algorithm: ClosedLoopControlAlgorithm,
+        pid: PidCoefficients | PidfCoefficients,
+    ): Promise<void>;
+
+    /**
+     * Get motor closed loop control coefficients.
+     * Will return either PidCoefficients or PidfCoefficients depending on which
+     * algorithm is active.
      * @param motorChannel
      * @param motorMode
      */
-    getMotorPIDCoefficients(
+    getMotorClosedLoopControlCoefficients(
         motorChannel: number,
         motorMode: MotorMode,
-    ): Promise<PidCoefficients>;
+    ): Promise<PidfCoefficients | PidCoefficients>;
 
     // Servo
     /**

packages/core/src/PidCoefficients.ts

@@ -1,5 +1,9 @@
+import { ClosedLoopControlAlgorithm } from "./ClosedLoopControlAlgorithm.js";
+
 export interface PidCoefficients {
     p: number;
     i: number;
     d: number;
+
+    algorithm: ClosedLoopControlAlgorithm.Pid;
 }

packages/core/src/PidfCoefficients.ts

@@ -0,0 +1,10 @@
+import { ClosedLoopControlAlgorithm } from "./ClosedLoopControlAlgorithm.js";
+
+export interface PidfCoefficients {
+    p: number;
+    i: number;
+    d: number;
+    f: number;
+
+    algorithm: ClosedLoopControlAlgorithm.Pidf;
+}

packages/core/src/RevHub.ts

@@ -15,6 +15,7 @@ export interface RevHub {
      * @param listener
      */
     on(eventName: "error", listener: (error: Error) => void): RevHub;
+    close(): any;
 }
 
 export interface ParentRevHub extends RevHub {

packages/core/src/index.ts

@@ -16,12 +16,15 @@ export * from "./LedPattern.js";
 export * from "./ModuleInterface.js";
 export * from "./ModuleStatus.js";
 export * from "./PidCoefficients.js";
+export * from "./PidfCoefficients.js";
+export * from "./ClosedLoopControlAlgorithm.js";
 export * from "./Rgb.js";
 export * from "./MotorMode.js";
 export * from "./SerialFlowControl.js";
 export * from "./SerialParity.js";
 export * from "./VerbosityLevel.js";
 export * from "./Version.js";
+export * from "./serial-errors.js";
 export * from "./nack-errors/NackError.js";
 export * from "./nack-errors/BatteryTooLowError.js";
 export * from "./nack-errors/diagnostic-errors.js";

packages/expansion-hub/src/discovery.ts

@@ -1,6 +1,6 @@
 import { SerialPort } from "serialport";
 import { openExpansionHubAndAllChildren } from "./open-rev-hub.js";
-import { ExpansionHub } from "@rev-robotics/rev-hub-core";
+import { ExpansionHub, ParentExpansionHub } from "@rev-robotics/rev-hub-core";
 
 export async function getPossibleExpansionHubSerialNumbers(): Promise<string[]> {
     const results: string[] = [];
@@ -22,10 +22,10 @@ export async function getPossibleExpansionHubSerialNumbers(): Promise<string[]>
  * Returns all connected RevHubs. The array contains parent RevHubs,
  * and child hubs are available via {@link ExpansionHub#children}.
  */
-export async function openConnectedExpansionHubs(): Promise<ExpansionHub[]> {
+export async function openConnectedExpansionHubs(): Promise<ParentExpansionHub[]> {
     let serialNumbers = await getPossibleExpansionHubSerialNumbers();
 
-    let hubs: ExpansionHub[] = [];
+    let hubs: ParentExpansionHub[] = [];
 
     for (let serialNumber of serialNumbers) {
         let hub = await openExpansionHubAndAllChildren(serialNumber);

packages/expansion-hub/src/internal/ExpansionHub.ts

@@ -1,37 +1,43 @@
 import {
+    NackCode,
     NativeRevHub,
-    Serial as SerialPort,
     RhspLibErrorCode,
-    NackCode,
+    Serial as SerialPort,
 } from "@rev-robotics/rhsplib";
 import {
     BulkInputData,
+    ClosedLoopControlAlgorithm,
+    CommandNotSupportedError,
     DebugGroup,
     DigitalChannelDirection,
+    DigitalState,
+    ExpansionHub,
+    GeneralSerialError,
     I2CReadStatus,
     I2CSpeedCode,
     I2CWriteStatus,
     LedPattern,
     ModuleInterface,
     ModuleStatus,
+    MotorMode,
+    nackCodeToError,
+    NoExpansionHubWithAddressError,
+    ParameterOutOfRangeError,
+    ParentRevHub,
     PidCoefficients,
+    PidfCoefficients,
+    RevHub,
+    RevHubType,
     Rgb,
+    TimeoutError,
     VerbosityLevel,
     Version,
-    TimeoutError,
-    nackCodeToError,
-    NoExpansionHubWithAddressError,
-    ParameterOutOfRangeError,
-    GeneralSerialError,
-    CommandNotSupportedError,
-    ExpansionHub,
-    DigitalState,
-    MotorMode,
 } from "@rev-robotics/rev-hub-core";
 import { closeSerialPort } from "../open-rev-hub.js";
-import { ParentRevHub, RevHub, RevHubType } from "@rev-robotics/rev-hub-core";
 import { EventEmitter } from "events";
 import { RhspLibError } from "../errors/RhspLibError.js";
+import { startKeepAlive } from "../start-keep-alive.js";
+import { performance } from "perf_hooks";
 
 export class ExpansionHubInternal implements ExpansionHub {
     constructor(isParent: true, serial: SerialPort, serialNumber: string);
@@ -284,16 +290,49 @@ export class ExpansionHubInternal implements ExpansionHub {
         });
     }
 
-    getMotorPIDCoefficients(
+    async setMotorClosedLoopControlCoefficients(
         motorChannel: number,
         motorMode: MotorMode,
-    ): Promise<PidCoefficients> {
-        return this.convertErrorPromise(() => {
-            //ToDo (landry): convert the PID coefficients in C code
-            return this.nativeRevHub.getMotorPIDCoefficients(motorChannel, motorMode);
+        algorithm: ClosedLoopControlAlgorithm,
+        pid: PidCoefficients | PidfCoefficients,
+    ): Promise<void> {
+        await this.convertErrorPromise(async () => {
+            await this.nativeRevHub.setMotorClosedLoopControlCoefficients(
+                motorChannel,
+                motorMode,
+                algorithm,
+                pid,
+            );
         });
     }
 
+    async getMotorClosedLoopControlCoefficients(
+        motorChannel: number,
+        motorMode: MotorMode,
+    ): Promise<PidfCoefficients | PidCoefficients> {
+        let pid: any = await this.nativeRevHub.getMotorClosedLoopControlCoefficients(
+            motorChannel,
+            motorMode,
+        );
+
+        if (pid.algorithm === ClosedLoopControlAlgorithm.Pid) {
+            return {
+                p: pid.p,
+                i: pid.i,
+                d: pid.d,
+                algorithm: ClosedLoopControlAlgorithm.Pid,
+            };
+        } else {
+            return {
+                p: pid.p,
+                i: pid.i,
+                d: pid.d,
+                f: pid.f,
+                algorithm: ClosedLoopControlAlgorithm.Pidf,
+            };
+        }
+    }
+
     getMotorTargetPosition(
         motorChannel: number,
     ): Promise<{ targetPosition: number; targetTolerance: number }> {
@@ -524,10 +563,7 @@ export class ExpansionHubInternal implements ExpansionHub {
 
     setMotorConstantPower(motorChannel: number, powerLevel: number): Promise<void> {
         return this.convertErrorPromise(() => {
-            return this.nativeRevHub.setMotorConstantPower(
-                motorChannel,
-                powerLevel * 32_767,
-            );
+            return this.nativeRevHub.setMotorConstantPower(motorChannel, powerLevel);
         });
     }
 
@@ -537,7 +573,6 @@ export class ExpansionHubInternal implements ExpansionHub {
         pid: PidCoefficients,
     ): Promise<void> {
         return this.convertErrorPromise(() => {
-            //ToDo (landry): convert the PID coefficients in C code
             return this.nativeRevHub.setMotorPIDCoefficients(
                 motorChannel,
                 motorMode,
@@ -546,6 +581,20 @@ export class ExpansionHubInternal implements ExpansionHub {
         });
     }
 
+    setMotorPIDFCoefficients(
+        motorChannel: number,
+        motorMode: MotorMode,
+        pidf: PidfCoefficients,
+    ): Promise<void> {
+        return this.convertErrorPromise(() => {
+            return this.nativeRevHub.setMotorPIDFCoefficients(
+                motorChannel,
+                motorMode,
+                pidf,
+            );
+        });
+    }
+
     setMotorTargetPosition(
         motorChannel: number,
         targetPosition_counts: number,
@@ -692,15 +741,32 @@ export class ExpansionHubInternal implements ExpansionHub {
 
         try {
             await childHub.open(moduleAddress);
+            // If discovery has not occurred on the hub, then we will
+            // need to send keep-alive signals until the hub responds.
+            // If we don't do this, the hub will be stuck waiting to
+            // find out if it's a parent or child and won't respond.
+            let startTime = performance.now();
+            while (true) {
+                try {
+                    if (performance.now() - startTime >= 1000) break;
+                    await childHub.sendKeepAlive();
+                    break;
+                } catch (e) {}
+            }
             await childHub.queryInterface("DEKA");
         } catch (e: any) {
+            console.log(e);
             if (e instanceof TimeoutError)
                 throw new NoExpansionHubWithAddressError(
                     this.serialNumber!, //Can only call this method on parent, so serialNumber is not undefined.
                     moduleAddress,
                 );
         }
 
+        if (childHub.isExpansionHub()) {
+            startKeepAlive(childHub as ExpansionHubInternal, 1000);
+        }
+
         this.addChild(childHub);
 
         return childHub;

packages/expansion-hub/src/open-rev-hub.ts

@@ -13,12 +13,14 @@ import {
     InvalidSerialArguments,
     NoExpansionHubWithAddressError,
     ParentExpansionHub,
+    RevHub,
     SerialConfigurationError,
     SerialIoError,
     SerialParity,
     TimeoutError,
     UnableToOpenSerialError,
 } from "@rev-robotics/rev-hub-core";
+import { performance } from "perf_hooks";
 
 /**
  * Maps the serial port path (/dev/tty1 or COM3 for example) to an open
@@ -59,6 +61,18 @@ export async function openParentExpansionHub(
 
     try {
         await parentHub.open(moduleAddress);
+        // If discovery has not occurred on the hub, then we will
+        // need to send keep-alive signals until the hub responds.
+        // If we don't do this, the hub will be stuck waiting to
+        // find out if it's a parent or child and won't respond.
+        let startTime = performance.now();
+        while (true) {
+            try {
+                if (performance.now() - startTime >= 500) break;
+                await parentHub.sendKeepAlive();
+                break;
+            } catch (e) {}
+        }
         await parentHub.queryInterface("DEKA");
     } catch (e: any) {
         if (e instanceof TimeoutError)
@@ -94,16 +108,10 @@ export async function openExpansionHubAndAllChildren(
 
     let discoveredModules = await NativeRevHub.discoverRevHubs(serialPort);
     let parentAddress = discoveredModules.parentAddress;
-    let parentHub = (await openParentExpansionHub(
-        serialNumber,
-        parentAddress,
-    )) as ParentExpansionHub & ExpansionHubInternal;
+    let parentHub = await openParentExpansionHub(serialNumber, parentAddress);
 
     for (let address of discoveredModules.childAddresses) {
-        let hub = await parentHub.addChildByAddress(address);
-        if (hub.isExpansionHub()) {
-            startKeepAlive(hub as ExpansionHubInternal, 1000);
-        }
+        await parentHub.addChildByAddress(address);
     }
 
     return parentHub;