Servo calibration and reset

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.
+     * (Re)set the knowledge of the absolute position to the current angle.
-     * @method AbsoluteMotor#resetAbsolutePosition
+     * @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 {

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.
      */

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]);
                 }
             }

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);

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<any>;
 }

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;
+};