Skip to content

Commit

Permalink
oooooh
Browse files Browse the repository at this point in the history
  • Loading branch information
@mayarajan3
mayarajan3 committed Nov 21, 2024
1 parent ce2cbc9 commit 1c62aa6
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 40 deletions.
72 changes: 47 additions & 25 deletions extensions/src/doodlebot/Doodlebot.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -550,6 +550,7 @@ export default class Doodlebot {
async connectionWorkflow(credentials: NetworkCredentials) {
await this.connectToWifi(credentials);
await this.connectToWebsocket(this.connection.ip);
this.detector = new LineDetector(this.connection.ip);
//await this.connectToImageWebSocket(this.connection.ip);
}

Expand Down Expand Up @@ -636,36 +637,57 @@ export default class Doodlebot {



i = 0;
motorCommands;
bezierPoints;
line;
lineCounter = 0;
detector;
async followLine() {

let first = true;
const delay = 0.5;
const previousSpeed = 0.1;
const detector = new LineDetector(this.connection.ip);
let motorCommands, bezierPoints, line;
const intervalId = setInterval(async () => {
try {
const lineData = await detector.detectLine();
console.log(this.connection.ip);
console.log("LINE DATA", 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

} catch (error) {
console.error("Error detecting line:", error);
// Optionally stop polling if there's a consistent error
clearInterval(intervalId);
}
}, 1000 / 15); // 66.67 ms interval

let lineString = "";
const sleep = (ms) => new Promise((resolve) => setTimeout(resolve, ms));
// for (let j = 0; j < 20; j++) {
// try {
// await sleep(1000);
console.log("before 1");
let lineData = await this.detector.detectLine();
console.log("after 1");
//lineData = lineData.map(pixel => [640 - pixel[0], 480 - pixel[1]])
lineData = lineData.sort((a, b) => a[1] - b[1]);
//const lineDataString = `export const line${lineCounter} = ${JSON.stringify(lineData)};\n`;
//lineString += lineDataString;
//console.log(lineString);
//lineCounter += 1;
console.log(this.connection.ip);
console.log("LINE DATA", lineData);
console.log("before");
if (first) {
({ motorCommands: this.motorCommands, bezierPoints: this.bezierPoints, line: this.line } = followLine(lineData, lineData, null, delay, previousSpeed, [], [], [], false, true));
first = false;
} else {
({ motorCommands: this.motorCommands, bezierPoints: this.bezierPoints, line: this.line } = followLine(this.line, lineData, null, delay, previousSpeed, this.motorCommands, [1], [1], false, false));
}
console.log("after");
for (const command of this.motorCommands) {
const { radius, angle } = command;
console.log(command);

this.motorCommand("arc", radius, angle);
}
console.log("after 2");
//console.log(j);
// Process the line data here

// } catch (error) {
// console.error("Error detecting line:", error);
// // Optionally stop polling if there's a consistent error
// }
// }

}

Expand Down
48 changes: 33 additions & 15 deletions extensions/src/doodlebot/LineFollowing.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,15 @@ import { type Point, type PointObject, type ProcrustesResult, type RobotPosition
const maxDistance = 100;
const epsilon = 0.3;
const bezierSamples = 2;
const controlLength = .02;
const controlLength = .01;
const lookahead = 0.06;

const imageDimensions = [640, 480];
const horizontalFOV = 53.4;
const verticalFOV = 41.41;
const cameraHeight = 0.098;
//const tiltAngle = 41.5;
const tiltAngle = 40;
const tiltAngle = 25;
//const tiltAngle = 38;

function cutOffLineOnDistance(line: Point[], maxDistance: number) {
let filteredLine = [line[0]]; // Start with the first point
Expand Down Expand Up @@ -351,11 +351,15 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]

}


let worldPoints = simplifyLine(pixels, epsilon, 0.1);
worldPoints = smoothLine(worldPoints, 2);
worldPoints = cutOffLineOnDistance(worldPoints.filter((point: Point) => point[1] < 370), maxDistance);
worldPoints = worldPoints.map(point => pixelToGroundCoordinates(point));

//console.log(worldPoints);
if (worldPoints[0] == undefined) {
worldPoints = [];
}
worldPoints = worldPoints.length > 0 ? worldPoints.map(point => pixelToGroundCoordinates(point)) : [];

if (first) {
previousLine = prependUntilTarget(worldPoints);
Expand Down Expand Up @@ -385,13 +389,19 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
}
}

console.log("here 4");
// 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]))
let segment1 = showLineAboveY(guessLine, Math.max(worldPoints.length > 0 ? worldPoints[0][1] : 0, guessLine[0][1]));
let segment2;
if (worldPoints.length > 0) {
segment2 = showLineBelowY(worldPoints, Math.min(guessLine[guessLine.length - 1][1], worldPoints[worldPoints.length - 1][1]))
} else {
segment2 = [];
}

// Distance of the world line
let worldDistance = 0;
Expand Down Expand Up @@ -419,7 +429,8 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
// Apply the Procrustes transformation
let result = procrustes(guessLine, worldPoints, scale);
// Calculate cumulative error
let guessLine2 = rotateCurve(guessLine.map(point => ({ x: point[0], y: point[1] })), result.rotation).map((point: { x: number, y: number }) => [point.x, point.y]);
//TODO
let guessLine2 = rotateCurve(guessLine.map(point => ({ x: point[0], y: point[1] })), result.rotation)?.map((point: { x: number, y: number }) => [point.x, point.y]);
guessLine2 = applyTranslation(guessLine2, result.translation);
guessLine2 = showLineAboveY(guessLine2, 0);
let cumulativeError = calculateLineError(worldPoints, guessLine2)
Expand Down Expand Up @@ -449,7 +460,7 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
}

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();
Expand All @@ -472,7 +483,7 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
if (test) {
distance = distanceTest * 0.8;
} else {
distance = previousSpeed * delay + lookahead;
distance = lookahead;
}

const x1 = findPointAtDistanceWithIncrements(spline, 0.001, distance - .01);
Expand All @@ -497,30 +508,35 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
if (test) {
x3 = spline.xs[0];
} else {
x3 = previousSpeed * delay;
x3 = spline.xs[0];
}
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]
console.log("TRANSLATION", procrustesResult);

let xOffset: number;
if (test) {
xOffset = 0;
} else {
xOffset = reference1[0] - reference2[0];
xOffset = (reference1[0] - reference2[0]);
//xOffset = 0;
}
console.log("X OFFSET", xOffset);

// 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
const bezier = new Bezier.Bezier(
{ x: point3.x - xOffset, y: point3.y },
{ x: point3.x - xOffset, y: point3.y + controlLength },
{ x: point3.x + xOffset, y: point3.y },
{ x: point3.x + xOffset, y: point3.y + controlLength },
extendedPoint1,
point2
);

console.log("BEZIER", bezier);

const motorCommands: Command[] = [];

// // Split the Bezier curve into a series of arcs
Expand All @@ -532,8 +548,10 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
// }

const command = createArcFromPoints(bezier.points[0], bezier.get(0.5), bezier.points[3]);
//command.angle = command.angle * -1;
motorCommands.push(command);


return { motorCommands, bezierPoints, line };

}
Expand Down Expand Up @@ -646,7 +664,7 @@ function createArcFromPoints(P1: PointObject, P2: PointObject, P3: PointObject):

return {
radius,
angle: sweepAngle * -1 * 2,
angle: sweepAngle * -1,
distance: 0
};
}
Expand Down

0 comments on commit 1c62aa6

Please sign in to comment.