@robotical/ricjs-robotical-addons
Version:
JS/TS library for Robotical RIC Addons
271 lines • 13.7 kB
JavaScript
"use strict";
Object.defineProperty(exports, "__esModule", { value: true });
const tslib_1 = require("tslib");
const ricjs_1 = require("@robotical/ricjs");
const RICRoboticalAddOns_1 = require("./RICRoboticalAddOns");
// colour sensor commands
const GET_CALIBRATION_COMMAND = (hwElemName) => `elem/${hwElemName}/json?cmd=raw&hexWr=104020&numToRd=8&msgKey=1234`;
const GET_COLOUR_SENSOR_READING_COMMAND = (hwElemName) => `elem/${hwElemName}/json?cmd=raw&hexWr=&numToRd=5&msgKey=1`;
const CALIBRATE_COMMAND = (hwElemName, baseAddr, highByte, lowByte, i) => `elem/${hwElemName}/json?cmd=raw&hexWr=ff4040${baseAddr.toString(16)}${highByte}${lowByte}&numToRd=0&msgKey=11${i}`;
const MAX_CALIBRATION_ATTEMPTS = 5;
const TOLERANCE = 3; // tolerance for difference between calibration values and colour sensor readings
class ColourSensorManualCalibrator {
static calibrate(ricConnector, retryTimes = 0) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
ricjs_1.RICLog.info(`\n==== Calibration attempt ${retryTimes + 1} ====`);
if (!this.prepareForCalibration(ricConnector)) {
return false;
}
try {
if (yield this.performCalibrationSequence()) {
const isSame = yield this.compareCalibrationValues(this._calibrationValues, this._colourSensorReadings);
return yield this.handleCalibrationOutcome(isSame, ricConnector, retryTimes);
}
return this.handleMaxAttempts(retryTimes, ricConnector);
}
catch (error) {
ricjs_1.RICLog.error("Error calibrating colour sensor: " + JSON.stringify(error));
return false;
}
finally {
this._isCalibrating = false;
}
});
}
static prepareForCalibration(ricConnector) {
if (this._isCalibrating) {
return false;
}
this._RICConnector = ricConnector;
this._isCalibrating = true;
return true;
}
static performCalibrationSequence() {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
const names = yield this.getColourSensorNames();
yield this.turnOnServosIfRequired(names);
const calibration1Success = yield this.getCalibration(names);
const csReadingSuccess = yield this.getColourSensorReading(names);
const calibration2Success = yield this.getCalibration(names);
return calibration1Success && csReadingSuccess && calibration2Success;
});
}
static handleCalibrationOutcome(isSame, ricConnector, retryTimes) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
if (isSame) {
return true;
}
this._isCalibrating = false;
return this.handleMaxAttempts(retryTimes, ricConnector);
});
}
static handleMaxAttempts(retryTimes, ricConnector) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
if (retryTimes >= MAX_CALIBRATION_ATTEMPTS) {
ricjs_1.RICLog.error("Max calibration attempts reached");
return false;
}
this._isCalibrating = false;
return yield this.calibrate(ricConnector, retryTimes + 1);
});
}
static almostEqual(v1, v2, tolerance = TOLERANCE) {
return Math.abs(v1 - v2) <= tolerance;
}
static compareCalibrationValues(expected, actual) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
ricjs_1.RICLog.info("\n==== Comparing calibration values ====");
ricjs_1.RICLog.info("CalibrationValues: " + JSON.stringify(this._calibrationValues));
ricjs_1.RICLog.info("ColourSensorReadings: " + JSON.stringify(this._colourSensorReadings));
if (expected === null || actual === null) {
return false;
}
const expectedKeys = Object.keys(expected);
const actualKeys = Object.keys(actual);
if (expectedKeys.length !== actualKeys.length) {
return false;
}
for (const key of expectedKeys) {
if (!actualKeys.includes(key)) {
return false;
}
const expectedValue = expected[key];
const actualValue = actual[key];
if (!this.almostEqual(expectedValue.clear, actualValue.clear)) {
return false;
}
if (!this.almostEqual(expectedValue.red, actualValue.red)) {
return false;
}
if (!this.almostEqual(expectedValue.green, actualValue.green)) {
return false;
}
if (!this.almostEqual(expectedValue.blue, actualValue.blue)) {
return false;
}
}
return true;
});
}
static turnOnServosIfRequired(names) {
var _a;
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
if (names.length === 0)
return;
for (const name of names) {
const hwElemList = yield this._RICConnector.getRICMsgHandler()
.sendRICRESTURL(`hwstatus/status/${name}`);
if ((hwElemList === null || hwElemList === void 0 ? void 0 : hwElemList.rslt) === "ok" && ((_a = hwElemList.hw) === null || _a === void 0 ? void 0 : _a.length) > 0) {
const hwElem = hwElemList.hw[0];
if (!hwElem.commsOk) {
yield this._RICConnector.getRICMsgHandler().sendRICRESTURL("pwrctrl/5von");
yield new Promise((resolve) => setTimeout(resolve, 4000));
return;
}
}
yield new Promise((resolve) => setTimeout(resolve, 200));
}
});
}
static getColourSensorNames() {
var _a;
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
ricjs_1.RICLog.info("\n==== getColourSensorNames ====");
const names = [];
try {
const hwElemList_Str = yield this._RICConnector.getRICMsgHandler()
.sendRICRESTURL(`hwstatus/strstat/?filterByType=${RICRoboticalAddOns_1.RIC_WHOAMI_TYPE_CODE_ADDON_COLOUR}`);
const hwElems = hwElemList_Str.hw;
let hwElemList;
if (hwElems.length) {
if (typeof hwElems[0] !== "object") {
// we are on a fw version that doesn't supports strstat
hwElemList = yield this._RICConnector.getRICMsgHandler().sendRICRESTURL(`hwstatus?filterByType=${RICRoboticalAddOns_1.RIC_WHOAMI_TYPE_CODE_ADDON_COLOUR}`);
}
else {
// we are on the fw version that supports strstat
hwElemList = ricjs_1.RICHWElemList_Str.expand(hwElemList_Str);
}
}
if ((hwElemList === null || hwElemList === void 0 ? void 0 : hwElemList.rslt) === "ok" && ((_a = hwElemList.hw) === null || _a === void 0 ? void 0 : _a.length) > 0) {
for (const hwElem of hwElemList.hw) {
names.push(hwElem.name);
}
}
ricjs_1.RICLog.info("Colour sensor names: " + JSON.stringify(names));
return names;
}
catch (e) {
new Error("Error getting colour sensor names: " + JSON.stringify(e));
return [];
}
});
}
static getCalibration(names) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
ricjs_1.RICLog.info("\n==== getCalibration ====");
const REPORT_MSG_KEY = "GET_CALIBRATION";
const reports = [];
this._RICConnector.getRICMsgHandler().reportMsgCallbacksSet(REPORT_MSG_KEY, (report) => {
reports.push(report);
ricjs_1.RICLog.debug(`Report callback ${JSON.stringify(report)}`);
});
// Run command for each hardware element
for (const hwElemName of names) {
const command = GET_CALIBRATION_COMMAND(hwElemName);
yield this._RICConnector.getRICSystem().runCommand(command, {});
}
// Wait for report messages to be received
yield new Promise((resolve) => setTimeout(resolve, 2000));
this._RICConnector.getRICMsgHandler().reportMsgCallbacksDelete(REPORT_MSG_KEY);
if (reports.length === 0) {
ricjs_1.RICLog.info("No reports received");
return false;
}
// Process reports
for (const report of reports) {
if (report.hexRd && report.elemName) {
const dataRead = report.hexRd;
const clear = parseInt(dataRead.slice(0, 4), 16);
const red = parseInt(dataRead.slice(4, 8), 16);
const green = parseInt(dataRead.slice(8, 12), 16);
const blue = parseInt(dataRead.slice(12, 16), 16);
ricjs_1.RICLog.info("Calibration Report -- " + "clear: " + clear + " red: " + red + " green: " + green + " blue: " + blue);
this._calibrationValues = Object.assign(Object.assign({}, this._calibrationValues), { [report.elemName]: { clear, red, green, blue } });
}
}
return true;
});
}
static getColourSensorReading(names) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
const REPORT_MSG_KEY = "GET_COLOUR_SENSOR_READING";
ricjs_1.RICLog.info("\n==== getColourSensorReading ====");
const reports = [];
this._RICConnector.getRICMsgHandler()
.reportMsgCallbacksSet(REPORT_MSG_KEY, function (report) {
reports.push(report);
ricjs_1.RICLog.debug(`getHWElemCB Report callback ${JSON.stringify(report)}`);
});
for (const hwElemName of names) {
const command = GET_COLOUR_SENSOR_READING_COMMAND(hwElemName);
yield this._RICConnector.getRICSystem().runCommand(command, {});
}
// wait a couple of seconds for any report messages to be received
yield new Promise((resolve) => setTimeout(resolve, 2000));
// remove report callback
this._RICConnector
.getRICMsgHandler()
.reportMsgCallbacksDelete(REPORT_MSG_KEY);
if (reports.length === 0) {
ricjs_1.RICLog.info("No reports received");
return false;
}
// process reports
for (const report of reports) {
if (report.hexRd && report.elemName) {
const dataRead = report.hexRd;
const clear = parseInt(dataRead.slice(2, 4), 16);
const red = parseInt(dataRead.slice(4, 6), 16);
const green = parseInt(dataRead.slice(6, 8), 16);
const blue = parseInt(dataRead.slice(8, 10), 16);
// RICLog.info("getColourSensorReading", "clear", clear, "red", red, "green", green, "blue", blue);
ricjs_1.RICLog.info("getColourSensorReading -- " + "clear: " + clear + " red: " + red + " green: " + green + " blue: " + blue);
this._colourSensorReadings = Object.assign(Object.assign({}, this._colourSensorReadings), { [report.elemName]: { clear, red, green, blue } });
if (!(yield this.calibrateCS(report.elemName, { clear, red, green, blue }))) {
return false;
}
}
}
return true;
});
}
static calibrateCS(name, reading) {
return (0, tslib_1.__awaiter)(this, void 0, void 0, function* () {
ricjs_1.RICLog.info("\n==== calibrateCS ====");
let baseAddr = 0x20;
const channels = [reading.clear, reading.red, reading.green, reading.blue];
for (let i = 0; i < channels.length; i++) {
const channel = channels[i];
ricjs_1.RICLog.info(`Setting 0x${baseAddr.toString(16)} to ${channel}`);
const highByte = (channel >> 8).toString(16).padStart(2, "0");
const lowByte = (channel & 0xff).toString(16).padStart(2, "0");
const command = CALIBRATE_COMMAND(name, baseAddr, highByte, lowByte, i);
const rslt = yield this._RICConnector.getRICSystem().runCommand(command, {});
// RICLog.info("rslt:" + JSON.stringify(rslt));
if (rslt.rslt !== "ok") {
ricjs_1.RICLog.info("Error setting calibration");
return false;
}
baseAddr += 2;
yield new Promise((resolve) => setTimeout(resolve, 1000));
}
return true;
});
}
}
exports.default = ColourSensorManualCalibrator;
ColourSensorManualCalibrator._isCalibrating = false;
ColourSensorManualCalibrator._calibrationValues = null;
ColourSensorManualCalibrator._colourSensorReadings = null;
//# sourceMappingURL=ColourSensorManualCalibrator.js.map