diff --git a/extensions/src/doodlebot/Doodlebot.ts b/extensions/src/doodlebot/Doodlebot.ts index f74a5e808..cd77aa1b0 100644 --- a/extensions/src/doodlebot/Doodlebot.ts +++ b/extensions/src/doodlebot/Doodlebot.ts @@ -643,7 +643,7 @@ export default class Doodlebot { const beforeLine = this.prependUntilTarget(lineData[0]); - let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], delay, previousSpeed, []); + let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], lineData[1], delay, previousSpeed, []); for (const command of motorCommands) { const { radius, angle } = command; // await this.motorCommand( diff --git a/extensions/src/doodlebot/LineFollowing.ts b/extensions/src/doodlebot/LineFollowing.ts index 43cd6b19d..b0b906f33 100644 --- a/extensions/src/doodlebot/LineFollowing.ts +++ b/extensions/src/doodlebot/LineFollowing.ts @@ -6,77 +6,77 @@ import { type Point, type ProcrustesResult, type RobotPosition, type Command, ap // CONSTANTS const maxDistance = 100; -const epsilon = 0.4; +const epsilon = 0.3; const bezierSamples = 3; const controlLength = .02; const lookahead = 0.06; -const imageDimensions = [640,480]; +const imageDimensions = [640, 480]; const horizontalFOV = 53.4; const verticalFOV = 41.41; -const cameraHeight = 0.098; +const cameraHeight = 0.098; const tiltAngle = 41.5; function cutOffLineOnDistance(line: Point[], maxDistance: number) { - let filteredLine = [line[0]]; // Start with the first point - - for (let i = 1; i < line.length; i++) { - const point1 = line[i - 1]; - const point2 = line[i]; - const distance = distanceBetweenPoints(point1, point2); - - // If the distance exceeds the threshold, stop adding points - if (distance > maxDistance) { - break; - } - - filteredLine.push(point2); // Add the current point if within distance - } - - return filteredLine; + let filteredLine = [line[0]]; // Start with the first point + + for (let i = 1; i < line.length; i++) { + const point1 = line[i - 1]; + const point2 = line[i]; + const distance = distanceBetweenPoints(point1, point2); + + // If the distance exceeds the threshold, stop adding points + if (distance > maxDistance) { + break; + } + + filteredLine.push(point2); // Add the current point if within distance + } + + return filteredLine; } - + function findPointAtDistanceWithIncrements(spline: Spline, increment: number, desiredDistance: number): number { - let totalDistance = 0; - const xValues = spline.xs; - - // Iterate through each pair of xValues in the array - for (let i = 0; i < xValues.length - 1; i++) { - let currentX = xValues[i]; - const nextX = xValues[i + 1]; - - // Step through each segment in increments, adjusting for direction - while (currentX < nextX) { - const nextXIncrement = currentX + increment; // Increment or decrement by step size - const currentY = spline.at(currentX); - const nextY = spline.at(nextXIncrement); - - // Calculate distance between current and next increment - if (!isNaN(nextY)) { - const distance = distanceBetweenPoints([currentX, currentY], [nextXIncrement, nextY]); - - totalDistance += distance; - if (totalDistance >= desiredDistance) { - return nextXIncrement; - } - currentX = nextXIncrement; - - // Stop if we overshoot the next point - if (currentX > nextX) { - currentX = nextX; - } - } else { - break; - } - + let totalDistance = 0; + const xValues = spline.xs; + + // Iterate through each pair of xValues in the array + for (let i = 0; i < xValues.length - 1; i++) { + let currentX = xValues[i]; + const nextX = xValues[i + 1]; + + // Step through each segment in increments, adjusting for direction + while (currentX < nextX) { + const nextXIncrement = currentX + increment; // Increment or decrement by step size + const currentY = spline.at(currentX); + const nextY = spline.at(nextXIncrement); + + // Calculate distance between current and next increment + if (!isNaN(nextY)) { + const distance = distanceBetweenPoints([currentX, currentY], [nextXIncrement, nextY]); + + totalDistance += distance; + if (totalDistance >= desiredDistance) { + return nextXIncrement; + } + currentX = nextXIncrement; + + // Stop if we overshoot the next point + if (currentX > nextX) { + currentX = nextX; + } + } else { + break; + } + + } } - } - // If the desired distance is beyond all xValues, return the last point - return xValues[xValues.length - 1]; + // If the desired distance is beyond all xValues, return the last point + return xValues[xValues.length - 1]; } - + function medianPoint(points: number[][]): number[] { if (points.length === 0) return [0, 0]; @@ -88,7 +88,7 @@ function medianPoint(points: number[][]): number[] { const middle = Math.floor(sortedPoints.length / 2); return sortedPoints[middle]; } - + function groupByYInterval(points: Point[], intervalSize: number): Point[] { const groupedPoints: Map = new Map(); @@ -116,47 +116,47 @@ function groupByYInterval(points: Point[], intervalSize: number): Point[] { // Sort by y-value (ascending) return medianPoints.sort((a, b) => a[1] - b[1]); } - - - function rdpSimplify(points: Point[], epsilon: number): Point[] { - if (points.length < 3) { - return points; - } - - let maxDistance = 0; - let index = 0; - - // Find the point with the maximum distance from the line formed by the first and last points. - for (let i = 1; i < points.length - 1; i++) { - const distance = perpendicularDistance(points[i], points[0], points[points.length - 1]); - if (distance > maxDistance) { - maxDistance = distance; - index = i; - } - } - - // If the maximum distance is greater than epsilon, recursively simplify. - if (maxDistance > epsilon) { - const leftSegment = rdpSimplify(points.slice(0, index + 1), epsilon); - const rightSegment = rdpSimplify(points.slice(index), epsilon); - - // Combine the results, excluding the duplicate point at the intersection. - return [...leftSegment.slice(0, -1), ...rightSegment]; - } else { - // If no point is far enough, return the endpoints. - return [points[0], points[points.length - 1]]; - } + + +function rdpSimplify(points: Point[], epsilon: number): Point[] { + if (points.length < 3) { + return points; + } + + let maxDistance = 0; + let index = 0; + + // Find the point with the maximum distance from the line formed by the first and last points. + for (let i = 1; i < points.length - 1; i++) { + const distance = perpendicularDistance(points[i], points[0], points[points.length - 1]); + if (distance > maxDistance) { + maxDistance = distance; + index = i; + } + } + + // If the maximum distance is greater than epsilon, recursively simplify. + if (maxDistance > epsilon) { + const leftSegment = rdpSimplify(points.slice(0, index + 1), epsilon); + const rightSegment = rdpSimplify(points.slice(index), epsilon); + + // Combine the results, excluding the duplicate point at the intersection. + return [...leftSegment.slice(0, -1), ...rightSegment]; + } else { + // If no point is far enough, return the endpoints. + return [points[0], points[points.length - 1]]; + } } - + function simplifyLine(points: Point[], epsilon: number, intervalSize: number): number[][] { - // Group points by y-interval and pick the median x-coordinate in each group - const medianPoints = groupByYInterval(points, intervalSize); - - // Simplify the remaining points using Ramer-Douglas-Peucker - return rdpSimplify(medianPoints, epsilon); - } - + // Group points by y-interval and pick the median x-coordinate in each group + const medianPoints = groupByYInterval(points, intervalSize); + + // Simplify the remaining points using Ramer-Douglas-Peucker + return rdpSimplify(medianPoints, epsilon); +} + function perpendicularDistance(point: number[], lineStart: number[], lineEnd: number[]): number { const [x, y] = point; @@ -168,7 +168,7 @@ function perpendicularDistance(point: number[], lineStart: number[], lineEnd: nu return den === 0 ? 0 : num / den; } - + function rotateAndTranslateLine(line: Point[], angle: number, translation: number[]) { const angleRadians = angle; // Convert angle to radians @@ -198,10 +198,10 @@ function getRobotPositionAfterArc(command: Command, initialPosition: RobotPositi radius = radius * 0.0254; // Calculate the center of the circular path - const centerX = direction === "left" + const centerX = direction === "left" ? initialX + radius * Math.cos(initialAngle) : initialX - radius * Math.cos(initialAngle); - const centerY = direction === "left" + const centerY = direction === "left" ? initialY + radius * Math.sin(initialAngle) : initialY - radius * Math.sin(initialAngle); @@ -209,10 +209,10 @@ function getRobotPositionAfterArc(command: Command, initialPosition: RobotPositi const finalAngleTraveled = initialAngle + angleRadians * (direction === "left" ? -1 : 1); // Calculate the new position along the arc - const newX = direction === "left" + const newX = direction === "left" ? centerX - radius * Math.cos(finalAngleTraveled) : centerX + radius * Math.cos(finalAngleTraveled); - const newY = direction === "left" + const newY = direction === "left" ? centerY - radius * Math.sin(finalAngleTraveled) : centerY + radius * Math.sin(finalAngleTraveled); @@ -256,97 +256,122 @@ function showLineBelowY(line: Point[], yLimit: number) { function smoothLine(line: Point[], windowSize = 3) { - const smoothedLine: Point[] = []; - - for (let i = 0; i < line.length; i++) { - // Define the range of indices for the smoothing window - let start = Math.max(0, i - Math.floor(windowSize / 2)); - let end = Math.min(line.length, i + Math.floor(windowSize / 2) + 1); - - // Sum the x and y values within the window - let sumX = 0; - let sumY = 0; - for (let j = start; j < end; j++) { - sumX += line[j][0]; - sumY += line[j][1]; - } - - // Push the averaged point to the smoothed line - let count = end - start; - smoothedLine.push([sumX / count, sumY / count]); - } - - return smoothedLine; + const smoothedLine: Point[] = []; + + for (let i = 0; i < line.length; i++) { + // Define the range of indices for the smoothing window + let start = Math.max(0, i - Math.floor(windowSize / 2)); + let end = Math.min(line.length, i + Math.floor(windowSize / 2) + 1); + + // Sum the x and y values within the window + let sumX = 0; + let sumY = 0; + for (let j = start; j < end; j++) { + sumX += line[j][0]; + sumY += line[j][1]; + } + + // Push the averaged point to the smoothed line + let count = end - start; + smoothedLine.push([sumX / count, sumY / count]); + } + + return smoothedLine; } function blendLines(entireLine: Point[], worldPoints: Point[], transitionLength: number = 3) { - // Start with the non-overlapping portion of entireLine - const blendedLine = [...cutOffLineAtOverlap(entireLine, worldPoints).line]; + // Start with the non-overlapping portion of entireLine + const blendedLine = [...cutOffLineAtOverlap(entireLine, worldPoints).line]; - // If either line is too short for blending, return them joined directly - if (entireLine.length < transitionLength || worldPoints.length < transitionLength) { - return [...entireLine, ...worldPoints]; - } + // If either line is too short for blending, return them joined directly + if (entireLine.length < transitionLength || worldPoints.length < transitionLength) { + return [...entireLine, ...worldPoints]; + } - // Blend the transition region between entireLine and worldPoints - for (let i = 0; i < transitionLength; i++) { - const t = i / (transitionLength - 1); // Interpolation factor (0 to 1) - const [x1, y1] = entireLine[entireLine.length - transitionLength + i]; - const [x2, y2] = worldPoints[i]; + // Blend the transition region between entireLine and worldPoints + for (let i = 0; i < transitionLength; i++) { + const t = i / (transitionLength - 1); // Interpolation factor (0 to 1) + const [x1, y1] = entireLine[entireLine.length - transitionLength + i]; + const [x2, y2] = worldPoints[i]; - // Linearly interpolate between corresponding points - const blendedX = (1 - t) * x1 + t * x2; - const blendedY = (1 - t) * y1 + t * y2; + // Linearly interpolate between corresponding points + const blendedX = (1 - t) * x1 + t * x2; + const blendedY = (1 - t) * y1 + t * y2; - blendedLine.push([blendedX, blendedY]); - } + blendedLine.push([blendedX, blendedY]); + } - // Append remaining points from worldPoints after the transition region - blendedLine.push(...worldPoints.slice(transitionLength)); + // Append remaining points from worldPoints after the transition region + blendedLine.push(...worldPoints.slice(transitionLength)); - return blendedLine; + return blendedLine; } +function prependUntilTarget(line) { + const targetX = line[0][0]; + const targetY = line[0][1]; + const startX = line[0][0]; + const startY = 0; // Start slightly below targetY + + + const incrementX = 0.01; // Small step for x + let x = startX; + let y = startY; + + const newSegment = []; + while (y < targetY) { + newSegment.push([x, y]); + y += incrementX; // Increment y based on slope + } + + // Prepend the new segment to the beginning of line + line.unshift(...newSegment); + return line; +} -export function followLine(previousLine: Point[], pixels: Point[], next: Point[], delay: number, previousSpeed: number, previousCommands: Command[]) { - let nextPoints = simplifyLine(pixels, epsilon, 0.1); - nextPoints = cutOffLineOnDistance(nextPoints.filter((point: Point) => point[1] < 370), maxDistance); - nextPoints = nextPoints.map(point => pixelToGroundCoordinates(point)); +export function followLine(previousLine: Point[], pixels: Point[], next: Point[], delay: number, previousSpeed: number, previousCommands: Command[], first = false) { + let 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); - worldPoints = worldPoints.map(point => pixelToGroundCoordinates(point)); + let worldPoints = simplifyLine(pixels, epsilon, 0.1); + worldPoints = cutOffLineOnDistance(worldPoints.filter((point: Point) => point[1] < 370), maxDistance); + worldPoints = worldPoints.map(point => pixelToGroundCoordinates(point)); - let multiplier = 1; - let distanceTest = 0.06/multiplier; + if (first) { + previousLine = prependUntilTarget(worldPoints); + } + + /* TESTING */ + let multiplier = 1; + let distanceTest = 0.06 / multiplier; try { if (nextPoints && nextPoints.length > 20 && worldPoints.length > 20) { let res = procrustes(worldPoints, nextPoints, 0.6); distanceTest = res.distance } - - } catch (e) {} - - - let robotPosition = {x:0, y:0, angle:0}; + } catch (e) { } + /* TESTING */ + + let robotPosition = { x: 0, y: 0, angle: 0 }; for (const command of previousCommands) { robotPosition = getRobotPositionAfterArc(command, robotPosition); } - // Guess the location of the previous line - let guessLine = rotateAndTranslateLine(previousLine, -1*robotPosition.angle, [-1*robotPosition.x, -1*robotPosition.y]); + // Guess the location of the previous line + let guessLine = rotateAndTranslateLine(previousLine, -1 * robotPosition.angle, [-1 * robotPosition.x, -1 * robotPosition.y]); - // Cutting off segments to the overlap portion - let segment1 = showLineAboveY(guessLine, Math.max(worldPoints[0][1], guessLine[0][1])); - let segment2 = showLineBelowY(worldPoints, Math.min(guessLine[guessLine.length - 1][1], worldPoints[worldPoints.length - 1][1])) + // Cutting off segments to the overlap portion + let segment1 = showLineAboveY(guessLine, Math.max(worldPoints[0][1], guessLine[0][1])); + let segment2 = showLineBelowY(worldPoints, Math.min(guessLine[guessLine.length - 1][1], worldPoints[worldPoints.length - 1][1])) // Distance of the world line let worldDistance = 0; for (let i = 0; i < worldPoints.length - 1; i++) { - worldDistance += distanceBetweenPoints(worldPoints[i], worldPoints[i + 1]); + worldDistance += distanceBetweenPoints(worldPoints[i], worldPoints[i + 1]); } // Collect the error between guess and world @@ -362,28 +387,29 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] } // Correct the guess of the previous line - let line = rotateCurve(guessLine.map((point: Point) => ({x: point[0], y: point[1]})), procrustesResult.rotation).map((point: {x: number, y: number}) => [point.x, point.y]); - line = applyTranslation(line, procrustesResult.translation); + let line = rotateCurve(guessLine.map((point: Point) => ({ x: point[0], y: point[1] })), procrustesResult.rotation).map((point: { x: number, y: number }) => [point.x, point.y]); + line = applyTranslation(line, procrustesResult.translation); line = showLineAboveY(line, 0); - + + if (worldDistance > 0.05) { // If we have enough points to append, add the new portion of the current camera frame let trimmedLine = cutOffLineAtOverlap(line, worldPoints); line = trimmedLine.overlap ? trimmedLine.line : blendLines(trimmedLine.line, worldPoints); - } + } line = smoothLine(line); - line = rebalanceCurve(line.map((point: Point) => ({x: point[0], y: point[1]})), {}).map((point: {x: number, y: number}) => [point.x, point.y]); + line = rebalanceCurve(line.map((point: Point) => ({ x: point[0], y: point[1] })), {}).map((point: { x: number, y: number }) => [point.x, point.y]); // Remove duplicate y values const seenY = new Set(); line = line.filter((point: Point) => { const y = point[1]; if (seenY.has(y)) { - return false; + return false; } seenY.add(y); - return true; + return true; }); // Create the spline @@ -393,11 +419,11 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] // Find the end point for the Bezier curve //const distance = previousSpeed*delay + lookahead; - const distance = distanceTest*0.9; + /* TESTING */ const distance = distanceTest * 0.9; /* TESTING */ const x1 = findPointAtDistanceWithIncrements(spline, 0.001, distance - .01); const x2 = findPointAtDistanceWithIncrements(spline, 0.001, distance); - const point1 = {x: spline.at(x1), y: x1} - const point2 = {x: spline.at(x2), y: x2} + const point1 = { x: spline.at(x1), y: x1 } + const point2 = { x: spline.at(x2), y: x2 } // Extend point1 in the direction of the unit vector to make the Bezier control point const dx = point1.x - point2.x; @@ -405,20 +431,22 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] const length = Math.sqrt(dx * dx + dy * dy); const unitDx = dx / length; const unitDy = dy / length; - + const extendedPoint1 = { x: point1.x + unitDx * controlLength, y: point1.y + unitDy * controlLength }; // Find the start point for the Bezier curve -- account for camera latency - const x3 = previousSpeed * delay; - const point3 = {x: spline.at(x3), y: x3} + //const x3 = previousSpeed * delay; + /* TESTING */ const x3 = spline.xs[0]; /* TESTING */ + 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 = reference1[0] - reference2[0]; + //let xOffset = reference1[0] - reference2[0]; + /* TESTING */ let xOffset = 0; /* TESTING */ // We want to correct the offset and direct the robot to a future point on the curve // TODO: Add angle correction to the control points @@ -433,20 +461,21 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[] // Split the Bezier curve into a series of arcs const bezierPoints = bezierCurvePoints(bezier, bezierSamples); + for (let i = 0; i < bezierPoints.length - 1; i++) { - const command = calculateCurveBetweenPoints(bezierPoints[i], bezierPoints[i+1]); + const command = calculateCurveBetweenPoints(bezierPoints[i], bezierPoints[i + 1]); motorCommands.push(command); } - return {motorCommands, bezierPoints, line}; + return { motorCommands, bezierPoints, line }; } export function pixelToGroundCoordinates(pixelCoords: Point): Point { // Based on Franklin's algorithm - const verticalPixels = imageDimensions[1]/verticalFOV; - const angleP = pixelCoords[1]/verticalPixels; + const verticalPixels = imageDimensions[1] / verticalFOV; + const angleP = pixelCoords[1] / verticalPixels; const angleC = (180 - (tiltAngle + 90)); const angleY = 180 - angleC; const sideB = solveForSide(90, angleC, cameraHeight); @@ -455,14 +484,14 @@ export function pixelToGroundCoordinates(pixelCoords: Point): Point { const distanceP = solveForSide(angleP, angleZ, sideB); const verticalOffset = sideA + distanceP; - const horizontalPixels = imageDimensions[0]/horizontalFOV; - const diversion = Math.abs(pixelCoords[0] - imageDimensions[0]/2); - const angleA = diversion/horizontalPixels; + const horizontalPixels = imageDimensions[0] / horizontalFOV; + const diversion = Math.abs(pixelCoords[0] - imageDimensions[0] / 2); + const angleA = diversion / horizontalPixels; const sideK = Math.sqrt(verticalOffset * verticalOffset + cameraHeight * cameraHeight); const angleB = 90 - angleA; - const horizontalOffset = pixelCoords[0] < imageDimensions[0]/2 ? solveForSide(angleA, angleB, sideK)*-1 : solveForSide(angleA, angleB, sideK); + const horizontalOffset = pixelCoords[0] < imageDimensions[0] / 2 ? solveForSide(angleA, angleB, sideK) * -1 : solveForSide(angleA, angleB, sideK); - return { x: horizontalOffset, y: verticalOffset}; + return [horizontalOffset, verticalOffset]; } @@ -474,15 +503,15 @@ function bezierCurvePoints(bezier: Bezier, n: number) { const position = bezier.get(t); let orientation: number; if (i == 0) { - orientation = Math.PI/2; - } else { - const position2 = bezier.get((i-1)/n); + orientation = Math.PI / 2; + } else { + const position2 = bezier.get((i - 1) / n); orientation = Math.atan2(position.y - position2.y, position.x - position2.x) } - points.push({ x: position.x, y: position.y, theta: orientation }); + points.push({ x: position.x, y: position.y, angle: orientation }); } - + return points; } @@ -490,13 +519,13 @@ function solveForSide(angleA: number, angleB: number, sideB: number) { // Convert angles from degrees to radians const angleARad = angleA * (Math.PI / 180); const angleBRad = angleB * (Math.PI / 180); - + // Calculate the side opposite angleA using the Law of Sines const sideA = (sideB * Math.sin(angleARad)) / Math.sin(angleBRad); - + return sideA; - } - +} + function calculateCurveBetweenPoints(pointA: RobotPosition, pointB: RobotPosition) { const { x: x1, y: y1, angle: theta1Rad } = pointA; diff --git a/extensions/src/doodlebot/index.ts b/extensions/src/doodlebot/index.ts index bb7ff54c7..201c5b87c 100644 --- a/extensions/src/doodlebot/index.ts +++ b/extensions/src/doodlebot/index.ts @@ -5,6 +5,9 @@ import { splitArgsString } from "./utils"; import EventEmitter from "events"; import { categoryByGesture, classes, emojiByGesture, gestureDetection, gestureMenuItems, gestures, objectDetection } from "./detection"; //import { createLineDetector } from "./LineDetection"; +import { line0, line1, line2, line3, line4, line5, line6, line7, line8 } from './Points'; +import { followLine } from "./LineFollowing"; + const details: ExtensionMenuDisplayDetails = { name: "Doodlebot", description: "Program a doodlebot robot", @@ -109,7 +112,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator // } // this.lineDetector = createLineDetector(ipAddress); // } - + // const lineCoordinates = await this.lineDetector(); // if (lineCoordinates.length === 0) { // console.log("No line detected"); @@ -117,19 +120,19 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator // } // console.log("Line coordinates:", JSON.stringify(lineCoordinates)); - + // if (!this.videoDrawable) { // this.videoDrawable = await this.createVideoStreamDrawable(); // } - + // const canvas = document.createElement('canvas'); // canvas.width = this.imageStream.width; // Assume these properties exist // canvas.height = this.imageStream.height; // const ctx = canvas.getContext('2d'); - + // if (ctx) { // ctx.drawImage(this.imageStream, 0, 0, canvas.width, canvas.height); - + // ctx.beginPath(); // ctx.moveTo(lineCoordinates[0][0], lineCoordinates[0][1]); // for (let i = 1; i < lineCoordinates.length; i++) { @@ -138,7 +141,7 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator // ctx.strokeStyle = 'red'; // ctx.lineWidth = 2; // ctx.stroke(); - + // this.videoDrawable.update(canvas); // } } @@ -200,12 +203,119 @@ export default class DoodlebotBlocks extends extension(details, "ui", "indicator await this.doodlebot?.motorCommand("stop"); } + prependUntilTarget = (line) => { + const targetX = line[0][0]; + const targetY = line[0][1]; + const startX = line[0][0]; + const startY = 0; // Start slightly below targetY + + + const incrementX = 0.01; // Small step for x + let x = startX; + let y = startY; + + const newSegment = []; + while (y < targetY) { + newSegment.push([x, y]); + y += incrementX; // Increment y based on slope + } + + // Prepend the new segment to the beginning of line + line.unshift(...newSegment); + return line; + } + + async followLine() { + + const delay = 0.5; + const previousSpeed = 0.1; + + const lineData = [line0, line1, line2, line3, line4, line5, line6, line7, line8]; + + const beforeLine = this.prependUntilTarget(lineData[0]); + + let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], lineData[1], delay, previousSpeed, [], true); + console.log("here"); + for (const command of motorCommands) { + const { radius, angle } = command; + // await this.motorCommand( + // "steps", + // { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, + // { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } + // ); + console.log("command"); + console.log(command); + } + + + ({ motorCommands, bezierPoints, line } = followLine(line, lineData[1], lineData[2], delay, previousSpeed, motorCommands)); + for (const command of motorCommands) { + const { radius, angle } = command; + // await this.motorCommand( + // "steps", + // { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, + // { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } + // ); + console.log("command"); + console.log(command); + } + + ({ motorCommands, bezierPoints, line } = followLine(line, lineData[2], lineData[3], delay, previousSpeed, motorCommands)); + for (const command of motorCommands) { + const { radius, angle } = command; + // await this.motorCommand( + // "steps", + // { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, + // { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } + // ); + console.log("command"); + console.log(command); + } + + ({ motorCommands, bezierPoints, line } = followLine(line, lineData[3], lineData[4], delay, previousSpeed, motorCommands)); + for (const command of motorCommands) { + const { radius, angle } = command; + // await this.motorCommand( + // "steps", + // { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, + // { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } + // ); + console.log("command"); + console.log(command); + } + + ({ motorCommands, bezierPoints, line } = followLine(line, lineData[4], lineData[5], delay, previousSpeed, motorCommands)); + for (const command of motorCommands) { + const { radius, angle } = command; + // await this.motorCommand( + // "steps", + // { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, + // { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } + // ); + console.log("command"); + console.log(command); + } + + ({ motorCommands, bezierPoints, line } = followLine(line, lineData[5], lineData[6], delay, previousSpeed, motorCommands)); + for (const command of motorCommands) { + const { radius, angle } = command; + // await this.motorCommand( + // "steps", + // { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, + // { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } + // ); + console.log("command"); + console.log(command); + } + + } + @block({ type: "command", text: "test line follow" }) async testLine() { - await this.doodlebot?.followLine(); + await this.followLine(); } @block({