fix: proper servo center position calculation for servo ranges > 360deg

* fix port value read in servo binding * remove angle snapping logic from servo binding * sequentialize servo calibration tasks * minor naming fixes *
nvsukhanov · Mar 12, 2024 · a7b289b · a7b289b
1 parent a5f9b61
commit a7b289b
Show file tree

Hide file tree

Showing 10 changed files with 109 additions and 47 deletions.
diff --git a/modules/bindings/src/lib/servo/servo-binding-edit.component.ts b/modules/bindings/src/lib/servo/servo-binding-edit.component.ts
@@ -150,7 +150,7 @@ export class ServoBindingEditComponent implements IBindingsDetailsEditComponent<
         ).subscribe(([position, offset]) => {
             // we temporarily store the result to use it later in the range read request
             this.centerReadPositionResult = position;
-            const absolutePosition = transformRelativeDegToAbsoluteDeg(position - offset);
+            const absolutePosition = transformRelativeDegToAbsoluteDeg(position + offset);
             if (this._form && this._form.controls.aposCenter.value !== absolutePosition) {
                 this._form.controls.aposCenter.setValue(absolutePosition);
                 this._form.controls.aposCenter.markAsDirty();

diff --git a/modules/bindings/src/lib/servo/servo-binding-task-payload-builder.service.ts b/modules/bindings/src/lib/servo/servo-binding-task-payload-builder.service.ts
@@ -1,6 +1,6 @@
 import { MotorServoEndState } from 'rxpoweredup';
 import { Injectable } from '@angular/core';
-import { ControlSchemeBindingType, getTranslationArcs } from '@app/shared-misc';
+import { ControlSchemeBindingType, getTranslationArcs, transformRelativeDegToAbsoluteDeg } from '@app/shared-misc';
 import {
     AttachedIoPropsModel,
     ControlSchemeServoBinding,
@@ -17,8 +17,6 @@ import { BindingInputExtractionResult } from '../i-binding-task-input-extractor'
 
 @Injectable()
 export class ServoBindingTaskPayloadBuilderService implements ITaskPayloadBuilder<ControlSchemeBindingType.Servo> {
-    private readonly snappingThreshold = 10;
-
     public buildPayload(
         binding: ControlSchemeServoBinding,
         currentInput: BindingInputExtractionResult<ControlSchemeBindingType.Servo>,
@@ -29,26 +27,37 @@ export class ServoBindingTaskPayloadBuilderService implements ITaskPayloadBuilde
         const cwInput = currentInput[ServoBindingInputAction.Cw];
         const ccwInput = currentInput[ServoBindingInputAction.Ccw];
 
+        // If this is not the first task and there is no input - we do nothing
         if (!cwInput && !ccwInput && !!previousTaskPayload) {
-            // If this is not the first task and there is no input - we do nothing
             return null;
         }
 
+        // without necessary data we can't do anything
+        if (!ioProps?.startupMotorPositionData || ioProps.motorEncoderOffset === null) {
+            // TODO: log an error
+            return null;
+        }
+
+        const startingAbsolutePosition = transformRelativeDegToAbsoluteDeg(ioProps.startupMotorPositionData.position + ioProps.motorEncoderOffset);
+
         const translationPaths = getTranslationArcs(
-            ioProps?.motorEncoderOffset ?? 0,
-            this.getArcCenter(binding, ioProps)
+            startingAbsolutePosition,
+            this.getAposCenter(binding, ioProps)
         );
-        const resultingCenter = translationPaths.cw < translationPaths.ccw ? translationPaths.cw : -translationPaths.ccw;
+
+        const resultingCenterPosition = translationPaths.cw < translationPaths.ccw
+                                        ? ioProps.startupMotorPositionData.position + translationPaths.cw
+                                        : ioProps.startupMotorPositionData.position - translationPaths.ccw;
 
         if (!cwInput && !ccwInput) {
             // If there were no inputs and no previous task, we should center the servo
             return this.composeResult(
-                resultingCenter,
+                resultingCenterPosition,
                 binding.speed,
                 binding.power,
                 binding.useAccelerationProfile,
                 binding.useDecelerationProfile,
-                Date.now()
+                0
             );
         }
 
@@ -57,26 +66,22 @@ export class ServoBindingTaskPayloadBuilderService implements ITaskPayloadBuilde
         const cwValue = extractDirectionAwareInputValue(cwInput?.value ?? 0, cwInputDirection);
         const ccwValue = extractDirectionAwareInputValue(ccwInput?.value ?? 0, ccwInputDirection);
 
-        const servoNonClampedInputValue = Math.abs(cwValue) - Math.abs(ccwValue);
+        const cumulativeInputValue = Math.abs(cwValue) - Math.abs(ccwValue);
 
         // TODO: create a function to clamp the value
-        const servoInputValue = Math.max(-1, Math.min(1, servoNonClampedInputValue));
-        
+        const clampedCumulativeInputValue = Math.max(-1, Math.min(1, cumulativeInputValue));
+
         // Math.max will never return 0 here because there is at least one input.
         // Null coalescing operator is used to avoid errors in case if any of the inputs being null
         const inputTimestamp = Math.max(cwInput?.timestamp ?? 0, ccwInput?.timestamp ?? 0);
 
-        const arcSize = this.getArcSize(binding, ioProps);
-        const arcPosition = servoInputValue * arcSize / 2;
+        const servoRange = this.getServoRange(binding, ioProps);
+        const rangePosition = clampedCumulativeInputValue * servoRange / 2;
 
-        const targetAngle = arcPosition + resultingCenter;
-        const minAngle = resultingCenter - arcSize / 2;
-        const maxAngle = resultingCenter + arcSize / 2;
-
-        const snappedAngle = this.snapAngle(targetAngle, resultingCenter, minAngle, maxAngle);
+        const targetAngle = rangePosition + resultingCenterPosition;
 
         return this.composeResult(
-            snappedAngle,
+            targetAngle,
             binding.speed,
             binding.power,
             binding.useAccelerationProfile,
@@ -117,26 +122,15 @@ export class ServoBindingTaskPayloadBuilderService implements ITaskPayloadBuilde
                 power,
                 endState: MotorServoEndState.hold,
                 useAccelerationProfile,
-                useDecelerationProfile,
+                useDecelerationProfile
             },
             inputTimestamp
         };
     }
 
-    private snapAngle(
-        targetAngle: number,
-        arcCenter: number,
-        maxAngle: number,
-        minAngle: number
-    ): number {
-        const snappedToZeroAngle = Math.abs(targetAngle - arcCenter) < this.snappingThreshold ? arcCenter : targetAngle;
-        const snappedToMaxAngle = Math.abs(snappedToZeroAngle - maxAngle) < this.snappingThreshold ? maxAngle : snappedToZeroAngle;
-        return Math.abs(snappedToMaxAngle - minAngle) < this.snappingThreshold ? minAngle : snappedToMaxAngle;
-    }
-
-    private getArcSize(
+    private getServoRange(
         binding: ControlSchemeServoBinding,
-        ioProps: Omit<AttachedIoPropsModel, 'hubId' | 'portId'> | null,
+        ioProps: Omit<AttachedIoPropsModel, 'hubId' | 'portId'> | null
     ): number {
         if (binding.calibrateOnStart) {
             const range = ioProps?.startupServoCalibrationData?.range;
@@ -148,9 +142,9 @@ export class ServoBindingTaskPayloadBuilderService implements ITaskPayloadBuilde
         return binding.range;
     }
 
-    private getArcCenter(
+    private getAposCenter(
         binding: ControlSchemeServoBinding,
-        ioProps: Omit<AttachedIoPropsModel, 'hubId' | 'portId'> | null,
+        ioProps: Omit<AttachedIoPropsModel, 'hubId' | 'portId'> | null
     ): number {
         if (binding.calibrateOnStart) {
             const center = ioProps?.startupServoCalibrationData?.aposCenter;

diff --git a/.../control-schemes/src/lib/motor-position-adjustment/motor-position-adjustment.component.ts b/.../control-schemes/src/lib/motor-position-adjustment/motor-position-adjustment.component.ts
@@ -52,12 +52,12 @@ export class MotorPositionAdjustmentComponent implements OnChanges, OnDestroy {
             this._canGoToZero$ = of(false);
             return;
         }
-        this._canExecuteStep$ = this.store.select(ATTACHED_IO_PORT_MODE_INFO_SELECTORS.selectIoCanOperateOutputPortModeName({
+        this._canExecuteStep$ = this.store.select(ATTACHED_IO_PORT_MODE_INFO_SELECTORS.selectIsIoSupportInputMode({
             hubId: this.hubId,
             portId: this.portId,
             portModeName: PortModeName.position
         }));
-        this._canGoToZero$ = this.store.select(ATTACHED_IO_PORT_MODE_INFO_SELECTORS.selectIoCanOperateOutputPortModeName({
+        this._canGoToZero$ = this.store.select(ATTACHED_IO_PORT_MODE_INFO_SELECTORS.selectIsIoSupportInputMode({
             hubId: this.hubId,
             portId: this.portId,
             portModeName: PortModeName.absolutePosition

diff --git a/modules/store/src/lib/actions/attached-io-props.actions.ts b/modules/store/src/lib/actions/attached-io-props.actions.ts
@@ -5,6 +5,7 @@ export const ATTACHED_IO_PROPS_ACTIONS = createActionGroup({
     events: {
         'motor encoder offset received': props<{ hubId: string; portId: number; offset: number }>(),
         'startup servo calibration data received': props<{ hubId: string; portId: number; range: number; aposCenter: number }>(),
+        'startup motor position received': props<{ hubId: string; portId: number; position: number }>(),
         'compensate pitch': props<{ hubId: string; portId: number; currentPitch: number }>(),
         'compensate yaw': props<{ hubId: string; portId: number; currentYaw: number }>(),
         'compensate roll': props<{ hubId: string; portId: number; currentRoll: number }>(),

diff --git a/modules/store/src/lib/effects/tasks-processing/compose-tasks.effect.ts b/modules/store/src/lib/effects/tasks-processing/compose-tasks.effect.ts
@@ -138,7 +138,7 @@ export const COMPOSE_TASKS_EFFECT = createEffect((
                 tasks
             );
             const shouldUpdateQueue = nextPendingTask !== pendingTask;
-            const isNextTaskMoreRecent = (nextPendingTask?.inputTimestamp ?? 0) > (currentTask?.inputTimestamp ?? 0);
+            const isNextTaskMoreRecent = (nextPendingTask?.inputTimestamp ?? -Infinity) > (currentTask?.inputTimestamp ?? -Infinity);
             return {
                 hubId,
                 portId,

diff --git a/.../lib/effects/tasks-processing/scheme-pre-run/create-pre-run-motor-position-query-tasks.ts b/.../lib/effects/tasks-processing/scheme-pre-run/create-pre-run-motor-position-query-tasks.ts
@@ -0,0 +1,39 @@
+import { EMPTY, Observable, first, switchMap, tap } from 'rxjs';
+import { Store } from '@ngrx/store';
+import { PortModeName, ValueTransformers } from 'rxpoweredup';
+
+import { ControlSchemeModel } from '../../../models';
+import { HubStorageService } from '../../../hub-storage.service';
+import { attachedIosIdFn } from '../../../reducers';
+import { ATTACHED_IO_PROPS_ACTIONS } from '../../../actions';
+import { ATTACHED_IO_PORT_MODE_INFO_SELECTORS } from '../../../selectors';
+
+export function createPreRunMotorPositionQueryTasks(
+    scheme: ControlSchemeModel,
+    hubStorage: HubStorageService,
+    store: Store
+): Array<Observable<unknown>> {
+    const uniqueIosMap = new Map<string, { hubId: string; portId: number }>();
+    scheme.bindings.forEach((binding) => {
+        uniqueIosMap.set(attachedIosIdFn(binding), { hubId: binding.hubId, portId: binding.portId });
+    });
+    const uniqueIos = Array.from(uniqueIosMap.values());
+
+    return uniqueIos.map(({ hubId, portId }) => {
+        return store.select(
+                ATTACHED_IO_PORT_MODE_INFO_SELECTORS.selectHubPortInputModeForPortModeName({ hubId, portId, portModeName: PortModeName.position })
+        ).pipe(
+            first(),
+            switchMap((portModeData) => {
+                if (portModeData === null) {
+                    return EMPTY;
+                }
+                return hubStorage.get(hubId).ports.getPortValue(portId, portModeData.modeId, ValueTransformers.position);
+            }),
+            tap((position) => {
+                store.dispatch(ATTACHED_IO_PROPS_ACTIONS.startupMotorPositionReceived({ hubId, portId, position }));
+            }),
+            first()
+        );
+    });
+}
diff --git a/modules/store/src/lib/effects/tasks-processing/scheme-pre-run/pre-run-scheme.effect.ts b/modules/store/src/lib/effects/tasks-processing/scheme-pre-run/pre-run-scheme.effect.ts
@@ -1,5 +1,5 @@
 import { Actions, concatLatestFrom, createEffect, ofType } from '@ngrx/effects';
-import { catchError, filter, forkJoin, map, of, switchMap, timeout } from 'rxjs';
+import { catchError, concatWith, filter, forkJoin, last, map, of, switchMap, tap, timeout } from 'rxjs';
 import { inject } from '@angular/core';
 import { Store } from '@ngrx/store';
 import { APP_CONFIG, IAppConfig } from '@app/shared-misc';
@@ -14,6 +14,7 @@ import { createPreRunSetDecelerationProfileTasks } from './create-pre-run-set-de
 import { createWidgetReadTasks } from './create-widget-read-tasks';
 import { HubServoCalibrationFacadeService } from '../../../hub-facades';
 import { IWidgetsReadTasksFactory, WIDGET_READ_TASKS_FACTORY } from './i-widgets-read-tasks-factory';
+import { createPreRunMotorPositionQueryTasks } from './create-pre-run-motor-position-query-tasks';
 
 export const PRE_RUN_SCHEME_EFFECT = createEffect((
     actions: Actions = inject(Actions),
@@ -32,17 +33,21 @@ export const PRE_RUN_SCHEME_EFFECT = createEffect((
             const combinedTasks = [
                 ...createPreRunSetAccelerationProfileTasks(scheme, hubStorage),
                 ...createPreRunSetDecelerationProfileTasks(scheme, hubStorage),
-                ...createWidgetReadTasks(scheme, store, widgetReadTaskFactory)
+                ...createWidgetReadTasks(scheme, store, widgetReadTaskFactory),
+                ...createPreRunMotorPositionQueryTasks(scheme, hubStorage, store)
             ];
             // TODO: move to Bindings module
             const calibrationServoTasks = createPreRunServoCalibrationTasks(scheme, hubCalibrationFacade, store, appConfig);
-            if (calibrationServoTasks.length > 0) {
-                combinedTasks.push(forkJoin(calibrationServoTasks));
-            }
-            if (combinedTasks.length === 0) {
+            if (combinedTasks.length + calibrationServoTasks.length === 0) {
                 return of(CONTROL_SCHEME_ACTIONS.schemeStarted({ name: scheme.name }));
             }
+
             return forkJoin(combinedTasks).pipe(
+                // We have to start calibration tasks after all other tasks are done to avoid race conditions with position queries.
+                // Also, somehow running calibration tasks in parallel causes issues with receiving port values.
+                // For now, we run them sequentially. TODO: investigate and fix the root cause.
+                calibrationServoTasks.length > 0 ? concatWith(...calibrationServoTasks) : tap(() => void 0),
+                last(),
                 timeout(appConfig.schemeStartStopTimeoutMs),
                 map(() => CONTROL_SCHEME_ACTIONS.schemeStarted({ name: scheme.name })),
                 catchError((e) => of(CONTROL_SCHEME_ACTIONS.schemeStartFailed({ reason: e })))

diff --git a/modules/store/src/lib/models/attached-io-props.model.ts b/modules/store/src/lib/models/attached-io-props.model.ts
@@ -8,5 +8,8 @@ export type AttachedIoPropsModel = {
         range: number;
         aposCenter: number;
     } | null;
+    startupMotorPositionData: {
+        position: number;
+    } | null;
     runtimeTiltCompensation: TiltData | null;
 };
diff --git a/modules/store/src/lib/reducers/attached-io-props.reducer.ts b/modules/store/src/lib/reducers/attached-io-props.reducer.ts
@@ -31,6 +31,7 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                     portId: data.portId,
                     motorEncoderOffset: data.offset,
                     startupServoCalibrationData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupServoCalibrationData ?? null,
+                    startupMotorPositionData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupMotorPositionData ?? null,
                     runtimeTiltCompensation: state.entities[hubAttachedIoPropsIdFn(data)]?.runtimeTiltCompensation ?? null,
                 },
                 state
@@ -41,6 +42,7 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                     hubId: data.hubId,
                     portId: data.portId,
                     motorEncoderOffset: state.entities[hubAttachedIoPropsIdFn(data)]?.motorEncoderOffset ?? 0,
+                    startupMotorPositionData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupMotorPositionData ?? null,
                     startupServoCalibrationData: {
                         aposCenter: data.aposCenter,
                         range: data.range,
@@ -57,6 +59,7 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                     portId: data.portId,
                     motorEncoderOffset: state.entities[hubAttachedIoPropsIdFn(data)]?.motorEncoderOffset ?? 0,
                     startupServoCalibrationData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupServoCalibrationData ?? null,
+                    startupMotorPositionData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupMotorPositionData ?? null,
                     runtimeTiltCompensation: {
                         yaw: currentCompensation.yaw,
                         pitch: currentCompensation.pitch + data.currentPitch,
@@ -73,6 +76,7 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                     portId: data.portId,
                     motorEncoderOffset: state.entities[hubAttachedIoPropsIdFn(data)]?.motorEncoderOffset ?? 0,
                     startupServoCalibrationData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupServoCalibrationData ?? null,
+                    startupMotorPositionData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupMotorPositionData ?? null,
                     runtimeTiltCompensation: {
                         yaw: currentCompensation.yaw,
                         pitch: 0,
@@ -89,6 +93,7 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                     portId: data.portId,
                     motorEncoderOffset: state.entities[hubAttachedIoPropsIdFn(data)]?.motorEncoderOffset ?? 0,
                     startupServoCalibrationData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupServoCalibrationData ?? null,
+                    startupMotorPositionData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupMotorPositionData ?? null,
                     runtimeTiltCompensation: {
                         yaw: currentCompensation.yaw + data.currentYaw,
                         pitch: currentCompensation.pitch,
@@ -105,6 +110,7 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                     portId: data.portId,
                     motorEncoderOffset: state.entities[hubAttachedIoPropsIdFn(data)]?.motorEncoderOffset ?? 0,
                     startupServoCalibrationData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupServoCalibrationData ?? null,
+                    startupMotorPositionData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupMotorPositionData ?? null,
                     runtimeTiltCompensation: {
                         yaw: 0,
                         pitch: currentCompensation.pitch,
@@ -114,6 +120,20 @@ export const ATTACHED_IO_PROPS_FEATURE = createFeature({
                 state
             );
         }),
+        on(ATTACHED_IO_PROPS_ACTIONS.startupMotorPositionReceived, (state, data): AttacheIoPropsState => {
+            return ATTACHED_IO_PROPS_ENTITY_ADAPTER.upsertOne({
+                    hubId: data.hubId,
+                    portId: data.portId,
+                    motorEncoderOffset: state.entities[hubAttachedIoPropsIdFn(data)]?.motorEncoderOffset ?? 0,
+                    startupServoCalibrationData: state.entities[hubAttachedIoPropsIdFn(data)]?.startupServoCalibrationData ?? null,
+                    startupMotorPositionData: {
+                        position: data.position,
+                    },
+                    runtimeTiltCompensation: state.entities[hubAttachedIoPropsIdFn(data)]?.runtimeTiltCompensation ?? null,
+                },
+                state
+            );
+        }),
         on(HUBS_ACTIONS.forgetHub, HUBS_ACTIONS.disconnected, (state, { hubId }): AttacheIoPropsState =>
             ATTACHED_IO_PROPS_ENTITY_ADAPTER.removeMany((io) => io.hubId === hubId, state)
         )

diff --git a/modules/store/src/lib/selectors/attached-io-port-mode-info.selectors.ts b/modules/store/src/lib/selectors/attached-io-port-mode-info.selectors.ts
@@ -67,7 +67,7 @@ export const ATTACHED_IO_PORT_MODE_INFO_SELECTORS = {
             return null;
         }
     ),
-    selectIoCanOperateOutputPortModeName: (
+    selectIsIoSupportInputMode: (
         { hubId, portId, portModeName }: { hubId: string; portId: number; portModeName: PortModeName }
     ) => createSelector(
         HUB_RUNTIME_DATA_SELECTORS.selectIsHubConnected(hubId),