Spaces:
Running
Running
File size: 4,896 Bytes
bdc1ac8 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 |
/**
* Motor Calibration Utilities
* Specialized functions for motor calibration procedures
*/
import { STS3215_PROTOCOL } from "./sts3215-protocol.js";
import { encodeSignMagnitude } from "./sign-magnitude.js";
import {
readAllMotorPositions,
writeMotorRegister,
type MotorCommunicationPort,
} from "./motor-communication.js";
/**
* Reset homing offsets to 0 for all motors
*/
export async function resetHomingOffsets(
port: MotorCommunicationPort,
motorIds: number[]
): Promise<void> {
for (let i = 0; i < motorIds.length; i++) {
const motorId = motorIds[i];
try {
const packet = new Uint8Array([
0xff,
0xff,
motorId,
0x05,
0x03,
STS3215_PROTOCOL.HOMING_OFFSET_ADDRESS,
0x00, // Low byte of 0
0x00, // High byte of 0
0x00, // Checksum
]);
const checksum =
~(
motorId +
0x05 +
0x03 +
STS3215_PROTOCOL.HOMING_OFFSET_ADDRESS +
0x00 +
0x00
) & 0xff;
packet[8] = checksum;
await port.write(packet);
try {
await port.read(200);
} catch (error) {
// Silent - response not required
}
} catch (error) {
throw new Error(`Failed to reset homing offset for motor ${motorId}`);
}
}
}
/**
* Write homing offsets to motor registers immediately
*/
export async function writeHomingOffsetsToMotors(
port: MotorCommunicationPort,
motorIds: number[],
motorNames: string[],
homingOffsets: { [motor: string]: number }
): Promise<void> {
for (let i = 0; i < motorIds.length; i++) {
const motorId = motorIds[i];
const motorName = motorNames[i];
const homingOffset = homingOffsets[motorName];
try {
const encodedOffset = encodeSignMagnitude(
homingOffset,
STS3215_PROTOCOL.SIGN_MAGNITUDE_BIT
);
const packet = new Uint8Array([
0xff,
0xff,
motorId,
0x05,
0x03,
STS3215_PROTOCOL.HOMING_OFFSET_ADDRESS,
encodedOffset & 0xff,
(encodedOffset >> 8) & 0xff,
0x00,
]);
const checksum =
~(
motorId +
0x05 +
0x03 +
STS3215_PROTOCOL.HOMING_OFFSET_ADDRESS +
(encodedOffset & 0xff) +
((encodedOffset >> 8) & 0xff)
) & 0xff;
packet[8] = checksum;
await port.write(packet);
try {
await port.read(200);
} catch (error) {
// Silent - response not required
}
} catch (error) {
throw new Error(`Failed to write homing offset for ${motorName}`);
}
}
}
/**
* Set homing offsets with immediate writing
*/
export async function setHomingOffsets(
port: MotorCommunicationPort,
motorIds: number[],
motorNames: string[]
): Promise<{ [motor: string]: number }> {
// Reset existing homing offsets to 0 first
await resetHomingOffsets(port, motorIds);
await new Promise((resolve) => setTimeout(resolve, 1000)); // Wait 1 second instead of 100ms
// Flush any cached position readings first
await readAllMotorPositions(port, motorIds); // Dummy read to flush cache
await new Promise((resolve) => setTimeout(resolve, 200)); // Small additional wait
// Read positions (which should now be true physical positions)
const currentPositions = await readAllMotorPositions(port, motorIds);
const homingOffsets: { [motor: string]: number } = {};
const halfTurn = Math.floor((STS3215_PROTOCOL.RESOLUTION - 1) / 2);
for (let i = 0; i < motorNames.length; i++) {
const motorName = motorNames[i];
const position = currentPositions[i];
const calculatedOffset = position - halfTurn;
homingOffsets[motorName] = calculatedOffset;
}
// Write homing offsets to motors immediately
await writeHomingOffsetsToMotors(port, motorIds, motorNames, homingOffsets);
return homingOffsets;
}
/**
* Write hardware position limits to motors
*/
export async function writeHardwarePositionLimits(
port: MotorCommunicationPort,
motorIds: number[],
motorNames: string[],
rangeMins: { [motor: string]: number },
rangeMaxes: { [motor: string]: number }
): Promise<void> {
for (let i = 0; i < motorIds.length; i++) {
const motorId = motorIds[i];
const motorName = motorNames[i];
const minLimit = rangeMins[motorName];
const maxLimit = rangeMaxes[motorName];
try {
// Write Min_Position_Limit register
await writeMotorRegister(
port,
motorId,
STS3215_PROTOCOL.MIN_POSITION_LIMIT_ADDRESS,
minLimit
);
// Write Max_Position_Limit register
await writeMotorRegister(
port,
motorId,
STS3215_PROTOCOL.MAX_POSITION_LIMIT_ADDRESS,
maxLimit
);
} catch (error) {
throw new Error(`Failed to write position limits for ${motorName}`);
}
}
}
|