From a7b289bc4956221232adb161db66e2fbee535fcf Mon Sep 17 00:00:00 2001 From: Nikolai Sukhanov Date: Tue, 12 Mar 2024 23:47:58 +0400 Subject: [PATCH] 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 * --- .../lib/servo/servo-binding-edit.component.ts | 2 +- ...vo-binding-task-payload-builder.service.ts | 66 +++++++++---------- .../motor-position-adjustment.component.ts | 4 +- .../lib/actions/attached-io-props.actions.ts | 1 + .../tasks-processing/compose-tasks.effect.ts | 2 +- ...eate-pre-run-motor-position-query-tasks.ts | 39 +++++++++++ .../scheme-pre-run/pre-run-scheme.effect.ts | 17 +++-- .../src/lib/models/attached-io-props.model.ts | 3 + .../lib/reducers/attached-io-props.reducer.ts | 20 ++++++ .../attached-io-port-mode-info.selectors.ts | 2 +- 10 files changed, 109 insertions(+), 47 deletions(-) create mode 100644 modules/store/src/lib/effects/tasks-processing/scheme-pre-run/create-pre-run-motor-position-query-tasks.ts diff --git a/modules/bindings/src/lib/servo/servo-binding-edit.component.ts b/modules/bindings/src/lib/servo/servo-binding-edit.component.ts index 840aa881..52aa766e 100644 --- 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 index 171f34da..442117c1 100644 --- 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 { - private readonly snappingThreshold = 10; - public buildPayload( binding: ControlSchemeServoBinding, currentInput: BindingInputExtractionResult, @@ -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 | null, + ioProps: Omit | 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 | null, + ioProps: Omit | null ): number { if (binding.calibrateOnStart) { const center = ioProps?.startupServoCalibrationData?.aposCenter; diff --git a/modules/shared/control-schemes/src/lib/motor-position-adjustment/motor-position-adjustment.component.ts b/modules/shared/control-schemes/src/lib/motor-position-adjustment/motor-position-adjustment.component.ts index c10954fc..5a8e2c48 100644 --- a/modules/shared/control-schemes/src/lib/motor-position-adjustment/motor-position-adjustment.component.ts +++ b/modules/shared/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 index d4202465..67929baa 100644 --- 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 index 73fa225e..84521014 100644 --- 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/modules/store/src/lib/effects/tasks-processing/scheme-pre-run/create-pre-run-motor-position-query-tasks.ts b/modules/store/src/lib/effects/tasks-processing/scheme-pre-run/create-pre-run-motor-position-query-tasks.ts new file mode 100644 index 00000000..144f7b3f --- /dev/null +++ b/modules/store/src/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> { + const uniqueIosMap = new Map(); + 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 index 83fb05ca..50f31696 100644 --- 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 index c2332807..ec3166de 100644 --- 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 index 3e14b1e5..98703f4d 100644 --- 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 index a949c3d4..d3fd8232 100644 --- 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),