= writable({
left: { x: 0, y: 0 },
right: { x: 0, y: 0 },
height: 70,
speed: 0
});
-
-export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
-
-export const jointNames = persistentStore('joint_names', []);
-
-export const model = writable();
diff --git a/app/src/lib/utilities/math-utilities.ts b/app/src/lib/utilities/math-utilities.ts
index e4e8996..95c70e7 100644
--- a/app/src/lib/utilities/math-utilities.ts
+++ b/app/src/lib/utilities/math-utilities.ts
@@ -5,7 +5,7 @@ export const toUint8 = (number: number, min: number, max: number) => {
};
export const toInt8 = (number: number, min: number, max: number) => {
- number = Math.max(min, Math.min(max, number));
- let scaled = ((number - min) / (max - min)) * 255 - 128;
- return Math.max(-128, Math.min(127, Math.round(scaled))) | 0;
-};
\ No newline at end of file
+ number = Math.max(min, Math.min(max, number));
+ let scaled = ((number - min) / (max - min)) * 255 - 128;
+ return Math.max(-128, Math.min(127, Math.round(scaled))) | 0;
+};
diff --git a/app/src/lib/utilities/string-utilities.ts b/app/src/lib/utilities/string-utilities.ts
index 4394aaa..0ffc397 100644
--- a/app/src/lib/utilities/string-utilities.ts
+++ b/app/src/lib/utilities/string-utilities.ts
@@ -3,3 +3,7 @@ export const humanFileSize = (size: number): string => {
var i = size == 0 ? 0 : Math.floor(Math.log(size) / Math.log(1024));
return Number((size / Math.pow(1024, i)).toFixed(2)) * 1 + units[i];
};
+
+export const capitalize = (str: string): string => {
+ return str.charAt(0).toUpperCase() + str.slice(1).toLowerCase();
+};
diff --git a/app/src/main.ts b/app/src/main.ts
index 6ff0ed9..2b8d35b 100644
--- a/app/src/main.ts
+++ b/app/src/main.ts
@@ -3,7 +3,7 @@ import './index.css';
import App from './App.svelte';
const app = new App({
- target: document.getElementById('app')
+ target: document.getElementById('app') as HTMLElement
});
export default app;
diff --git a/app/src/routes/Controller.svelte b/app/src/routes/Controller.svelte
index 6475a03..88db930 100644
--- a/app/src/routes/Controller.svelte
+++ b/app/src/routes/Controller.svelte
@@ -2,7 +2,7 @@
import Stream from '$components/Views/Stream.svelte';
import Model from '$components/Views/Model.svelte';
import Controls from '$components/Controls.svelte';
- import { emulateModel } from '$lib/store';
+ import { emulateModel } from '$lib/stores';
diff --git a/app/src/routes/Settings.svelte b/app/src/routes/Settings.svelte
index c9b5b89..73a82c5 100644
--- a/app/src/routes/Settings.svelte
+++ b/app/src/routes/Settings.svelte
@@ -5,13 +5,10 @@
import Configuration from '../components/settings/Configuration.svelte';
import {
Icon,
- Wifi,
- CommandLine,
InformationCircle,
BookOpen,
AdjustmentsVertical,
- Cog6Tooth,
- Newspaper
+ Cog6Tooth
} from 'svelte-hero-icons';
import Calibration from '../components/settings/Calibration.svelte';
diff --git a/app/test/specs/number-convert.spec.ts b/app/test/specs/number-convert.spec.ts
index 6499494..39b10e1 100644
--- a/app/test/specs/number-convert.spec.ts
+++ b/app/test/specs/number-convert.spec.ts
@@ -42,5 +42,3 @@ describe('toInt8', () => {
expect(toInt8(2, -1, 1)).toBe(127);
});
});
-
-
diff --git a/mock/server.js b/mock/server.js
index 6ccf222..b717578 100644
--- a/mock/server.js
+++ b/mock/server.js
@@ -19,22 +19,29 @@ const randomFloatFromInterval = (min, max) =>
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,
+ },
};
}
-function subscribeClientToCategory(ws, category) {
- if (!subscriptions[category]) {
- subscriptions[category] = new Set();
- }
- subscriptions[category].add(ws);
-}
-
const unsubscribeClientFromCategory = (ws, category) => {
if (!subscriptions[category]) return;
subscriptions[category].delete(ws);
@@ -52,11 +59,11 @@ const sendUpdateToSubscribers = (category, data) => {
}
};
-if (!Array.prototype.last){
- Array.prototype.last = function(){
- return this[this.length - 1];
- };
-};
+if (!Array.prototype.last) {
+ Array.prototype.last = function () {
+ return this[this.length - 1];
+ };
+}
const model = {
battery: {
@@ -90,6 +97,8 @@ const model = {
},
running: true,
mode: "stand",
+ rotation: [0, 0, 0],
+ position: [0, 0, 0],
};
const settings = {
@@ -217,28 +226,111 @@ const unpackMessageBuffer = (data) => {
};
};
-const updateStanding = (ws, controller) => {
- if (!ws.clientState.model.running) return;
- const data = unpackMessageBuffer(controller);
- ws.send(
+const rest = {
+ rotation: [0, 0, 0],
+ position: [0, 10, 0],
+};
+
+const idle = (client) => {
+ for (let i = 0; i < 3; i++) {
+ client.clientState.model.position[i] = lerp(
+ client.clientState.model.position[i],
+ rest.position[i],
+ 0.01
+ );
+ client.clientState.model.rotation[i] = lerp(
+ client.clientState.model.rotation[i],
+ rest.rotation[i],
+ 0.01
+ );
+ }
+ client.send(
JSON.stringify({
type: "angles",
- data: updateBodyState(ws.clientState.model, data.angles, data.position),
+ 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 = { 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;
}
- if (ws.clientState.model.mode === "stand") {
- updateStanding(ws, controllerData);
- }
};
const handleBufferMessage = (ws, buffer) => {
@@ -247,19 +339,11 @@ const handleBufferMessage = (ws, buffer) => {
}
};
-const handleJsonMessage = (ws, message) => {
- let data = message;
- try {
- data = JSON.parse(message);
- } catch (error) {
- return;
- }
+const handleJsonMessage = (ws, data) => {
switch (data.type) {
- case "subscribe":
- subscribeClientToCategory(ws, data.category);
- break;
- case "unsubscribe":
- unsubscribeClientFromCategory(ws, data.category);
+ 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()) });
@@ -353,15 +437,17 @@ const handleJsonMessage = (ws, message) => {
wss.on("connection", (ws) => {
const clientState = createNewClientState();
ws.clientState = clientState;
+ start_dynamics(ws);
ws.on("error", console.error);
ws.on("message", (message) => {
- if (typeof message === "object") {
+ let data = message;
+ try {
+ data = JSON.parse(message);
+ handleJsonMessage(ws, data);
+ } catch (error) {
handleBufferMessage(ws, message);
- return;
}
-
- handleJsonMessage(ws, message);
});
ws.on("close", () => {