sophia_controller/feetechDefinitions.js

export class ServoMotor {
    constructor(payload) {
        this.CHANNEL = payload[0];
        this.ID = payload[1];

        this.MODEL = getModelType(payload[3], payload[2]); // minor, major

        this.MIN_ANGLE_LIMIT = (payload[4] << 8) | payload[5];
        this.MAX_ANGLE_LIMIT = (payload[6] << 8) | payload[7];
        this.POSITION = (payload[8] << 8) | payload[9];

        this.CW_DEAD_ZONE = payload[10];
        this.CCW_DEAD_ZONE = payload[11];
        this.OFFSET = (payload[12] << 8) | payload[13];
        this.MODE = payload[14];
        this.TORQUE_ENABLE = payload[15];
        this.ACCELERATION = payload[16];

        this.GOAL_POSITION = (payload[17] << 8) | payload[18];
        this.GOAL_TIME = (payload[19] << 8) | payload[20];
        this.GOAL_SPEED = (payload[21] << 8) | payload[22];
        this.LOCK = payload[23];

        const rawSpeed = (payload[24] << 8) | payload[25];
        this.CURRENT_SPEED = rawSpeed > 0x7FFF ? rawSpeed - 0x10000 : rawSpeed;

        this.CURRENT_LOAD = (payload[26] << 8) | payload[27];
        this.TEMPERATURE = payload[28];
        this.MOVING = payload[29];
        this.CURRENT_CURRENT = (payload[30] << 8) | payload[31];
        this.VOLTAGE = payload[32];
    }
}


// Takes Motor object and updates simulated value, returns packet for real device
export function writeData(motor, key) {
    const entry = DataMap[key];
    if (!entry) {
        throw new Error(`Invalid data key: ${key}`);
    }

    let { address, length } = entry;
    const value = motor[key];

    if (value === undefined) {
        throw new Error(`Motor does not contain value for key: ${key}`);
    }

    

    // HANDLE DIFFERENT ADDRESS FOR LOCK ON SMS_STS    
    if (key == 'LOCK') {
        if (motor.MODEL && (motor.MODEL.includes("SMS") || motor.MODEL.includes("STS"))) {       
            address = DataMap.LOCK_SMS_STS.address;
        } else {
            address = DataMap.LOCK.address;
        }
    }

    const packet = [motor.CHANNEL, motor.ID, address, length]; // channel, id, address, value

    if (length === 2) {
        packet.push(value & 0xFF);
        packet.push((value >> 8) & 0xFF);
    } else if (length === 1) {
        packet.push(value);
    } else {
        throw new Error(`Unsupported byte length: ${length}`);
    }

    return new Uint8Array(packet);
}


const DataMap = Object.freeze({
    MODEL: { address: 0x03, length: 2 },
    ID: { address: 0x05, length: 1 },
    BAUD_RATE: { address: 0x06, length: 1 },
    MIN_ANGLE_LIMIT: { address: 0x09, length: 2 },
    MAX_ANGLE_LIMIT: { address: 0x0B, length: 2 },
    CW_DEAD_ZONE: { address: 0x1A, length: 1 },
    CCW_DEAD_ZONE: { address: 0x1B, length: 1 },
    OFFSET: { address: 0x1F, length: 2 },
    MODE: { address: 0x21, length: 1 },
    TORQUE_ENABLE: { address: 0x28, length: 1 },
    ACCELERATION: { address: 0x29, length: 1 },
    GOAL_POSITION: { address: 0x2A, length: 2 },
    GOAL_TIME: { address: 0x2C, length: 2 },
    GOAL_SPEED: { address: 0x2E, length: 2 },
    LOCK: { address: 0x30, length: 1 }, //SCS
    LOCK_SMS_STS: { address: 0x37, length: 1 },//SMS_STS
    POSITION: { address: 0x38, length: 2 },
    CURRENT_SPEED: { address: 0x3A, length: 2 },
    CURRENT_LOAD: { address: 0x3C, length: 2 },
    VOLTAGE: { address: 0x3E, length: 1 },
    TEMPERATURE: { address: 0x3F, length: 1 },
    MOVING: { address: 0x42, length: 1 },
    CURRENT_CURRENT: { address: 0x45, length: 2 }
});



export function getModelType(major, minor) {
    const modelList = new Map([
        [combine(5, 0), "SCSXX"],
        [combine(5, 4), "SCS009"],
        [combine(5, 8), "SCS2332"],
        [combine(5, 12), "SCS45"],
        [combine(5, 15), "SCS15"],
        [combine(5, 16), "SCS315"],
        [combine(5, 25), "SCS115"],
        [combine(5, 35), "SCS215"],
        [combine(5, 40), "SCS40"],
        [combine(5, 60), "SCS6560"],
        [combine(5, 240), "SCDZZ"],
        [combine(6, 0), "SMXX-360M"],
        [combine(6, 3), "SM30-360M"],
        [combine(6, 8), "SM60-360M"],
        [combine(6, 12), "SM80-360M"],
        [combine(6, 16), "SM100-360M"],
        [combine(6, 20), "SM150-360M"],
        [combine(6, 24), "SM85-360M"],
        [combine(6, 26), "SM60-360M"],
        [combine(8, 0), "SM30BL"],
        [combine(8, 1), "SM30BL"],
        [combine(8, 2), "SM30BL"],
        [combine(8, 3), "SM30BL"],
        [combine(8, 4), "SM30BL"],
        [combine(8, 5), "SM30BL"],
        [combine(8, 6), "SM30BL"],
        [combine(8, 7), "SM30BL"],
        [combine(8, 8), "SM30BL"],
        [combine(8, 9), "SM30BL"],
        [combine(8, 10), "SM30BL"],
        [combine(8, 11), "SM30BL"],
        [combine(8, 12), "SM30BL"],
        [combine(8, 13), "SM30BL"],
        [combine(8, 14), "SM30BL"],
        [combine(8, 15), "SM30BL"],
        [combine(8, 16), "SM30BL"],
        [combine(8, 17), "SM30BL"],
        [combine(8, 18), "SM30BL"],
        [combine(8, 19), "SM30BL"],
        [combine(8, 25), "SM29BL(LJ)"],
        [combine(8, 29), "SM29BL(FT)"],
        [combine(8, 30), "SM30BL(FT)"],
        [combine(8, 20), "SM30BL(LJ)"],
        [combine(8, 40), "SM40BLHV"],
        [combine(8, 42), "SM45BLHV"],
        [combine(8, 44), "SM85BLHV"],
        [combine(8, 120), "SM120BLHV"],
        [combine(8, 220), "SM200BLHV"],
        [combine(9, 0), "STSXX"],
        [combine(9, 2), "STS3032"],
        [combine(9, 3), "STS3215"],
        [combine(9, 4), "STS3040"],
        [combine(9, 5), "STS3020"],
        [combine(9, 6), "STS3046"],
        [combine(9, 20), "SCSXX-2"],
        [combine(9, 15), "SCS15-2"],
        [combine(9, 35), "SCS225"],
        [combine(9, 40), "SCS40-2"]
    ]);

    const id = combine(major, minor);
    return modelList.get(id) || "Unknown Model";
}

function combine(major, minor) {
    return (minor << 8) | major;
}