Skip to content

Commit addb341

Browse files
author
Eiren Rain
committed
Math despair
1 parent c12d7a1 commit addb341

File tree

4 files changed

+100
-62
lines changed

4 files changed

+100
-62
lines changed

‎dist/src/index.js

+19-13
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,7 @@ const home_calibration_json_1 = __importDefault(require("../home-calibration.jso
4141
const odrive_api_1 = require("./api-generator/odrive-api");
4242
const generated_api_1 = require("./generated-api");
4343
const udp_server_1 = require("./udp-server");
44-
const _3dmath_1 = require("./3dmath");
45-
const node_util_1 = __importDefault(require("node:util"));
44+
const three_1 = require("three");
4645
const simulateCircularMotion = true;
4746
const targetPrecision = 0.01;
4847
const channel = can.createRawChannel("can0", true);
@@ -110,17 +109,22 @@ const sendToRaw = (axis, encoder_raw) => {
110109
};
111110
const goToAngle = async (axis, angle, wait = true) => {
112111
var circularPosition = angle / (Math.PI * 2);
112+
if (circularPosition < 0 || circularPosition > 1) {
113+
console.log('Axis %d can\'t go to angle %d (%d), violates circularity at %d', axis, angle.toFixed(4), radToDeg(angle).toFixed(2), circularPosition.toFixed(4));
114+
return;
115+
}
113116
if (simulateCircularMotion) {
114117
const currentAngle = wrapToCircle(positions[axis]);
115118
var diff = circularPosition - currentAngle;
116119
if (diff < -0.5)
117120
diff += 1;
121+
console.log('Axis %d going to %s (%s), target pos %s', axis, angle.toFixed(2), radToDeg(angle).toFixed(2), circularPosition.toFixed(4));
118122
circularPosition = positions[axis] + diff;
119123
}
120-
if (wait)
121-
await goToRaw(axis, circularPosition, 10000);
122-
else
123-
sendToRaw(axis, circularPosition);
124+
// if (wait)
125+
// await goToRaw(axis, circularPosition, 10000)
126+
//else
127+
// sendToRaw(axis, circularPosition)
124128
};
125129
const goToAngles = async ({ x, y, z }, wait = true) => {
126130
await Promise.all([
@@ -135,8 +139,9 @@ const goToHome = async () => {
135139
};
136140
const degToRad = (degrees) => (degrees * Math.PI) / 180;
137141
const radToDeg = (rad) => rad * 180 / Math.PI;
138-
const AXIS_OFFSET = _3dmath_1.Quaternion.fromRotationVector(-Math.PI / 2, 0, 0);
142+
const AXIS_OFFSET = new three_1.Quaternion().setFromAxisAngle({ x: -1, y: 0, z: 0 }, Math.PI / 2);
139143
(async () => {
144+
//console.log(AXIS_OFFSET)
140145
forEachController((odrive) => odrive.sendClearErrors({ identify: 0 }));
141146
forEachController(initOdrive);
142147
currentLimit(odrive1, 8);
@@ -158,13 +163,14 @@ const AXIS_OFFSET = _3dmath_1.Quaternion.fromRotationVector(-Math.PI / 2, 0, 0);
158163
onPacket: (rinfo, packet) => {
159164
if (packet.sensorId == 0 && packet.dataType == 1) {
160165
const time = Date.now();
161-
if (time - lastUpdateTime > 50) {
166+
if (time - lastUpdateTime > 100) {
162167
lastUpdateTime = time;
163-
const quat = new _3dmath_1.Quaternion(packet.rotation.w, packet.rotation.x, packet.rotation.y, packet.rotation.z);
164-
const ofseted = AXIS_OFFSET.timesQuat(quat);
165-
const euler = ofseted.toEulerAngles(_3dmath_1.EulerOrder.XYZ);
166-
console.log(node_util_1.default.format('Angles: %5.2d, %5.2d, %5.2d', radToDeg(euler.x), radToDeg(euler.y), radToDeg(euler.z)));
167-
//goToAngles({x: euler.x, y: euler.y, z: euler.z}, false)
168+
const quat = new three_1.Quaternion(packet.rotation.x, packet.rotation.y, packet.rotation.z, packet.rotation.w);
169+
const ofseted = AXIS_OFFSET.clone().multiply(quat);
170+
const euler = new three_1.Euler().setFromQuaternion(ofseted);
171+
console.log('Angles: %s, %s, %s', radToDeg(euler.x).toFixed(2), radToDeg(euler.y).toFixed(2), radToDeg(euler.z).toFixed(2));
172+
//goToAngles({ x: euler.y, y: euler.z, z: euler.x }, false)
173+
goToAngle(0, euler.y + Math.PI, false);
168174
}
169175
}
170176
}

‎package.json

+2
Original file line numberDiff line numberDiff line change
@@ -17,10 +17,12 @@
1717
"dependencies": {
1818
"@inquirer/prompts": "^7.3.2",
1919
"@types/node": "^22.13.1",
20+
"@types/three": "^0.174.0",
2021
"bit-buffer": "^0.2.5",
2122
"rxjs": "^7.8.1",
2223
"socketcan": "^4.0.5",
2324
"strict-event-emitter-types": "^2.0.0",
25+
"three": "^0.174.0",
2426
"ts-creator": "^1.2.5",
2527
"typescript": "^5.7.3"
2628
}

‎pnpm-lock.yaml

+53-28
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎src/index.ts

+26-21
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,7 @@ import home_calibration from '../home-calibration.json'
55
import { getCmdId, getNodeId, initOdriveApi } from "./api-generator/odrive-api"
66
import { apiFunctions, GetEncoderEstimatesMessage, HeartbeatMessage, inboundPacketsMap, Packets } from "./generated-api"
77
import { createSever, PacketHandshakeBuilder, PacketReturnType, PacketRotationData, PacketRotationDataBuilder } from "./udp-server"
8-
import { Quaternion, EulerAngles, EulerOrder } from "./3dmath"
9-
10-
import util from "node:util"
8+
import { Quaternion, Euler } from 'three'
119

1210

1311
const simulateCircularMotion = true
@@ -88,17 +86,22 @@ const sendToRaw = (axis: number, encoder_raw: number) => {
8886

8987
const goToAngle = async (axis: 0 | 1 | 2, angle: number, wait = true) => {
9088
var circularPosition = angle / (Math.PI * 2)
89+
if(circularPosition < 0 || circularPosition > 1) {
90+
console.log('Axis %d can\'t go to angle %d (%d), violates circularity at %d', axis, angle.toFixed(4), radToDeg(angle).toFixed(2), circularPosition.toFixed(4))
91+
return
92+
}
9193
if (simulateCircularMotion) {
9294
const currentAngle = wrapToCircle(positions[axis])
9395
var diff = circularPosition - currentAngle
94-
if(diff < -0.5)
96+
if (diff < -0.5)
9597
diff += 1
98+
console.log('Axis %d going to %s (%s), target pos %s', axis, angle.toFixed(2), radToDeg(angle).toFixed(2), circularPosition.toFixed(4))
9699
circularPosition = positions[axis] + diff
97100
}
98-
if(wait)
99-
await goToRaw(axis, circularPosition, 10000)
100-
else
101-
sendToRaw(axis, circularPosition)
101+
// if (wait)
102+
// await goToRaw(axis, circularPosition, 10000)
103+
//else
104+
// sendToRaw(axis, circularPosition)
102105
}
103106

104107
const goToAngles = async ({ x, y, z }: { x: number; y: number; z: number }, wait = true) => {
@@ -115,11 +118,12 @@ const goToHome = async () => {
115118
}
116119

117120
const degToRad = (degrees: number) => (degrees * Math.PI) / 180;
118-
const radToDeg = (rad: number) => rad * 180 / Math.PI
121+
const radToDeg = (rad: number) => rad * 180 / Math.PI;
119122

120-
const AXIS_OFFSET = Quaternion.fromRotationVector(-Math.PI / 2, 0, 0);
123+
const AXIS_OFFSET = new Quaternion().setFromAxisAngle({ x: -1, y: 0, z: 0 }, Math.PI / 2);
121124

122125
(async () => {
126+
//console.log(AXIS_OFFSET)
123127
forEachController((odrive) => odrive.sendClearErrors({ identify: 0 }))
124128
forEachController(initOdrive)
125129
currentLimit(odrive1, 8)
@@ -138,29 +142,30 @@ const AXIS_OFFSET = Quaternion.fromRotationVector(-Math.PI / 2, 0, 0);
138142
const server = createSever(6969, '0.0.0.0');
139143

140144
var lastUpdateTime = Date.now()
141-
145+
142146
server.onPacketReceived<typeof PacketRotationDataBuilder>(PacketRotationDataBuilder.id, {
143-
onPacket: (rinfo, packet) => {
144-
if(packet.sensorId == 0 && packet.dataType == 1) {
147+
onPacket: (rinfo, packet) => {
148+
if (packet.sensorId == 0 && packet.dataType == 1) {
145149
const time = Date.now()
146-
if(time - lastUpdateTime > 50) {
150+
if (time - lastUpdateTime > 100) {
147151
lastUpdateTime = time
148-
const quat = new Quaternion(packet.rotation.w, packet.rotation.x, packet.rotation.y, packet.rotation.z)
149-
const ofseted = AXIS_OFFSET.timesQuat(quat)
150-
const euler = ofseted.toEulerAngles(EulerOrder.XYZ)
151-
console.log(util.format('Angles: %5.2d, %5.2d, %5.2d', radToDeg(euler.x), radToDeg(euler.y), radToDeg(euler.z)))
152-
//goToAngles({x: euler.x, y: euler.y, z: euler.z}, false)
152+
const quat = new Quaternion(packet.rotation.x, packet.rotation.y, packet.rotation.z, packet.rotation.w)
153+
const ofseted = AXIS_OFFSET.clone().multiply(quat)
154+
const euler = new Euler().setFromQuaternion(ofseted)
155+
console.log('Angles: %s, %s, %s', radToDeg(euler.x).toFixed(2), radToDeg(euler.y).toFixed(2), radToDeg(euler.z).toFixed(2))
156+
//goToAngles({ x: euler.y, y: euler.z, z: euler.x }, false)
157+
goToAngle(0, euler.y + Math.PI, false)
153158
}
154159
}
155160
}
156161
})
157162

158163
server.onPacketReceived<typeof PacketHandshakeBuilder>(PacketHandshakeBuilder.id, {
159-
onPacket: (rinfo, packet) => {
164+
onPacket: (rinfo, packet) => {
160165
server.sendPacket(PacketHandshakeBuilder, {} as never, rinfo.address, rinfo.port)
161166
}
162167
})
163168

164-
169+
165170
console.log('DONE')
166171
})();

0 commit comments

Comments
 (0)