@robotical/ricjs
Version:
Javascript/TS library for Robotical RIC
170 lines (169 loc) • 7.56 kB
JavaScript
;
/**
* The RICServoFaultDetection class is responsible for detecting faults in the RIC servos.
* These are the faults that can currently be detected:
1) Wiring fault - Intermittent connection to servo potentiometer
2) Wiring fault - no connection to servo potentiometer
3) Wiring fault - faulty connection to servo drive
4) Servo horn has shifted out of position
* To detect these faults:
1) get the list of all servos
1.1) We filter out the servos that don't have the fault bit enabled in their status byte
2) sending an atomic read command to the servos
2.1) the servos will respond with a report msg
2.2) the fault flags are on the 11th byte of the report msg
2.3) The 'fault' byte has 8 bits, each of which corresponds to a specific type of fault (there are only 4 types of faults so far, so the remaining 4 bits are unused.)
Bit 0: Intermittent connection to potentiometer
Bit 1: No connection to potentiometer
Bit 2: Faulty connection to motor drive
Bit 3: Servo horn position error
3) in a second phase, we receive the report msg from the app, and check the fault flags
*/
Object.defineProperty(exports, "__esModule", { value: true });
const tslib_1 = require("tslib");
const RICLog_1 = tslib_1.__importDefault(require("./RICLog"));
class HWStatusLastReported {
constructor() {
this.lastReported = null;
this.lastReportedTime = 0;
}
static getInstance() {
if (!HWStatusLastReported.instance) {
HWStatusLastReported.instance = new HWStatusLastReported();
}
return HWStatusLastReported.instance;
}
setLastReported(hwStatus) {
// only set the last reported if it is not a string
if (typeof hwStatus === "string") {
return;
}
this.lastReported = hwStatus;
this.lastReportedTime = Date.now();
}
getLastReported() {
if (Date.now() - this.lastReportedTime > HWStatusLastReported.MAX_TIME) {
return null;
}
return this.lastReported;
}
}
HWStatusLastReported.MAX_TIME = 30000; // 30 seconds
class RICServoFaultDetector {
constructor(ricMsgHandler, ricStateReference) {
this._ricMsgHandler = ricMsgHandler;
this.ricStateReference = ricStateReference;
}
async getAllServos() {
RICServoFaultDetector._servoList = [];
const cachedHwstatus = HWStatusLastReported.getInstance().getLastReported();
let response;
if (cachedHwstatus) {
response = cachedHwstatus;
}
else {
response = await this._ricMsgHandler.sendRICRESTURL("hwstatus/minstat?filterByType=SmartServo");
if (!response || !response.hw) {
RICLog_1.default.warn("RICServoFaultDetector: Error getting servo list");
return;
}
HWStatusLastReported.getInstance().setLastReported(response);
}
const servosWithIdAndName = response.hw.map((smartServo) => ({ id: smartServo.I, name: smartServo.n }));
// filter only the servos that they have enabled the fault bit in their status byte
const servosWithFaultBitEnabled = servosWithIdAndName.filter((smartServo) => {
const foundSmartServoStatus = this.ricStateReference.smartServos.smartServos.find((smartServoStat) => smartServoStat.id === +smartServo.id);
if (!foundSmartServoStatus) {
return false;
}
return RICServoFaultDetector.isFaultBitEnabled(foundSmartServoStatus.status);
});
const filteredServoArrayWithNames = servosWithFaultBitEnabled.map((smartServo) => smartServo.name);
try {
RICServoFaultDetector._servoList = filteredServoArrayWithNames;
}
catch (e) {
console.log("Error getting servo list");
}
}
async atomicReadOperation(expirationTime = 5000) {
await this.getAllServos();
// setting an expiration date within which the interpreter should receive the report msg
RICServoFaultDetector.expirationDate = new Date(Date.now() + expirationTime);
for (let i = 0; i < RICServoFaultDetector._servoList.length; i++) {
const servoName = RICServoFaultDetector._servoList[i];
try {
await this._ricMsgHandler.sendRICRESTURL(`elem/${servoName}/json?cmd=raw&numToRd=11&msgKey=100${i}`);
}
catch (e) {
RICLog_1.default.warn(`RICServoFaultDetector: Error sending atomic read command to servo ${servoName}`);
}
}
}
static interpretReportMsg(reportMsg) {
// make sure the report msg is not expired
if (new Date() > RICServoFaultDetector.expirationDate) {
RICLog_1.default.warn(`RICServoFaultDetector: Received report msg after expiration date: ${reportMsg}`);
return;
}
// make sure this is a raw report msg
if (reportMsg.msgType !== "raw") {
RICLog_1.default.warn(`RICServoFaultDetector: Received non-raw report msg: ${reportMsg}`);
return;
}
// making sure the report msg is from a servo
if (reportMsg.elemName && !RICServoFaultDetector._servoList.includes(reportMsg.elemName)) {
RICLog_1.default.warn(`RICServoFaultDetector: Received report msg from non-servo: ${reportMsg}`);
return;
}
const hexRd = reportMsg.hexRd;
if (!hexRd) {
RICLog_1.default.warn(`RICServoFaultDetector: Received report msg with no hexRd: ${reportMsg}`);
return;
}
const faultByteHex = hexRd.slice(20, 22); // the fault byte is the 11th byte of the report msg
if (!faultByteHex || hexRd.length !== 22) { // since we are reading 11 bytes, the hexRd should be 22 characters long
RICLog_1.default.warn(`RICServoFaultDetector: Received report msg with invalid hexRd: ${reportMsg}`);
return;
}
try {
return RICServoFaultDetector.decodeFault(faultByteHex);
}
catch (e) {
RICLog_1.default.warn(`RICServoFaultDetector: Received report msg with invalid fault byte: ${reportMsg}`);
return;
}
}
static decodeFault(faultByteHex) {
const byte = parseInt(faultByteHex, 16);
return {
intermittentConnection: !!(byte & 0b0001),
noConnection: !!(byte & 0b0010),
faultyConnection: !!(byte & 0b0100),
servoHornPositionError: !!(byte & 0b1000)
};
}
static isFaultBitEnabled(input) {
let num;
// If input is a hexadecimal string, convert it to a decimal number
if (typeof input === "string") {
if (!input.startsWith("0x")) {
throw new Error("Input string must start with '0x' for hexadecimal representation");
}
num = parseInt(input, 16);
}
else {
num = input;
}
// Check if number is an 8-bit number
if (num < 0 || num > 255 || !Number.isInteger(num)) {
throw new Error("Input is not an 8-bit number");
}
// Check if the 6th bit (from the right) is enabled
return Boolean(num & 64);
}
}
exports.default = RICServoFaultDetector;
RICServoFaultDetector.expirationDate = new Date();
RICServoFaultDetector._servoList = [];
//# sourceMappingURL=RICServoFaultDetector.js.map