✂️ Removes mock server
This commit is contained in:
@@ -1 +0,0 @@
|
||||
/node_modules
|
||||
@@ -1,397 +0,0 @@
|
||||
export default class Kinematic {
|
||||
l1;
|
||||
l2;
|
||||
l3;
|
||||
l4;
|
||||
|
||||
L;
|
||||
W;
|
||||
|
||||
constructor() {
|
||||
this.l1 = 50;
|
||||
this.l2 = 20;
|
||||
this.l3 = 120;
|
||||
this.l4 = 155;
|
||||
|
||||
this.L = 140;
|
||||
this.W = 75;
|
||||
}
|
||||
|
||||
bodyIK(omega, phi, psi, xm, ym, zm) {
|
||||
const { cos, sin } = Math;
|
||||
|
||||
const Rx = [
|
||||
[1, 0, 0, 0],
|
||||
[0, cos(omega), -sin(omega), 0],
|
||||
[0, sin(omega), cos(omega), 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
const Ry = [
|
||||
[cos(phi), 0, sin(phi), 0],
|
||||
[0, 1, 0, 0],
|
||||
[-sin(phi), 0, cos(phi), 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
const Rz = [
|
||||
[cos(psi), -sin(psi), 0, 0],
|
||||
[sin(psi), cos(psi), 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
const Rxyz = this.matrixMultiply(this.matrixMultiply(Rx, Ry), Rz);
|
||||
|
||||
const T = [
|
||||
[0, 0, 0, xm],
|
||||
[0, 0, 0, ym],
|
||||
[0, 0, 0, zm],
|
||||
[0, 0, 0, 0],
|
||||
];
|
||||
const Tm = this.matrixAdd(T, Rxyz);
|
||||
|
||||
const sHp = sin(Math.PI / 2);
|
||||
const cHp = cos(Math.PI / 2);
|
||||
|
||||
return [
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, this.L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, this.W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, this.L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, -this.W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, -this.L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, this.W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, -this.L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, -this.W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
];
|
||||
}
|
||||
|
||||
legIK(point) {
|
||||
const [x, y, z] = point;
|
||||
const { atan2, cos, sin, sqrt, acos } = Math;
|
||||
|
||||
let F;
|
||||
|
||||
try {
|
||||
F = sqrt(x ** 2 + y ** 2 - this.l1 ** 2);
|
||||
if (isNaN(F)) throw new Error("F is NaN");
|
||||
} catch (error) {
|
||||
F = this.l1;
|
||||
}
|
||||
const G = F - this.l2;
|
||||
const H = sqrt(G ** 2 + z ** 2);
|
||||
|
||||
const theta1 = -atan2(y, x) - atan2(F, -this.l1);
|
||||
let theta3;
|
||||
try {
|
||||
theta3 = acos(
|
||||
(H ** 2 - this.l3 ** 2 - this.l4 ** 2) / (2 * this.l3 * this.l4)
|
||||
);
|
||||
if (isNaN(theta3)) throw new Error("theta3 is NaN");
|
||||
} catch (error) {
|
||||
theta3 = 0;
|
||||
}
|
||||
const theta2 =
|
||||
atan2(z, G) -
|
||||
atan2(this.l4 * sin(theta3), this.l3 + this.l4 * cos(theta3));
|
||||
|
||||
return [theta1, theta2, theta3];
|
||||
}
|
||||
|
||||
matrixMultiply(a, b) {
|
||||
const result = [];
|
||||
|
||||
for (let i = 0; i < a.length; i++) {
|
||||
const row = [];
|
||||
|
||||
for (let j = 0; j < b[0].length; j++) {
|
||||
let sum = 0;
|
||||
|
||||
for (let k = 0; k < a[i].length; k++) {
|
||||
sum += a[i][k] * b[k][j];
|
||||
}
|
||||
|
||||
row.push(sum);
|
||||
}
|
||||
|
||||
result.push(row);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
multiplyVector(matrix, vector) {
|
||||
const rows = matrix.length;
|
||||
const cols = matrix[0].length;
|
||||
const vectorLength = vector.length;
|
||||
|
||||
if (cols !== vectorLength) {
|
||||
throw new Error(
|
||||
"Matrix and vector dimensions do not match for multiplication."
|
||||
);
|
||||
}
|
||||
|
||||
const result = [];
|
||||
|
||||
for (let i = 0; i < rows; i++) {
|
||||
let sum = 0;
|
||||
|
||||
for (let j = 0; j < cols; j++) {
|
||||
sum += matrix[i][j] * vector[j];
|
||||
}
|
||||
|
||||
result.push(sum);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
matrixAdd(a, b) {
|
||||
const result = [];
|
||||
|
||||
for (let i = 0; i < a.length; i++) {
|
||||
const row = [];
|
||||
|
||||
for (let j = 0; j < a[i].length; j++) {
|
||||
row.push(a[i][j] + b[i][j]);
|
||||
}
|
||||
|
||||
result.push(row);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
calcLegPoints(angles) {
|
||||
const [theta1, theta2, theta3] = angles;
|
||||
const theta23 = theta2 + theta3;
|
||||
|
||||
const T0 = [0, 0, 0, 1];
|
||||
const T1 = this.vectorAdd(T0, [
|
||||
-this.l1 * Math.cos(theta1),
|
||||
this.l1 * Math.sin(theta1),
|
||||
0,
|
||||
0,
|
||||
]);
|
||||
const T2 = this.vectorAdd(T1, [
|
||||
-this.l2 * Math.sin(theta1),
|
||||
-this.l2 * Math.cos(theta1),
|
||||
0,
|
||||
0,
|
||||
]);
|
||||
const T3 = this.vectorAdd(T2, [
|
||||
-this.l3 * Math.sin(theta1) * Math.cos(theta2),
|
||||
-this.l3 * Math.cos(theta1) * Math.cos(theta2),
|
||||
this.l3 * Math.sin(theta2),
|
||||
0,
|
||||
]);
|
||||
const T4 = this.vectorAdd(T3, [
|
||||
-this.l4 * Math.sin(theta1) * Math.cos(theta23),
|
||||
-this.l4 * Math.cos(theta1) * Math.cos(theta23),
|
||||
this.l4 * Math.sin(theta23),
|
||||
0,
|
||||
]);
|
||||
|
||||
return [T0, T1, T2, T3, T4];
|
||||
}
|
||||
|
||||
calcIK(Lp, angles, center) {
|
||||
const [omega, phi, psi] = angles;
|
||||
const [xm, ym, zm] = center;
|
||||
|
||||
const [Tlf, Trf, Tlb, Trb] = this.bodyIK(omega, phi, psi, xm, ym, zm);
|
||||
|
||||
const Ix = [
|
||||
[-1, 0, 0, 0],
|
||||
[0, 1, 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
|
||||
return [
|
||||
this.legIK(this.multiplyVector(this.matrixInverse(Tlf), Lp[0])),
|
||||
this.legIK(
|
||||
this.multiplyVector(
|
||||
Ix,
|
||||
this.multiplyVector(this.matrixInverse(Trf), Lp[1])
|
||||
)
|
||||
),
|
||||
this.legIK(this.multiplyVector(this.matrixInverse(Tlb), Lp[2])),
|
||||
this.legIK(
|
||||
this.multiplyVector(
|
||||
Ix,
|
||||
this.multiplyVector(this.matrixInverse(Trb), Lp[3])
|
||||
)
|
||||
),
|
||||
];
|
||||
}
|
||||
|
||||
vectorAdd(a, b) {
|
||||
return a.map((val, index) => val + b[index]);
|
||||
}
|
||||
|
||||
matrixInverse(matrix) {
|
||||
const det = this.determinant(matrix);
|
||||
const adjugate = this.adjugate(matrix);
|
||||
const scalar = 1 / det;
|
||||
const inverse = [];
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const row = [];
|
||||
|
||||
for (let j = 0; j < matrix[i].length; j++) {
|
||||
row.push(adjugate[i][j] * scalar);
|
||||
}
|
||||
|
||||
inverse.push(row);
|
||||
}
|
||||
|
||||
return inverse;
|
||||
}
|
||||
|
||||
determinant(matrix) {
|
||||
if (matrix.length !== matrix[0].length) {
|
||||
throw new Error("The matrix is not square.");
|
||||
}
|
||||
|
||||
if (matrix.length === 2) {
|
||||
return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
|
||||
}
|
||||
|
||||
let det = 0;
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const sign = i % 2 === 0 ? 1 : -1;
|
||||
const subMatrix = [];
|
||||
|
||||
for (let j = 1; j < matrix.length; j++) {
|
||||
const row = [];
|
||||
|
||||
for (let k = 0; k < matrix.length; k++) {
|
||||
if (k !== i) {
|
||||
row.push(matrix[j][k]);
|
||||
}
|
||||
}
|
||||
|
||||
subMatrix.push(row);
|
||||
}
|
||||
|
||||
det += sign * matrix[0][i] * this.determinant(subMatrix);
|
||||
}
|
||||
|
||||
return det;
|
||||
}
|
||||
|
||||
adjugate(matrix) {
|
||||
if (matrix.length !== matrix[0].length) {
|
||||
throw new Error("The matrix is not square.");
|
||||
}
|
||||
|
||||
const adjugate = [];
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const row = [];
|
||||
|
||||
for (let j = 0; j < matrix[i].length; j++) {
|
||||
const sign = (i + j) % 2 === 0 ? 1 : -1;
|
||||
const subMatrix = [];
|
||||
|
||||
for (let k = 0; k < matrix.length; k++) {
|
||||
if (k !== i) {
|
||||
const subRow = [];
|
||||
|
||||
for (let l = 0; l < matrix.length; l++) {
|
||||
if (l !== j) {
|
||||
subRow.push(matrix[k][l]);
|
||||
}
|
||||
}
|
||||
|
||||
subMatrix.push(subRow);
|
||||
}
|
||||
}
|
||||
|
||||
const cofactor = sign * this.determinant(subMatrix);
|
||||
row.push(cofactor);
|
||||
}
|
||||
|
||||
adjugate.push(row);
|
||||
}
|
||||
|
||||
return this.transpose(adjugate);
|
||||
}
|
||||
|
||||
transpose(matrix) {
|
||||
const transposed = [];
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const row = [];
|
||||
|
||||
for (let j = 0; j < matrix[i].length; j++) {
|
||||
row.push(matrix[j][i]);
|
||||
}
|
||||
|
||||
transposed.push(row);
|
||||
}
|
||||
|
||||
return transposed;
|
||||
}
|
||||
}
|
||||
|
||||
class ForwardKinematics {
|
||||
l1;
|
||||
l2;
|
||||
l3;
|
||||
l4;
|
||||
|
||||
constructor() {
|
||||
this.l1 = 50;
|
||||
this.l2 = 20;
|
||||
this.l3 = 120;
|
||||
this.l4 = 155;
|
||||
}
|
||||
|
||||
calculateFootpoint(theta1, theta2, theta3) {
|
||||
const { cos, sin } = Math;
|
||||
|
||||
const x =
|
||||
this.l1 * cos(theta1) +
|
||||
this.l2 * cos(theta1) +
|
||||
this.l3 * cos(theta1 + theta2) +
|
||||
this.l4 * cos(theta1 + theta2 + theta3);
|
||||
const y =
|
||||
this.l1 * sin(theta1) +
|
||||
this.l2 * sin(theta1) +
|
||||
this.l3 * sin(theta1 + theta2) +
|
||||
this.l4 * sin(theta1 + theta2 + theta3);
|
||||
const z = 0;
|
||||
|
||||
return [x, y, z];
|
||||
}
|
||||
|
||||
calculateFootpoints(angles) {
|
||||
const footpoints = [];
|
||||
|
||||
for (let i = 0; i < angles.length; i += 3) {
|
||||
const theta1 = angles[i];
|
||||
const theta2 = angles[i + 1];
|
||||
const theta3 = angles[i + 2];
|
||||
const footpoint = this.calculateFootpoint(theta1, theta2, theta3);
|
||||
footpoints.push(footpoint);
|
||||
}
|
||||
|
||||
return footpoints;
|
||||
}
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
{
|
||||
"name": "mock",
|
||||
"version": "1.0.0",
|
||||
"description": "",
|
||||
"main": "index.js",
|
||||
"scripts": {
|
||||
"test": "echo \"Error: no test specified\" && exit 1"
|
||||
},
|
||||
"keywords": [],
|
||||
"author": "",
|
||||
"license": "ISC",
|
||||
"type": "module",
|
||||
"dependencies": {
|
||||
"cors": "^2.8.5",
|
||||
"express": "^4.18.2",
|
||||
"ws": "^8.16.0"
|
||||
}
|
||||
}
|
||||
Generated
-483
@@ -1,483 +0,0 @@
|
||||
lockfileVersion: '6.0'
|
||||
|
||||
settings:
|
||||
autoInstallPeers: true
|
||||
excludeLinksFromLockfile: false
|
||||
|
||||
dependencies:
|
||||
cors:
|
||||
specifier: ^2.8.5
|
||||
version: 2.8.5
|
||||
express:
|
||||
specifier: ^4.18.2
|
||||
version: 4.18.2
|
||||
ws:
|
||||
specifier: ^8.16.0
|
||||
version: 8.16.0
|
||||
|
||||
packages:
|
||||
|
||||
/accepts@1.3.8:
|
||||
resolution: {integrity: sha512-PYAthTa2m2VKxuvSD3DPC/Gy+U+sOA1LAuT8mkmRuvw+NACSaeXEQ+NHcVF7rONl6qcaxV3Uuemwawk+7+SJLw==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dependencies:
|
||||
mime-types: 2.1.35
|
||||
negotiator: 0.6.3
|
||||
dev: false
|
||||
|
||||
/array-flatten@1.1.1:
|
||||
resolution: {integrity: sha512-PCVAQswWemu6UdxsDFFX/+gVeYqKAod3D3UVm91jHwynguOwAvYPhx8nNlM++NqRcK6CxxpUafjmhIdKiHibqg==}
|
||||
dev: false
|
||||
|
||||
/body-parser@1.20.1:
|
||||
resolution: {integrity: sha512-jWi7abTbYwajOytWCQc37VulmWiRae5RyTpaCyDcS5/lMdtwSz5lOpDE67srw/HYe35f1z3fDQw+3txg7gNtWw==}
|
||||
engines: {node: '>= 0.8', npm: 1.2.8000 || >= 1.4.16}
|
||||
dependencies:
|
||||
bytes: 3.1.2
|
||||
content-type: 1.0.5
|
||||
debug: 2.6.9
|
||||
depd: 2.0.0
|
||||
destroy: 1.2.0
|
||||
http-errors: 2.0.0
|
||||
iconv-lite: 0.4.24
|
||||
on-finished: 2.4.1
|
||||
qs: 6.11.0
|
||||
raw-body: 2.5.1
|
||||
type-is: 1.6.18
|
||||
unpipe: 1.0.0
|
||||
transitivePeerDependencies:
|
||||
- supports-color
|
||||
dev: false
|
||||
|
||||
/bytes@3.1.2:
|
||||
resolution: {integrity: sha512-/Nf7TyzTx6S3yRJObOAV7956r8cr2+Oj8AC5dt8wSP3BQAoeX58NoHyCU8P8zGkNXStjTSi6fzO6F0pBdcYbEg==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/call-bind@1.0.5:
|
||||
resolution: {integrity: sha512-C3nQxfFZxFRVoJoGKKI8y3MOEo129NQ+FgQ08iye+Mk4zNZZGdjfs06bVTr+DBSlA66Q2VEcMki/cUCP4SercQ==}
|
||||
dependencies:
|
||||
function-bind: 1.1.2
|
||||
get-intrinsic: 1.2.2
|
||||
set-function-length: 1.2.0
|
||||
dev: false
|
||||
|
||||
/content-disposition@0.5.4:
|
||||
resolution: {integrity: sha512-FveZTNuGw04cxlAiWbzi6zTAL/lhehaWbTtgluJh4/E95DqMwTmha3KZN1aAWA8cFIhHzMZUvLevkw5Rqk+tSQ==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dependencies:
|
||||
safe-buffer: 5.2.1
|
||||
dev: false
|
||||
|
||||
/content-type@1.0.5:
|
||||
resolution: {integrity: sha512-nTjqfcBFEipKdXCv4YDQWCfmcLZKm81ldF0pAopTvyrFGVbcR6P/VAAd5G7N+0tTr8QqiU0tFadD6FK4NtJwOA==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/cookie-signature@1.0.6:
|
||||
resolution: {integrity: sha512-QADzlaHc8icV8I7vbaJXJwod9HWYp8uCqf1xa4OfNu1T7JVxQIrUgOWtHdNDtPiywmFbiS12VjotIXLrKM3orQ==}
|
||||
dev: false
|
||||
|
||||
/cookie@0.5.0:
|
||||
resolution: {integrity: sha512-YZ3GUyn/o8gfKJlnlX7g7xq4gyO6OSuhGPKaaGssGB2qgDUS0gPgtTvoyZLTt9Ab6dC4hfc9dV5arkvc/OCmrw==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/cors@2.8.5:
|
||||
resolution: {integrity: sha512-KIHbLJqu73RGr/hnbrO9uBeixNGuvSQjul/jdFvS/KFSIH1hWVd1ng7zOHx+YrEfInLG7q4n6GHQ9cDtxv/P6g==}
|
||||
engines: {node: '>= 0.10'}
|
||||
dependencies:
|
||||
object-assign: 4.1.1
|
||||
vary: 1.1.2
|
||||
dev: false
|
||||
|
||||
/debug@2.6.9:
|
||||
resolution: {integrity: sha512-bC7ElrdJaJnPbAP+1EotYvqZsb3ecl5wi6Bfi6BJTUcNowp6cvspg0jXznRTKDjm/E7AdgFBVeAPVMNcKGsHMA==}
|
||||
peerDependencies:
|
||||
supports-color: '*'
|
||||
peerDependenciesMeta:
|
||||
supports-color:
|
||||
optional: true
|
||||
dependencies:
|
||||
ms: 2.0.0
|
||||
dev: false
|
||||
|
||||
/define-data-property@1.1.1:
|
||||
resolution: {integrity: sha512-E7uGkTzkk1d0ByLeSc6ZsFS79Axg+m1P/VsgYsxHgiuc3tFSj+MjMIwe90FC4lOAZzNBdY7kkO2P2wKdsQ1vgQ==}
|
||||
engines: {node: '>= 0.4'}
|
||||
dependencies:
|
||||
get-intrinsic: 1.2.2
|
||||
gopd: 1.0.1
|
||||
has-property-descriptors: 1.0.1
|
||||
dev: false
|
||||
|
||||
/depd@2.0.0:
|
||||
resolution: {integrity: sha512-g7nH6P6dyDioJogAAGprGpCtVImJhpPk/roCzdb3fIh61/s/nPsfR6onyMwkCAR/OlC3yBC0lESvUoQEAssIrw==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/destroy@1.2.0:
|
||||
resolution: {integrity: sha512-2sJGJTaXIIaR1w4iJSNoN0hnMY7Gpc/n8D4qSCJw8QqFWXf7cuAgnEHxBpweaVcPevC2l3KpjYCx3NypQQgaJg==}
|
||||
engines: {node: '>= 0.8', npm: 1.2.8000 || >= 1.4.16}
|
||||
dev: false
|
||||
|
||||
/ee-first@1.1.1:
|
||||
resolution: {integrity: sha512-WMwm9LhRUo+WUaRN+vRuETqG89IgZphVSNkdFgeb6sS/E4OrDIN7t48CAewSHXc6C8lefD8KKfr5vY61brQlow==}
|
||||
dev: false
|
||||
|
||||
/encodeurl@1.0.2:
|
||||
resolution: {integrity: sha512-TPJXq8JqFaVYm2CWmPvnP2Iyo4ZSM7/QKcSmuMLDObfpH5fi7RUGmd/rTDf+rut/saiDiQEeVTNgAmJEdAOx0w==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/escape-html@1.0.3:
|
||||
resolution: {integrity: sha512-NiSupZ4OeuGwr68lGIeym/ksIZMJodUGOSCZ/FSnTxcrekbvqrgdUxlJOMpijaKZVjAJrWrGs/6Jy8OMuyj9ow==}
|
||||
dev: false
|
||||
|
||||
/etag@1.8.1:
|
||||
resolution: {integrity: sha512-aIL5Fx7mawVa300al2BnEE4iNvo1qETxLrPI/o05L7z6go7fCw1J6EQmbK4FmJ2AS7kgVF/KEZWufBfdClMcPg==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/express@4.18.2:
|
||||
resolution: {integrity: sha512-5/PsL6iGPdfQ/lKM1UuielYgv3BUoJfz1aUwU9vHZ+J7gyvwdQXFEBIEIaxeGf0GIcreATNyBExtalisDbuMqQ==}
|
||||
engines: {node: '>= 0.10.0'}
|
||||
dependencies:
|
||||
accepts: 1.3.8
|
||||
array-flatten: 1.1.1
|
||||
body-parser: 1.20.1
|
||||
content-disposition: 0.5.4
|
||||
content-type: 1.0.5
|
||||
cookie: 0.5.0
|
||||
cookie-signature: 1.0.6
|
||||
debug: 2.6.9
|
||||
depd: 2.0.0
|
||||
encodeurl: 1.0.2
|
||||
escape-html: 1.0.3
|
||||
etag: 1.8.1
|
||||
finalhandler: 1.2.0
|
||||
fresh: 0.5.2
|
||||
http-errors: 2.0.0
|
||||
merge-descriptors: 1.0.1
|
||||
methods: 1.1.2
|
||||
on-finished: 2.4.1
|
||||
parseurl: 1.3.3
|
||||
path-to-regexp: 0.1.7
|
||||
proxy-addr: 2.0.7
|
||||
qs: 6.11.0
|
||||
range-parser: 1.2.1
|
||||
safe-buffer: 5.2.1
|
||||
send: 0.18.0
|
||||
serve-static: 1.15.0
|
||||
setprototypeof: 1.2.0
|
||||
statuses: 2.0.1
|
||||
type-is: 1.6.18
|
||||
utils-merge: 1.0.1
|
||||
vary: 1.1.2
|
||||
transitivePeerDependencies:
|
||||
- supports-color
|
||||
dev: false
|
||||
|
||||
/finalhandler@1.2.0:
|
||||
resolution: {integrity: sha512-5uXcUVftlQMFnWC9qu/svkWv3GTd2PfUhK/3PLkYNAe7FbqJMt3515HaxE6eRL74GdsriiwujiawdaB1BpEISg==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dependencies:
|
||||
debug: 2.6.9
|
||||
encodeurl: 1.0.2
|
||||
escape-html: 1.0.3
|
||||
on-finished: 2.4.1
|
||||
parseurl: 1.3.3
|
||||
statuses: 2.0.1
|
||||
unpipe: 1.0.0
|
||||
transitivePeerDependencies:
|
||||
- supports-color
|
||||
dev: false
|
||||
|
||||
/forwarded@0.2.0:
|
||||
resolution: {integrity: sha512-buRG0fpBtRHSTCOASe6hD258tEubFoRLb4ZNA6NxMVHNw2gOcwHo9wyablzMzOA5z9xA9L1KNjk/Nt6MT9aYow==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/fresh@0.5.2:
|
||||
resolution: {integrity: sha512-zJ2mQYM18rEFOudeV4GShTGIQ7RbzA7ozbU9I/XBpm7kqgMywgmylMwXHxZJmkVoYkna9d2pVXVXPdYTP9ej8Q==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/function-bind@1.1.2:
|
||||
resolution: {integrity: sha512-7XHNxH7qX9xG5mIwxkhumTox/MIRNcOgDrxWsMt2pAr23WHp6MrRlN7FBSFpCpr+oVO0F744iUgR82nJMfG2SA==}
|
||||
dev: false
|
||||
|
||||
/get-intrinsic@1.2.2:
|
||||
resolution: {integrity: sha512-0gSo4ml/0j98Y3lngkFEot/zhiCeWsbYIlZ+uZOVgzLyLaUw7wxUL+nCTP0XJvJg1AXulJRI3UJi8GsbDuxdGA==}
|
||||
dependencies:
|
||||
function-bind: 1.1.2
|
||||
has-proto: 1.0.1
|
||||
has-symbols: 1.0.3
|
||||
hasown: 2.0.0
|
||||
dev: false
|
||||
|
||||
/gopd@1.0.1:
|
||||
resolution: {integrity: sha512-d65bNlIadxvpb/A2abVdlqKqV563juRnZ1Wtk6s1sIR8uNsXR70xqIzVqxVf1eTqDunwT2MkczEeaezCKTZhwA==}
|
||||
dependencies:
|
||||
get-intrinsic: 1.2.2
|
||||
dev: false
|
||||
|
||||
/has-property-descriptors@1.0.1:
|
||||
resolution: {integrity: sha512-VsX8eaIewvas0xnvinAe9bw4WfIeODpGYikiWYLH+dma0Jw6KHYqWiWfhQlgOVK8D6PvjubK5Uc4P0iIhIcNVg==}
|
||||
dependencies:
|
||||
get-intrinsic: 1.2.2
|
||||
dev: false
|
||||
|
||||
/has-proto@1.0.1:
|
||||
resolution: {integrity: sha512-7qE+iP+O+bgF9clE5+UoBFzE65mlBiVj3tKCrlNQ0Ogwm0BjpT/gK4SlLYDMybDh5I3TCTKnPPa0oMG7JDYrhg==}
|
||||
engines: {node: '>= 0.4'}
|
||||
dev: false
|
||||
|
||||
/has-symbols@1.0.3:
|
||||
resolution: {integrity: sha512-l3LCuF6MgDNwTDKkdYGEihYjt5pRPbEg46rtlmnSPlUbgmB8LOIrKJbYYFBSbnPaJexMKtiPO8hmeRjRz2Td+A==}
|
||||
engines: {node: '>= 0.4'}
|
||||
dev: false
|
||||
|
||||
/hasown@2.0.0:
|
||||
resolution: {integrity: sha512-vUptKVTpIJhcczKBbgnS+RtcuYMB8+oNzPK2/Hp3hanz8JmpATdmmgLgSaadVREkDm+e2giHwY3ZRkyjSIDDFA==}
|
||||
engines: {node: '>= 0.4'}
|
||||
dependencies:
|
||||
function-bind: 1.1.2
|
||||
dev: false
|
||||
|
||||
/http-errors@2.0.0:
|
||||
resolution: {integrity: sha512-FtwrG/euBzaEjYeRqOgly7G0qviiXoJWnvEH2Z1plBdXgbyjv34pHTSb9zoeHMyDy33+DWy5Wt9Wo+TURtOYSQ==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dependencies:
|
||||
depd: 2.0.0
|
||||
inherits: 2.0.4
|
||||
setprototypeof: 1.2.0
|
||||
statuses: 2.0.1
|
||||
toidentifier: 1.0.1
|
||||
dev: false
|
||||
|
||||
/iconv-lite@0.4.24:
|
||||
resolution: {integrity: sha512-v3MXnZAcvnywkTUEZomIActle7RXXeedOR31wwl7VlyoXO4Qi9arvSenNQWne1TcRwhCL1HwLI21bEqdpj8/rA==}
|
||||
engines: {node: '>=0.10.0'}
|
||||
dependencies:
|
||||
safer-buffer: 2.1.2
|
||||
dev: false
|
||||
|
||||
/inherits@2.0.4:
|
||||
resolution: {integrity: sha512-k/vGaX4/Yla3WzyMCvTQOXYeIHvqOKtnqBduzTHpzpQZzAskKMhZ2K+EnBiSM9zGSoIFeMpXKxa4dYeZIQqewQ==}
|
||||
dev: false
|
||||
|
||||
/ipaddr.js@1.9.1:
|
||||
resolution: {integrity: sha512-0KI/607xoxSToH7GjN1FfSbLoU0+btTicjsQSWQlh/hZykN8KpmMf7uYwPW3R+akZ6R/w18ZlXSHBYXiYUPO3g==}
|
||||
engines: {node: '>= 0.10'}
|
||||
dev: false
|
||||
|
||||
/media-typer@0.3.0:
|
||||
resolution: {integrity: sha512-dq+qelQ9akHpcOl/gUVRTxVIOkAJ1wR3QAvb4RsVjS8oVoFjDGTc679wJYmUmknUF5HwMLOgb5O+a3KxfWapPQ==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/merge-descriptors@1.0.1:
|
||||
resolution: {integrity: sha512-cCi6g3/Zr1iqQi6ySbseM1Xvooa98N0w31jzUYrXPX2xqObmFGHJ0tQ5u74H3mVh7wLouTseZyYIq39g8cNp1w==}
|
||||
dev: false
|
||||
|
||||
/methods@1.1.2:
|
||||
resolution: {integrity: sha512-iclAHeNqNm68zFtnZ0e+1L2yUIdvzNoauKU4WBA3VvH/vPFieF7qfRlwUZU+DA9P9bPXIS90ulxoUoCH23sV2w==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/mime-db@1.52.0:
|
||||
resolution: {integrity: sha512-sPU4uV7dYlvtWJxwwxHD0PuihVNiE7TyAbQ5SWxDCB9mUYvOgroQOwYQQOKPJ8CIbE+1ETVlOoK1UC2nU3gYvg==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/mime-types@2.1.35:
|
||||
resolution: {integrity: sha512-ZDY+bPm5zTTF+YpCrAU9nK0UgICYPT0QtT1NZWFv4s++TNkcgVaT0g6+4R2uI4MjQjzysHB1zxuWL50hzaeXiw==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dependencies:
|
||||
mime-db: 1.52.0
|
||||
dev: false
|
||||
|
||||
/mime@1.6.0:
|
||||
resolution: {integrity: sha512-x0Vn8spI+wuJ1O6S7gnbaQg8Pxh4NNHb7KSINmEWKiPE4RKOplvijn+NkmYmmRgP68mc70j2EbeTFRsrswaQeg==}
|
||||
engines: {node: '>=4'}
|
||||
hasBin: true
|
||||
dev: false
|
||||
|
||||
/ms@2.0.0:
|
||||
resolution: {integrity: sha512-Tpp60P6IUJDTuOq/5Z8cdskzJujfwqfOTkrwIwj7IRISpnkJnT6SyJ4PCPnGMoFjC9ddhal5KVIYtAt97ix05A==}
|
||||
dev: false
|
||||
|
||||
/ms@2.1.3:
|
||||
resolution: {integrity: sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==}
|
||||
dev: false
|
||||
|
||||
/negotiator@0.6.3:
|
||||
resolution: {integrity: sha512-+EUsqGPLsM+j/zdChZjsnX51g4XrHFOIXwfnCVPGlQk/k5giakcKsuxCObBRu6DSm9opw/O6slWbJdghQM4bBg==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/object-assign@4.1.1:
|
||||
resolution: {integrity: sha512-rJgTQnkUnH1sFw8yT6VSU3zD3sWmu6sZhIseY8VX+GRu3P6F7Fu+JNDoXfklElbLJSnc3FUQHVe4cU5hj+BcUg==}
|
||||
engines: {node: '>=0.10.0'}
|
||||
dev: false
|
||||
|
||||
/object-inspect@1.13.1:
|
||||
resolution: {integrity: sha512-5qoj1RUiKOMsCCNLV1CBiPYE10sziTsnmNxkAI/rZhiD63CF7IqdFGC/XzjWjpSgLf0LxXX3bDFIh0E18f6UhQ==}
|
||||
dev: false
|
||||
|
||||
/on-finished@2.4.1:
|
||||
resolution: {integrity: sha512-oVlzkg3ENAhCk2zdv7IJwd/QUD4z2RxRwpkcGY8psCVcCYZNq4wYnVWALHM+brtuJjePWiYF/ClmuDr8Ch5+kg==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dependencies:
|
||||
ee-first: 1.1.1
|
||||
dev: false
|
||||
|
||||
/parseurl@1.3.3:
|
||||
resolution: {integrity: sha512-CiyeOxFT/JZyN5m0z9PfXw4SCBJ6Sygz1Dpl0wqjlhDEGGBP1GnsUVEL0p63hoG1fcj3fHynXi9NYO4nWOL+qQ==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/path-to-regexp@0.1.7:
|
||||
resolution: {integrity: sha512-5DFkuoqlv1uYQKxy8omFBeJPQcdoE07Kv2sferDCrAq1ohOU+MSDswDIbnx3YAM60qIOnYa53wBhXW0EbMonrQ==}
|
||||
dev: false
|
||||
|
||||
/proxy-addr@2.0.7:
|
||||
resolution: {integrity: sha512-llQsMLSUDUPT44jdrU/O37qlnifitDP+ZwrmmZcoSKyLKvtZxpyV0n2/bD/N4tBAAZ/gJEdZU7KMraoK1+XYAg==}
|
||||
engines: {node: '>= 0.10'}
|
||||
dependencies:
|
||||
forwarded: 0.2.0
|
||||
ipaddr.js: 1.9.1
|
||||
dev: false
|
||||
|
||||
/qs@6.11.0:
|
||||
resolution: {integrity: sha512-MvjoMCJwEarSbUYk5O+nmoSzSutSsTwF85zcHPQ9OrlFoZOYIjaqBAJIqIXjptyD5vThxGq52Xu/MaJzRkIk4Q==}
|
||||
engines: {node: '>=0.6'}
|
||||
dependencies:
|
||||
side-channel: 1.0.4
|
||||
dev: false
|
||||
|
||||
/range-parser@1.2.1:
|
||||
resolution: {integrity: sha512-Hrgsx+orqoygnmhFbKaHE6c296J+HTAQXoxEF6gNupROmmGJRoyzfG3ccAveqCBrwr/2yxQ5BVd/GTl5agOwSg==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dev: false
|
||||
|
||||
/raw-body@2.5.1:
|
||||
resolution: {integrity: sha512-qqJBtEyVgS0ZmPGdCFPWJ3FreoqvG4MVQln/kCgF7Olq95IbOp0/BWyMwbdtn4VTvkM8Y7khCQ2Xgk/tcrCXig==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dependencies:
|
||||
bytes: 3.1.2
|
||||
http-errors: 2.0.0
|
||||
iconv-lite: 0.4.24
|
||||
unpipe: 1.0.0
|
||||
dev: false
|
||||
|
||||
/safe-buffer@5.2.1:
|
||||
resolution: {integrity: sha512-rp3So07KcdmmKbGvgaNxQSJr7bGVSVk5S9Eq1F+ppbRo70+YeaDxkw5Dd8NPN+GD6bjnYm2VuPuCXmpuYvmCXQ==}
|
||||
dev: false
|
||||
|
||||
/safer-buffer@2.1.2:
|
||||
resolution: {integrity: sha512-YZo3K82SD7Riyi0E1EQPojLz7kpepnSQI9IyPbHHg1XXXevb5dJI7tpyN2ADxGcQbHG7vcyRHk0cbwqcQriUtg==}
|
||||
dev: false
|
||||
|
||||
/send@0.18.0:
|
||||
resolution: {integrity: sha512-qqWzuOjSFOuqPjFe4NOsMLafToQQwBSOEpS+FwEt3A2V3vKubTquT3vmLTQpFgMXp8AlFWFuP1qKaJZOtPpVXg==}
|
||||
engines: {node: '>= 0.8.0'}
|
||||
dependencies:
|
||||
debug: 2.6.9
|
||||
depd: 2.0.0
|
||||
destroy: 1.2.0
|
||||
encodeurl: 1.0.2
|
||||
escape-html: 1.0.3
|
||||
etag: 1.8.1
|
||||
fresh: 0.5.2
|
||||
http-errors: 2.0.0
|
||||
mime: 1.6.0
|
||||
ms: 2.1.3
|
||||
on-finished: 2.4.1
|
||||
range-parser: 1.2.1
|
||||
statuses: 2.0.1
|
||||
transitivePeerDependencies:
|
||||
- supports-color
|
||||
dev: false
|
||||
|
||||
/serve-static@1.15.0:
|
||||
resolution: {integrity: sha512-XGuRDNjXUijsUL0vl6nSD7cwURuzEgglbOaFuZM9g3kwDXOWVTck0jLzjPzGD+TazWbboZYu52/9/XPdUgne9g==}
|
||||
engines: {node: '>= 0.8.0'}
|
||||
dependencies:
|
||||
encodeurl: 1.0.2
|
||||
escape-html: 1.0.3
|
||||
parseurl: 1.3.3
|
||||
send: 0.18.0
|
||||
transitivePeerDependencies:
|
||||
- supports-color
|
||||
dev: false
|
||||
|
||||
/set-function-length@1.2.0:
|
||||
resolution: {integrity: sha512-4DBHDoyHlM1IRPGYcoxexgh67y4ueR53FKV1yyxwFMY7aCqcN/38M1+SwZ/qJQ8iLv7+ck385ot4CcisOAPT9w==}
|
||||
engines: {node: '>= 0.4'}
|
||||
dependencies:
|
||||
define-data-property: 1.1.1
|
||||
function-bind: 1.1.2
|
||||
get-intrinsic: 1.2.2
|
||||
gopd: 1.0.1
|
||||
has-property-descriptors: 1.0.1
|
||||
dev: false
|
||||
|
||||
/setprototypeof@1.2.0:
|
||||
resolution: {integrity: sha512-E5LDX7Wrp85Kil5bhZv46j8jOeboKq5JMmYM3gVGdGH8xFpPWXUMsNrlODCrkoxMEeNi/XZIwuRvY4XNwYMJpw==}
|
||||
dev: false
|
||||
|
||||
/side-channel@1.0.4:
|
||||
resolution: {integrity: sha512-q5XPytqFEIKHkGdiMIrY10mvLRvnQh42/+GoBlFW3b2LXLE2xxJpZFdm94we0BaoV3RwJyGqg5wS7epxTv0Zvw==}
|
||||
dependencies:
|
||||
call-bind: 1.0.5
|
||||
get-intrinsic: 1.2.2
|
||||
object-inspect: 1.13.1
|
||||
dev: false
|
||||
|
||||
/statuses@2.0.1:
|
||||
resolution: {integrity: sha512-RwNA9Z/7PrK06rYLIzFMlaF+l73iwpzsqRIFgbMLbTcLD6cOao82TaWefPXQvB2fOC4AjuYSEndS7N/mTCbkdQ==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/toidentifier@1.0.1:
|
||||
resolution: {integrity: sha512-o5sSPKEkg/DIQNmH43V0/uerLrpzVedkUh8tGNvaeXpfpuwjKenlSox/2O/BTlZUtEe+JG7s5YhEz608PlAHRA==}
|
||||
engines: {node: '>=0.6'}
|
||||
dev: false
|
||||
|
||||
/type-is@1.6.18:
|
||||
resolution: {integrity: sha512-TkRKr9sUTxEH8MdfuCSP7VizJyzRNMjj2J2do2Jr3Kym598JVdEksuzPQCnlFPW4ky9Q+iA+ma9BGm06XQBy8g==}
|
||||
engines: {node: '>= 0.6'}
|
||||
dependencies:
|
||||
media-typer: 0.3.0
|
||||
mime-types: 2.1.35
|
||||
dev: false
|
||||
|
||||
/unpipe@1.0.0:
|
||||
resolution: {integrity: sha512-pjy2bYhSsufwWlKwPc+l3cN7+wuJlK6uz0YdJEOlQDbl6jo/YlPi4mb8agUkVC8BF7V8NuzeyPNqRksA3hztKQ==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/utils-merge@1.0.1:
|
||||
resolution: {integrity: sha512-pMZTvIkT1d+TFGvDOqodOclx0QWkkgi6Tdoa8gC8ffGAAqz9pzPTZWAybbsHHoED/ztMtkv/VoYTYyShUn81hA==}
|
||||
engines: {node: '>= 0.4.0'}
|
||||
dev: false
|
||||
|
||||
/vary@1.1.2:
|
||||
resolution: {integrity: sha512-BNGbWLfd0eUPabhkXUVm0j8uuvREyTh5ovRa/dyow/BqAbZJyC+5fU+IzQOzmAKzYqYRAISoRhdQr3eIZ/PXqg==}
|
||||
engines: {node: '>= 0.8'}
|
||||
dev: false
|
||||
|
||||
/ws@8.16.0:
|
||||
resolution: {integrity: sha512-HS0c//TP7Ina87TfiPUz1rQzMhHrl/SG2guqRcTOIUYD2q8uhUdNHZYJUaQ8aTGPzCh+c6oawMKW35nFl1dxyQ==}
|
||||
engines: {node: '>=10.0.0'}
|
||||
peerDependencies:
|
||||
bufferutil: ^4.0.1
|
||||
utf-8-validate: '>=5.0.2'
|
||||
peerDependenciesMeta:
|
||||
bufferutil:
|
||||
optional: true
|
||||
utf-8-validate:
|
||||
optional: true
|
||||
dev: false
|
||||
-496
@@ -1,496 +0,0 @@
|
||||
import express from "express";
|
||||
import cors from "cors";
|
||||
import Kinematic from "./kinematic.js";
|
||||
import { WebSocketServer } from "ws";
|
||||
|
||||
const app = express();
|
||||
const kinematic = new Kinematic();
|
||||
const wss = new WebSocketServer({ port: 2096 });
|
||||
|
||||
app.use(cors());
|
||||
app.use(express.json());
|
||||
|
||||
const port = 3000;
|
||||
const subscriptions = {};
|
||||
|
||||
const randomFloatFromInterval = (min, max) =>
|
||||
Math.floor((Math.random() * (max - min + 1) + min) * 100) / 100;
|
||||
|
||||
const radToDeg = (val) => val * (180 / Math.PI);
|
||||
const degToRad = (val) => val * (Math.PI / 180);
|
||||
|
||||
const lerp = (start, end, amt) => {
|
||||
return (1 - amt) * start + amt * end;
|
||||
};
|
||||
|
||||
function createNewClientState() {
|
||||
return {
|
||||
model: JSON.parse(JSON.stringify(model)),
|
||||
settings: JSON.parse(JSON.stringify(settings)),
|
||||
logs: JSON.parse(JSON.stringify(logs)),
|
||||
subscriptions: {},
|
||||
mode: "idle",
|
||||
controller: {
|
||||
stop: 0,
|
||||
lx: 0,
|
||||
ly: 0,
|
||||
rx: 0,
|
||||
ry: 0,
|
||||
h: 70,
|
||||
s: 0,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
const unsubscribeClientFromCategory = (ws, category) => {
|
||||
if (!subscriptions[category]) return;
|
||||
subscriptions[category].delete(ws);
|
||||
if (subscriptions[category].size === 0) {
|
||||
delete subscriptions[category].size;
|
||||
}
|
||||
};
|
||||
|
||||
const sendUpdateToSubscribers = (category, data) => {
|
||||
if (subscriptions[category]) {
|
||||
const message = JSON.stringify(data);
|
||||
for (const client of subscriptions[category]) {
|
||||
client.send(message);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
if (!Array.prototype.last) {
|
||||
Array.prototype.last = function () {
|
||||
return this[this.length - 1];
|
||||
};
|
||||
}
|
||||
|
||||
const model = {
|
||||
battery: {
|
||||
voltage: randomFloatFromInterval(7.6, 8.2),
|
||||
ampere: randomFloatFromInterval(0.2, 3),
|
||||
power_button: false,
|
||||
},
|
||||
servos: {
|
||||
angles: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
|
||||
dir: [-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1],
|
||||
on: true,
|
||||
},
|
||||
mpu: {
|
||||
x: 0,
|
||||
y: 0,
|
||||
z: 0,
|
||||
heading: 240,
|
||||
temperature: 21,
|
||||
},
|
||||
display: [[]],
|
||||
distance_sensors: {
|
||||
left: 22,
|
||||
right: 23,
|
||||
},
|
||||
appTime: 1123321,
|
||||
connectivity: {
|
||||
ssid: "best network",
|
||||
ip: "192.168.0.118",
|
||||
mDNS: "leika.local",
|
||||
rssi: 100,
|
||||
},
|
||||
running: true,
|
||||
mode: "stand",
|
||||
rotation: [0, 0, 0],
|
||||
position: [0, 0, 0],
|
||||
};
|
||||
|
||||
const settings = {
|
||||
useMetric: true,
|
||||
name: "Leika",
|
||||
ssid: "Rune private network",
|
||||
pass: "12345678",
|
||||
ap: "Leika",
|
||||
apPass: "12345678",
|
||||
apChannel: 1,
|
||||
};
|
||||
|
||||
const logs = [
|
||||
"[2023-02-05 10:00:00] [verbose] Booting up",
|
||||
"[2023-02-05 10:00:10] [verbose] Starting webserver",
|
||||
"[2023-02-05 10:00:20] [verbose] Loading setting",
|
||||
"[2023-02-05 10:00:30] [verbose] Connected to Rune private network",
|
||||
];
|
||||
|
||||
const system = {
|
||||
HeapSize: 400000,
|
||||
HeapFree: 0,
|
||||
HeapMin: 0,
|
||||
DmaHeapSize: 0,
|
||||
DmaHeapFree: 0,
|
||||
DmaHeapMin: 0,
|
||||
PsramSize: 400000,
|
||||
PsramFree: 0,
|
||||
PsramMin: 0,
|
||||
ChipModel: 0,
|
||||
ChipRevision: 0,
|
||||
ChipCores: 2,
|
||||
CpuFreqMHz: 80,
|
||||
SketchSize: 0,
|
||||
FreeSketchSpace: 10200,
|
||||
FlashChipSize: 0,
|
||||
CpuUsed: 0,
|
||||
CpuUsedCore0: 0,
|
||||
CpuUsedCore1: 0,
|
||||
arduinoVersion: "3.2.1",
|
||||
};
|
||||
|
||||
const updateBattery = () => {
|
||||
model.battery.voltage = randomFloatFromInterval(7.6, 8.2);
|
||||
model.battery.ampere = randomFloatFromInterval(0.2, 3);
|
||||
return model.battery;
|
||||
};
|
||||
|
||||
const updateMpu = () => {
|
||||
model.mpu.x = randomFloatFromInterval(0, 1);
|
||||
model.mpu.y = randomFloatFromInterval(0, 1);
|
||||
model.mpu.z = randomFloatFromInterval(0, 1);
|
||||
model.mpu.temperature = randomFloatFromInterval(20, 22);
|
||||
return model.mpu;
|
||||
};
|
||||
|
||||
const updateDistances = () => {
|
||||
model.distance_sensors = {
|
||||
left: randomFloatFromInterval(10, 220),
|
||||
right: randomFloatFromInterval(10, 220),
|
||||
};
|
||||
return model.distance_sensors;
|
||||
};
|
||||
|
||||
const updateDistance = (position) => {
|
||||
model.distance_sensors[position] = randomFloatFromInterval(10, 220);
|
||||
return model.distance_sensors[position];
|
||||
};
|
||||
|
||||
const updateSystem = () => {
|
||||
system.CpuUsedCore0 = randomFloatFromInterval(0, 100);
|
||||
system.CpuUsedCore1 = randomFloatFromInterval(0, 100);
|
||||
system.CpuUsed =
|
||||
Math.floor((system.CpuUsedCore0 + system.CpuUsedCore1) / 0.02) / 100;
|
||||
system.HeapFree = randomFloatFromInterval(0, 20000);
|
||||
system.HeapMin = randomFloatFromInterval(0, 20000);
|
||||
return system;
|
||||
};
|
||||
|
||||
const updateBodyState = (model, angles, position) => {
|
||||
const Lp = [
|
||||
[100, -100, 100, 1],
|
||||
[100, -100, -100, 1],
|
||||
[-100, -100, 100, 1],
|
||||
[-100, -100, -100, 1],
|
||||
];
|
||||
|
||||
model.servos.angles = kinematic
|
||||
.calcIK(
|
||||
Lp,
|
||||
angles.map((x) => degToRad(x)),
|
||||
position
|
||||
)
|
||||
.flat()
|
||||
.map((x, i) => radToDeg(x * model.servos.dir[i]));
|
||||
return model.servos.angles;
|
||||
};
|
||||
|
||||
const updateAngle = (id, angle) => {
|
||||
model.servos.angles[id] = angle;
|
||||
return model.servos.angles;
|
||||
};
|
||||
|
||||
const updateAngles = (angles) => {
|
||||
model.servos.angles = angles;
|
||||
return model.servos.angles;
|
||||
};
|
||||
|
||||
const bufferToController = (buffer) => {
|
||||
return {
|
||||
stop: buffer[0],
|
||||
lx: buffer[1],
|
||||
ly: buffer[2],
|
||||
rx: buffer[3],
|
||||
ry: buffer[4],
|
||||
h: buffer[5],
|
||||
s: buffer[6],
|
||||
};
|
||||
};
|
||||
|
||||
const unpackMessageBuffer = (data) => {
|
||||
return {
|
||||
angles: [0, data.rx / 4, data.ry / 4],
|
||||
position: [data.ly / 2, 70, data.lx / 2],
|
||||
};
|
||||
};
|
||||
|
||||
const rest_stance = {
|
||||
rotation: [0, 0, 0],
|
||||
position: [0, 10, 0],
|
||||
};
|
||||
|
||||
const idle = () => {};
|
||||
|
||||
const rest = (client) => {
|
||||
for (let i = 0; i < 3; i++) {
|
||||
client.clientState.model.position[i] = lerp(
|
||||
client.clientState.model.position[i],
|
||||
rest_stance.position[i],
|
||||
0.01
|
||||
);
|
||||
client.clientState.model.rotation[i] = lerp(
|
||||
client.clientState.model.rotation[i],
|
||||
rest_stance.rotation[i],
|
||||
0.01
|
||||
);
|
||||
}
|
||||
client.send(
|
||||
JSON.stringify({
|
||||
type: "angles",
|
||||
data: updateBodyState(
|
||||
client.clientState.model,
|
||||
client.clientState.model.rotation,
|
||||
client.clientState.model.position
|
||||
),
|
||||
})
|
||||
);
|
||||
};
|
||||
|
||||
const stand = (client) => {
|
||||
if (!client.clientState.model.running) return;
|
||||
const data = unpackMessageBuffer(client.clientState.controller);
|
||||
client.send(
|
||||
JSON.stringify({
|
||||
type: "angles",
|
||||
data: updateBodyState(
|
||||
client.clientState.model,
|
||||
data.angles,
|
||||
data.position
|
||||
),
|
||||
})
|
||||
);
|
||||
};
|
||||
|
||||
// https://www.hindawi.com/journals/cin/2016/9853070/
|
||||
|
||||
const step = (model, controller, tick) => {
|
||||
const y1 = -100 * Math.sin(-0.05 * tick) - 150;
|
||||
const y2 = -100 * Math.sin(-0.05 * tick + Math.PI) - 150;
|
||||
const x1 = Math.abs((tick % 120) - 60) - 60;
|
||||
const Lp = [
|
||||
// -50 is minimum
|
||||
[100, y1, 100, 1],
|
||||
[100, y2, -100, 1],
|
||||
[-100, y2, 100, 1],
|
||||
[-100, y1, -100, 1],
|
||||
];
|
||||
|
||||
model.servos.angles = kinematic
|
||||
.calcIK(
|
||||
Lp,
|
||||
model.rotation.map((x) => degToRad(x)),
|
||||
model.position
|
||||
)
|
||||
.flat()
|
||||
.map((x, i) => radToDeg(x * model.servos.dir[i]));
|
||||
return model.servos.angles;
|
||||
};
|
||||
|
||||
const walk = (client) => {
|
||||
const angles = step(
|
||||
client.clientState.model,
|
||||
client.clientState.controller,
|
||||
client.tick
|
||||
);
|
||||
client.send(JSON.stringify({ type: "angles", data: angles }));
|
||||
};
|
||||
|
||||
const start_dynamics = (client) => {
|
||||
client.tick = 0;
|
||||
client.clientState.mode = "idle";
|
||||
client.clientState.next_mode = "walk";
|
||||
const modes = { rest, idle, stand, walk };
|
||||
client.id = setInterval(() => {
|
||||
client.tick += 1;
|
||||
|
||||
if (client.clientState.mode !== client.clientState.next_mode) {
|
||||
// Transition
|
||||
client.clientState.mode = client.clientState.next_mode;
|
||||
} else {
|
||||
modes[client.clientState.mode](client);
|
||||
}
|
||||
}, 10);
|
||||
};
|
||||
|
||||
const handelController = (ws, buffer) => {
|
||||
const controllerData = bufferToController(new Int8Array(buffer));
|
||||
ws.clientState.controller = controllerData;
|
||||
if (controllerData.stop) {
|
||||
ws.clientState.model.running = false;
|
||||
ws.clientState.logs.push("[2024-02-05 19:10:00] [Warning] STOPPING SERVOS");
|
||||
ws.send(JSON.stringify({ type: "log", data: ws.clientState.logs.last() }));
|
||||
return;
|
||||
}
|
||||
};
|
||||
|
||||
const handleBufferMessage = (ws, buffer) => {
|
||||
if (buffer.length === 6) {
|
||||
handelController(ws, buffer);
|
||||
}
|
||||
};
|
||||
|
||||
const handleJsonMessage = (ws, data) => {
|
||||
switch (data.type) {
|
||||
case "mode":
|
||||
ws.clientState.next_mode = data.data;
|
||||
// ws.send({ type: "battery", data: JSON.stringify(updateBattery()) });
|
||||
break;
|
||||
case "sensor/battery":
|
||||
ws.send({ type: "battery", data: JSON.stringify(updateBattery()) });
|
||||
break;
|
||||
case "sensor/mpu":
|
||||
ws.send({ type: "battery", data: JSON.stringify(updateMpu()) });
|
||||
break;
|
||||
case "sensor/distances":
|
||||
ws.send(JSON.stringify(updateDistances()));
|
||||
break;
|
||||
case "sensor/distance":
|
||||
ws.send(JSON.stringify({ distance: updateDistance(data.position) }));
|
||||
break;
|
||||
case "kinematic/angle":
|
||||
if (data.angle && data.id) {
|
||||
ws.clientState.model.servos.angles[data.id] = data.angle;
|
||||
ws.send(
|
||||
JSON.stringify({
|
||||
type: "angles",
|
||||
data: ws.clientState.model.servos.angles,
|
||||
})
|
||||
);
|
||||
} else {
|
||||
ws.send(JSON.stringify(updateAngle(data.id, data.angle)));
|
||||
}
|
||||
break;
|
||||
case "kinematic/angles":
|
||||
if (data.angles) {
|
||||
ws.clientState.model.servos.angles = data.angles;
|
||||
ws.send(
|
||||
JSON.stringify({
|
||||
type: "angles",
|
||||
data: ws.clientState.model.servos.angles,
|
||||
})
|
||||
);
|
||||
} else {
|
||||
ws.send(JSON.stringify(updateAngles(data.angles)));
|
||||
}
|
||||
break;
|
||||
case "kinematic/bodystate":
|
||||
if (data.angles) {
|
||||
ws.send(
|
||||
JSON.stringify({
|
||||
type: "angles",
|
||||
data: updateBodyState(
|
||||
ws.clientState.model,
|
||||
data.angles,
|
||||
data.position
|
||||
),
|
||||
})
|
||||
);
|
||||
} else {
|
||||
ws.send(JSON.stringify({ angles: model.servos.angles }));
|
||||
}
|
||||
break;
|
||||
case "system/logs":
|
||||
ws.send(JSON.stringify({ type: "logs", data: ws.clientState.logs }));
|
||||
break;
|
||||
case "system/info":
|
||||
ws.send(JSON.stringify({ type: "info", data: updateSystem() }));
|
||||
break;
|
||||
case "system/settings":
|
||||
if (data.settings) {
|
||||
Object.entries(data.settings).forEach(
|
||||
([key, value]) => (ws.clientState.settings[key] = value)
|
||||
);
|
||||
ws.send(JSON.stringify(ws.clientState.settings));
|
||||
} else {
|
||||
ws.send(
|
||||
JSON.stringify({
|
||||
type: "settings",
|
||||
settings: ws.clientState.settings,
|
||||
})
|
||||
);
|
||||
}
|
||||
break;
|
||||
case "system/stop":
|
||||
ws.clientState.model.running = false;
|
||||
ws.clientState.logs.push(
|
||||
"[2024-02-05 19:10:00] [Warning] STOPPING SERVOS"
|
||||
);
|
||||
ws.send(
|
||||
JSON.stringify({ type: "log", data: ws.clientState.logs.last() })
|
||||
);
|
||||
break;
|
||||
default:
|
||||
ws.send(JSON.stringify({ error: "Unknown request type" }));
|
||||
}
|
||||
};
|
||||
|
||||
wss.on("connection", (ws) => {
|
||||
const clientState = createNewClientState();
|
||||
ws.clientState = clientState;
|
||||
start_dynamics(ws);
|
||||
ws.on("error", console.error);
|
||||
|
||||
ws.on("message", (message) => {
|
||||
let data = message;
|
||||
try {
|
||||
data = JSON.parse(message);
|
||||
handleJsonMessage(ws, data);
|
||||
} catch (error) {
|
||||
handleBufferMessage(ws, message);
|
||||
}
|
||||
});
|
||||
|
||||
ws.on("close", () => {
|
||||
for (const category in subscriptions) {
|
||||
unsubscribeClientFromCategory(ws, category);
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
app.get("/sensor/battery", (req, res) => res.send(updateBattery()));
|
||||
app.get("/sensor/mpu", (req, res) => res.send(updateMpu()));
|
||||
app.get("/sensor/distances", (req, res) => res.send(updateDistances()));
|
||||
app.get("/sensor/distance/:position", (req, res) =>
|
||||
res.send({ distance: updateDistance(req.params.position) })
|
||||
);
|
||||
|
||||
// ----------------------------------------------------------- //
|
||||
|
||||
app.post("/kinematic/angle/:id", (req, res) =>
|
||||
res.send(updateAngle(req.params.id, req.body.angle))
|
||||
);
|
||||
app.post("/kinematic/angles/", (req, res) =>
|
||||
res.send(updateAngles(req.body.angles))
|
||||
);
|
||||
app.get("/kinematic/bodystate", (req, res) => res.send(model.servos.angles));
|
||||
app.post("/kinematic/bodystate", (req, res) => {
|
||||
sendUpdateToSubscribers("angles", model.servos.angles);
|
||||
res.send(updateBodyState(model, req.body.angles, req.body.position));
|
||||
});
|
||||
|
||||
// ----------------------------------------------------------- //
|
||||
|
||||
app.get("/system/log", (req, res) => res.send(logs));
|
||||
app.get("/system/info", (req, res) => res.send(updateSystem()));
|
||||
app.get("/system/settings", (req, res) => res.send(settings));
|
||||
app.post("/system/settings", (req, res) => {
|
||||
Object.entries(req.body).forEach((x) => (settings[x[0]] = x[1]));
|
||||
res.send(settings);
|
||||
});
|
||||
app.post("/system/stop", (req, res) => {
|
||||
model.running = false;
|
||||
model.res.send(settings);
|
||||
});
|
||||
app.listen(port, () => console.log(`Open at http://localhost:${port}`));
|
||||
Reference in New Issue
Block a user