From 96b567122ca252d0ea59cc467f4f6261b1a0615a Mon Sep 17 00:00:00 2001 From: ymayarajan3 Date: Sun, 10 Nov 2024 16:00:09 -0500 Subject: [PATCH] integration updates --- extensions/src/doodlebot/Doodlebot.ts | 8 ++++- extensions/src/doodlebot/LineFollowing.ts | 42 +++++++++++------------ extensions/src/doodlebot/index.ts | 8 +++++ extensions/src/doodlebot/package.json | 3 +- 4 files changed, 38 insertions(+), 23 deletions(-) diff --git a/extensions/src/doodlebot/Doodlebot.ts b/extensions/src/doodlebot/Doodlebot.ts index 19a353cde..ff41c9b91 100644 --- a/extensions/src/doodlebot/Doodlebot.ts +++ b/extensions/src/doodlebot/Doodlebot.ts @@ -646,13 +646,19 @@ export default class Doodlebot { const intervalId = setInterval(async () => { try { const lineData = await detector.detectLine(); + console.log(lineData); if (first) { ({ motorCommands, bezierPoints, line } = followLine(lineData, lineData, null, delay, previousSpeed, [], false, true)); + first = false; } else { ({ motorCommands, bezierPoints, line } = followLine(line, lineData, null, delay, previousSpeed, motorCommands, false, false)); } + for (const command of motorCommands) { + const { radius, angle } = command; + await this.motorCommand("arc", radius, angle); + } // Process the line data here - console.log(lineData); + } catch (error) { console.error("Error detecting line:", error); // Optionally stop polling if there's a consistent error diff --git a/extensions/src/doodlebot/LineFollowing.ts b/extensions/src/doodlebot/LineFollowing.ts index f1812c464..70535e9a0 100644 --- a/extensions/src/doodlebot/LineFollowing.ts +++ b/extensions/src/doodlebot/LineFollowing.ts @@ -7,7 +7,7 @@ import { type Point, type ProcrustesResult, type RobotPosition, type Command, ap // CONSTANTS const maxDistance = 100; const epsilon = 0.3; -const bezierSamples = 3; +const bezierSamples = 2; const controlLength = .02; const lookahead = 0.06; @@ -332,13 +332,13 @@ function prependUntilTarget(line) { export function followLine(previousLine: Point[], pixels: Point[], next: Point[], delay: number, previousSpeed: number, previousCommands: Command[], test: Boolean, first = false) { - - let nextPoints: Point[]; - if (test) { - nextPoints = simplifyLine(next, epsilon, 0.1); - nextPoints = cutOffLineOnDistance(nextPoints.filter((point: Point) => point[1] < 370), maxDistance); - nextPoints = nextPoints.map(point => pixelToGroundCoordinates(point)); - } + + let nextPoints: Point[]; + if (test) { + nextPoints = simplifyLine(next, epsilon, 0.1); + nextPoints = cutOffLineOnDistance(nextPoints.filter((point: Point) => point[1] < 370), maxDistance); + nextPoints = nextPoints.map(point => pixelToGroundCoordinates(point)); + } let worldPoints = simplifyLine(pixels, epsilon, 0.1); worldPoints = cutOffLineOnDistance(worldPoints.filter((point: Point) => point[1] < 370), maxDistance); @@ -352,13 +352,13 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] let multiplier = 1; let distanceTest = 0.06 / multiplier; if (test) { - try { - if (nextPoints && nextPoints.length > 20 && worldPoints.length > 20) { - let res = procrustes(worldPoints, nextPoints, 0.6); - distanceTest = res.distance - } + try { + if (nextPoints && nextPoints.length > 20 && worldPoints.length > 20) { + let res = procrustes(worldPoints, nextPoints, 0.6); + distanceTest = res.distance + } - } catch (e) { } + } catch (e) { } } /* TESTING */ @@ -426,9 +426,9 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] // Find the end point for the Bezier curve let distance: number; if (test) { - distance = distanceTest * 0.9; + distance = distanceTest * 0.9; } else { - distance = previousSpeed*delay + lookahead; + distance = previousSpeed * delay + lookahead; } const x1 = findPointAtDistanceWithIncrements(spline, 0.001, distance - .01); @@ -451,21 +451,21 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] // Find the start point for the Bezier curve -- account for camera latency let x3: number; if (test) { - x3 = spline.xs[0]; + x3 = spline.xs[0]; } else { - x3 = previousSpeed * delay; + x3 = previousSpeed * delay; } const point3 = { x: spline.at(x3), y: x3 } // Find the x offset to correct const reference1 = [spline.at(spline.xs[0]), 0] // First point should be very close to 0 const reference2 = [0, 0] - + let xOffset: number; if (test) { - xOffset = 0; + xOffset = 0; } else { - xOffset = reference1[0] - reference2[0]; + xOffset = reference1[0] - reference2[0]; } // We want to correct the offset and direct the robot to a future point on the curve diff --git a/extensions/src/doodlebot/index.ts b/extensions/src/doodlebot/index.ts index a511fd075..1aa56298f 100644 --- a/extensions/src/doodlebot/index.ts +++ b/extensions/src/doodlebot/index.ts @@ -318,6 +318,14 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator await this.followLine(); } + @block({ + type: "command", + text: "test line follow 2" + }) + async testLine2() { + await this.doodlebot.followLine(); + } + @block({ type: "command", text: (direction) => `move pen ${direction}`, diff --git a/extensions/src/doodlebot/package.json b/extensions/src/doodlebot/package.json index 108757a8a..6b40ca0d2 100644 --- a/extensions/src/doodlebot/package.json +++ b/extensions/src/doodlebot/package.json @@ -15,10 +15,11 @@ "dependencies": { "@mediapipe/tasks-vision": "^0.10.12", "@types/web-bluetooth": "^0.0.20", + "axios": "^1.7.7", "bezier-js": "^6.1.4", "canvas": "^2.11.2", "cubic-spline": "^3.0.3", "curve-matcher": "^1.1.1", "events": "^3.3.0" } -} +} \ No newline at end of file