From 63f1c10fcc8ae3921c02e5337a81ef8306cd6bc9 Mon Sep 17 00:00:00 2001 From: Nathan Kellenicki Date: Fri, 10 Jan 2020 12:27:23 -0800 Subject: [PATCH] Servo calibration and reset --- src/devices/absolutemotor.ts | 95 ++++++++++++++++++++++++++++++++---- src/devices/basicmotor.ts | 12 ++++- src/devices/device.ts | 1 - src/devices/tachomotor.ts | 12 ++--- src/interfaces.ts | 1 + src/utils.ts | 17 +++++++ 6 files changed, 120 insertions(+), 18 deletions(-) diff --git a/src/devices/absolutemotor.ts b/src/devices/absolutemotor.ts index 7d89a0d..54e2f53 100644 --- a/src/devices/absolutemotor.ts +++ b/src/devices/absolutemotor.ts @@ -3,7 +3,7 @@ import { TachoMotor } from "./tachomotor"; import { IDeviceInterface } from "../interfaces"; import * as Consts from "../consts"; -import { mapSpeed, normalizeAngle } from "../utils"; +import { mapSpeed, normalizeAngle, roundAngleToNearest90 } from "../utils"; export class AbsoluteMotor extends TachoMotor { @@ -32,12 +32,12 @@ export class AbsoluteMotor extends TachoMotor { /** * Rotate a motor by a given angle. - * @method AbsoluteMotor#gotoAbsolutePosition + * @method AbsoluteMotor#gotoAngle * @param {number} angle Absolute position the motor should go to (degrees from 0). * @param {number} [speed=100] For forward, a value between 1 - 100 should be set. For reverse, a value between -1 to -100. * @returns {Promise} Resolved upon successful completion of command (ie. once the motor is finished). */ - public gotoAbsoluteAngle (angle: [number, number] | number, speed: number = 100) { + public gotoAngle (angle: [number, number] | number, speed: number = 100) { if (!this.isVirtualPort && angle instanceof Array) { throw new Error("Only virtual ports can accept multiple positions"); } @@ -51,28 +51,27 @@ export class AbsoluteMotor extends TachoMotor { } let message; if (angle instanceof Array) { - message = Buffer.from([0x81, this.portId, 0x11, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7e, 0x00]); message.writeInt32LE(normalizeAngle(angle[0]), 4); message.writeInt32LE(normalizeAngle(angle[1]), 8); } else { - message = Buffer.from([0x81, this.portId, 0x11, 0x0d, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x0d, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7e, 0x00]); message.writeInt32LE(normalizeAngle(angle), 4); } this.send(message); this._finished = () => { - console.log("RESOLVE"); return resolve(); }; }); } /** - * (Re)set the knowledge of the absolute position to the current position. - * @method AbsoluteMotor#resetAbsolutePosition + * (Re)set the knowledge of the absolute position to the current angle. + * @method AbsoluteMotor#resetAngle * @param {number} angle Position to set (degrees from 0). * @returns {Promise} Resolved upon successful completion of command (ie. once the motor is finished). */ - public resetAbsoluteAngle (angle: [number, number] | number) { + public resetAngle (angle: [number, number] | number) { if (!this.isVirtualPort && angle instanceof Array) { throw new Error("Only virtual ports can accept multiple positions"); } @@ -85,7 +84,7 @@ export class AbsoluteMotor extends TachoMotor { if (angle instanceof Array) { message = Buffer.from([0x81, this.portId, 0x11, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]); message.writeInt32LE(normalizeAngle(angle[0]), 4); - message.writeInt32LE(normalizeAngle(angle[0]), 8); + message.writeInt32LE(normalizeAngle(angle[1]), 8); } else { message = Buffer.from([0x81, this.portId, 0x11, 0x51, 0x02, 0x00, 0x00, 0x00, 0x00]); message.writeInt32LE(normalizeAngle(angle), 5); @@ -97,6 +96,82 @@ export class AbsoluteMotor extends TachoMotor { }); } + public async calibrateServo () { + const oldMode = this.mode; + let currentAngle = 0; + const listener = ({ angle }: { angle: number }) => { + currentAngle = angle; + }; + this.on("absolute", listener); + await this.resetAngle(0); + await this.stop(); + this.gotoAngle(0, 50); + await this.hub.sleep(600); + await this.stop(); + await this.hub.sleep(500); + const absPositionAt0 = currentAngle; + this.gotoAngle(-160, 60); + await this.hub.sleep(600); + await this.stop(); + await this.hub.sleep(500); + const absPositionAtMin160 = currentAngle; + this.gotoAngle(160, 60); + await this.hub.sleep(600); + await this.stop(); + await this.hub.sleep(500); + const absPositionAt160 = currentAngle; + const midPoint1 = normalizeAngle((absPositionAtMin160 + absPositionAt160) / 2); + const midPoint2 = normalizeAngle(midPoint1 + 180); + const baseAngle = (Math.abs(normalizeAngle(midPoint1 - absPositionAt0)) < Math.abs(normalizeAngle(midPoint2 - absPositionAt0))) ? + roundAngleToNearest90(midPoint1) : + roundAngleToNearest90(midPoint2); + const resetToAngle = normalizeAngle(currentAngle - baseAngle); + await this.resetAngle(0); + await this.stop(); + this.gotoAngle(0, 40); + await this.hub.sleep(50); + await this.stop(); + await this.resetAngle(resetToAngle); + this.gotoAngle(0, 40); + await this.hub.sleep(600); + await this.stop(); + this.removeListener("absolute", listener); + if (oldMode !== undefined) { + this.subscribe(oldMode); + } + } + + public async resetServo (angle: number) { + const oldMode = this.mode; + let currentAngle = 0; + const listener = ({ angle }: { angle: number }) => { + currentAngle = angle; + }; + this.on("absolute", listener); + angle = Math.max(-180, Math.min(179, angle)); + const resetToAngle = normalizeAngle(currentAngle - angle); + await this.resetAngle(0); + await this.stop(); + this.gotoAngle(0, 40); + await this.hub.sleep(50); + await this.stop(); + await this.resetAngle(resetToAngle); + this.gotoAngle(0, 40); + await this.hub.sleep(500); + const diff = Math.abs(normalizeAngle(currentAngle - angle)); + if (diff > 5) { + await this.resetAngle(0); + await this.stop(); + this.gotoAngle(0, 40); + await this.hub.sleep(50); + await this.stop(); + } + this.removeListener("absolute", listener); + if (oldMode !== undefined) { + this.subscribe(oldMode); + } + } + } export enum Mode { diff --git a/src/devices/basicmotor.ts b/src/devices/basicmotor.ts index 922d00f..c72151d 100644 --- a/src/devices/basicmotor.ts +++ b/src/devices/basicmotor.ts @@ -29,7 +29,17 @@ export class BasicMotor extends Device { /** - * Fully (hard) stop the motor. + * Stop the motor. + * @method BasicMotor#stop + * @returns {Promise} Resolved upon successful completion of command. + */ + public stop () { + return this.setPower(0); + } + + + /** + * Brake the motor. * @method BasicMotor#brake * @returns {Promise} Resolved upon successful completion of command. */ diff --git a/src/devices/device.ts b/src/devices/device.ts index 2897abd..fc34d28 100644 --- a/src/devices/device.ts +++ b/src/devices/device.ts @@ -36,7 +36,6 @@ export class Device extends EventEmitter { } if (this.autoSubscribe) { if (this._modeMap[event] !== undefined) { - console.log(this._modeMap[event]); this.subscribe(this._modeMap[event]); } } diff --git a/src/devices/tachomotor.ts b/src/devices/tachomotor.ts index ffceeab..f7b192d 100644 --- a/src/devices/tachomotor.ts +++ b/src/devices/tachomotor.ts @@ -48,16 +48,16 @@ export class TachoMotor extends BasicMotor { let message; if (time !== undefined) { if (speed instanceof Array) { - message = Buffer.from([0x81, this.portId, 0x11, 0x0a, 0x00, 0x00, mapSpeed(speed[0]), mapSpeed(speed[1]), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x0a, 0x00, 0x00, mapSpeed(speed[0]), mapSpeed(speed[1]), 0x64, 0x7e, 0x00]); } else { - message = Buffer.from([0x81, this.portId, 0x11, 0x09, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x09, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7e, 0x00]); } message.writeUInt16LE(time, 4); } else { if (speed instanceof Array) { - message = Buffer.from([0x81, this.portId, 0x11, 0x08, mapSpeed(speed[0]), mapSpeed(speed[1]), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x08, mapSpeed(speed[0]), mapSpeed(speed[1]), 0x64, 0x7e, 0x00]); } else { - message = Buffer.from([0x81, this.portId, 0x11, 0x07, mapSpeed(speed), 0x64, 0x03, 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x07, mapSpeed(speed), 0x64, 0x03, 0x64, 0x7e, 0x00]); } } this.send(message); @@ -88,9 +88,9 @@ export class TachoMotor extends BasicMotor { } let message; if (speed instanceof Array) { - message = Buffer.from([0x81, this.portId, 0x11, 0x0c, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed[0]), mapSpeed(speed[1]), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x0c, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed[0]), mapSpeed(speed[1]), 0x64, 0x7e, 0x00]); } else { - message = Buffer.from([0x81, this.portId, 0x11, 0x0b, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7f, 0x03]); + message = Buffer.from([0x81, this.portId, 0x11, 0x0b, 0x00, 0x00, 0x00, 0x00, mapSpeed(speed), 0x64, 0x7e, 0x00]); } message.writeUInt32LE(degrees, 4); this.send(message); diff --git a/src/interfaces.ts b/src/interfaces.ts index 3b399f1..5390575 100644 --- a/src/interfaces.ts +++ b/src/interfaces.ts @@ -22,4 +22,5 @@ export interface IDeviceInterface extends EventEmitter { send: (message: Buffer, uuid: string, callback?: () => void) => void; subscribe: (portId: number, deviceType: number, mode: number) => void; isPortVirtual: (portId: number) => boolean; + sleep: (delay: number) => Promise; } diff --git a/src/utils.ts b/src/utils.ts index 4e330a4..590d998 100644 --- a/src/utils.ts +++ b/src/utils.ts @@ -38,3 +38,20 @@ export const normalizeAngle = (angle: number) => { } return angle; }; + +export const roundAngleToNearest90 = (angle: number) => { + angle = normalizeAngle(angle); + if (angle < -135) { + return -180; + } + if (angle < -45) { + return -90; + } + if (angle < 45) { + return 0; + } + if (angle < 135) { + return 90; + } + return -180; +};