nkellenicki/node-poweredup
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Servo calibration and reset
Some checks failed
continuous-integration/drone/push Build is failing
Details

Browse Source
This commit is contained in:
Nathan Kellenicki 2020-01-10 12:27:23 -08:00
parent 56eaa7ca49
commit 63f1c10fcc
6 changed files with 120 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
src/devices/absolutemotor.ts
View File

@ -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 {

12
src/devices/basicmotor.ts
View File

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

1
src/devices/device.ts
View File

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

12
src/devices/tachomotor.ts
View File

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

1
src/interfaces.ts
View File

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

17
src/utils.ts
View File

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