From 1ec554b38f1b097aafcf46f96d059cdb7594dcca Mon Sep 17 00:00:00 2001 From: Maya Rajan Date: Sun, 3 Nov 2024 11:38:33 -0500 Subject: [PATCH] testing on bot --- extensions/src/doodlebot/Doodlebot.ts | 183 +++++++++------------- extensions/src/doodlebot/LineFollowing.ts | 26 ++- extensions/src/doodlebot/LineHelper.ts | 2 +- extensions/src/doodlebot/Procrustes.ts | 8 +- 4 files changed, 102 insertions(+), 117 deletions(-) diff --git a/extensions/src/doodlebot/Doodlebot.ts b/extensions/src/doodlebot/Doodlebot.ts index 83b260e10..f74a5e808 100644 --- a/extensions/src/doodlebot/Doodlebot.ts +++ b/extensions/src/doodlebot/Doodlebot.ts @@ -4,6 +4,7 @@ import UartService from "./communication/UartService"; import { followLine } from "./LineFollowing"; import { Command, DisplayKey, NetworkStatus, ReceivedCommand, SensorKey, command, display, endpoint, keyBySensor, motorCommandReceived, networkStatus, port, sensor } from "./enums"; import { base64ToInt32Array, makeWebsocket, Max32Int, testWebSocket } from "./utils"; +import { line0, line1, line2, line3, line4, line5, line6, line7, line8 } from './Points'; export type Services = Awaited>; export type MotorStepRequest = { @@ -610,149 +611,111 @@ export default class Doodlebot { document.body.appendChild(image); // Example: Display the image in the document } + prependUntilTarget = (line) => { + const targetX = line[0][0]; + const targetY = line[0][1]; + const startX = line[0][0]; + const startY = 0; // Start slightly below targetY - async followLine() { - let line0; - let line1; - let line2; - let line3; - let line4; - let line5; - let line6; - line0=[[0.0, 0.0], [-17.104673497907527, 20.564281569525853], [-42.011478766789594, 39.423381796634736], [-75.92074377141333, 56.314965090186675], [-120.03279647654222, 70.97669585904111], [-164.14484918167207, 85.63842662789493], [-198.05411418629512, 102.53000992144658], [-222.9609194551773, 121.38911014855627], [-240.0655929530852, 141.95339171808214], [-250.56846264478227, 163.96051903888355], [-255.66985649503542, 187.14815651981806], [-256.5701024686096, 211.2539685697469], [-254.4695285302698, 236.015619597528], [-250.56846264478273, 261.17077401202096], [-246.06723277691222, 286.4570962220841], [-242.1661668914246, 311.6122506365771], [-240.06559295308506, 336.3739016643582], [-240.96583892665913, 360.4797137142863], [-246.06723277691242, 383.66735119522167], [-256.5701024686097, 405.6744785160225], [-273.674775966517, 426.2387600855489], [-298.5815812353995, 445.0978603126579], [-332.4908462400224, 461.98944360620965]] - // line0=[[-383.1296156102412, 1230.5622560465351], [-365.3300647514816, 1202.4185890994863], [-347.53051389272196, 1174.2749221524373], [-329.677510929282, 1146.0467396890313], [-285.5654582241526, 1131.3850089201762], [-251.65619321952863, 1114.4934256266242], [-226.74938795064645, 1095.6343253995153], [-209.64471445273944, 1075.0700438299891], [-199.1418447610422, 1053.0629165091882], [-194.040450910789, 1029.8752790282526], [-193.1402049372149, 1005.7694669783241], [-195.24077887555472, 981.007815950543], [-199.1418447610423, 955.8526615360504], [-203.64307462891315, 930.5663393259867], [-207.54414051440074, 905.4111849114944], [-209.64471445274, 880.649533883713], [-208.74446847916602, 856.5437218337844], [-203.64307462891315, 833.3560843528488], [-193.14020493721546, 811.348957032048], [-176.0355314393089, 790.7846754625223], [-151.1287261704265, 771.9255752354127], [-117.21946116580324, 755.0339919418606], [-73.10740846067358, 740.3722611730069], [-28.99535575554421, 725.7105304041522], [4.913909249078415, 708.8189471106002], [29.820714517961108, 689.9598468834912], [46.92538801586829, 669.3955653139649], [57.428257707565535, 647.3884379931638], [62.52965155781868, 624.2008005122286], [63.42989753139284, 600.0949884623001], [61.329323593053175, 575.3333374345189], [57.428257707565706, 550.1781830200262], [52.92702783969469, 524.8918608099631], [49.02596195420699, 499.73670639547066], [46.925388015868066, 474.97505536768887], [47.82563398944188, 450.8692433177603], [52.927027839694745, 427.68160583682493], [63.429897531392214, 405.6744785160235], [80.53457102929917, 385.1101969464983], [105.4413762981817, 366.2510967193885], [139.35064130280483, 349.3595134258366], [183.4626940079338, 334.6977826569824], [227.57474671306295, 320.0360518881285], [261.48401171768626, 303.1444685945765], [286.3908169865691, 284.2853683674667]] - line1=[[320.0, 0.0], [320.0, 23.742172418771816], [324.30030842448235, 47.4783887316922], [331.67226572359556, 71.21035068329016], [340.8872123474861, 94.93976001809521], [350.7164887463037, 118.66831848063619], [359.93143537019427, 142.3977278154408], [367.30339266930685, 166.12968976703922], [371.6037010937892, 189.86590607995961], [371.60370109378977, 213.60807849873117], [366.07473311945506, 237.35790876788278], [353.788137620935, 261.11709863194324], [333.51525504837565, 284.88734983544157], [304.0274258519256, 308.6703641229065], [264.09599048173254, 332.46784323886675], [224.1645551115397, 356.26532235482824], [194.6767259150903, 380.0483366422928], [174.4038433425307, 403.8185878457908], [162.11724784401008, 427.5777777098518], [156.58827986967603, 451.32760797900346]] - line2=[[320.0, 0.0], [320.0, 23.74217241877156], [314.4710320256647, 47.492002687923026], [302.1844365271441, 71.2511925519832], [281.9115539545842, 95.02144375548104], [252.42372475813357, 118.80445804294527], [212.49228938793993, 142.60193715890455], [172.56085401774652, 166.3994162748651], [143.07302482129654, 190.18243056232893], [122.80014224873636, 213.95268176582644], [110.51354675021517, 237.71187162988713], [104.98457877588055, 261.4617018990387], [104.98457877588001, 285.20387431781006], [109.2848872003618, 308.9400906307306], [116.65684449947389, 332.672052582329], [125.87179112336378, 356.4014619171344], [135.70106752218086, 380.1300203796749], [144.91601414607132, 403.8594297144797], [152.28797144518282, 427.59139166607855], [156.588279869665, 451.3276079789992]] - line3=[[320.0, 0.0], [320.0, 31.24123253135492], [326.069280541584, 57.2916330205665], [337.27410615681515, 78.94979101257925], [352.6807413777573, 97.01429605233866], [371.3554507364751, 112.28373768479007], [392.36449876503275, 125.55670545487848], [414.7741499954941, 137.63178890754958], [437.65066895992396, 149.30757758774695], [460.06032019038537, 161.38266104041722], [481.0693682189428, 174.65562881050636], [499.7440775776611, 189.9250704429576], [515.1507127986035, 207.98957548271702], [526.3555384138346, 229.64773347472993], [532.4248189554181, 255.6981339639407], [532.4248189554188, 286.93936649529616], [525.4218029458999, 324.17002061373967], [510.4820354589266, 368.1886858642177], [495.54226797195287, 412.20735111469503]] - line4=[[320.0, 0.0], [320.0, 25.45584625699707], [318.2810797223905, 50.246914922370934], [313.6972923154343, 73.9300209350388], [305.1026909273915, 96.06197923391805], [291.35132870652296, 116.19960475792656], [271.2972588010903, 133.8997124459814], [243.79453435935335, 148.71911723700137], [207.6972085295742, 160.21463406990244], [161.85933446001326, 167.94307788360373], [116.02146039045272, 175.67152169730434], [79.9241345606728, 187.16703853020582], [52.42141011893614, 201.98644332122558], [32.367340213503155, 219.68655100928046], [18.61597799263444, 239.82417653328918], [10.02137660459141, 261.95613483216806], [5.437589197636157, 285.63924084483597], [3.718668920026289, 310.43030951020984], [3.7186689200267438, 335.88615576720713], [4.2916423458953545, 361.56359455474563], [4.291642345894672, 387.01944081174275], [2.5727220682860548, 411.810509477117], [-2.011065338669596, 435.49361548978413], [-10.605666726712684, 457.6255737886634]] - line5=[[320.0, 0.0], [320.0, 46.484831321609256], [319.9999999999994, 92.969662643218], [325.3340819230948, 130.47581426337925], [335.37470671951024, 160.0596014410853], [349.49433533946905, 182.77733943533053], [367.0654287331962, 199.68534350510947], [387.4604478509146, 211.83992890941542], [410.0518536428496, 220.2974109072414], [434.2121070592242, 226.11410475758467], [459.3136690502629, 230.3463257194352], [484.72900056618926, 234.05038905179012], [509.83056255722755, 238.28261001364174], [533.9908159736027, 244.09930386398383], [556.5822217655369, 252.5567858618101], [576.9772408832557, 264.71137126611615], [594.5483342769828, 281.61937533589463], [608.667962896942, 304.3371133301405], [618.708587693357, 333.92090050784526], [624.0426696164516, 371.42705212800644], [624.0426696164528, 417.9118834496162]] - line6=[[320.0, 0.0], [320.0, 24.85058897676393], [321.760786046391, 50.24546525444484], [324.1085007749103, 75.82177063243151], [325.86928682130014, 101.21664691011227], [325.8692868213014, 126.06723588687649], [322.9346434106544, 150.0106793621108], [315.8914992250986, 172.68411913520555], [303.5659969003755, 193.72469700554865], [284.78427907222476, 212.76955477252855], [258.3724883763891, 229.45583423553302], [223.15676744860562, 243.42067719395098], [177.96325892461726, 254.30122544717358], [132.76975040062902, 265.18177370039484], [97.55402947284608, 279.1466166588124], [71.14223877701002, 295.83289612181727], [52.36052094885912, 314.8777538887973], [40.0350186241368, 335.91833175914024], [32.99187443857954, 358.59177153223476], [30.057231027931437, 382.53521500746945], [30.057231027933767, 407.385803984234], [31.81801707432402, 432.7806802619144]] - //line7=[[320.0, 0.0], [320.0, 26.74807529413604], [312.911198184821, 57.174437201259146], [297.6430096598178, 91.84497596952409], [273.1048495303509, 131.3255818470832], [248.5666894008832, 170.80618772464132], [233.2985008758801, 205.47672649290558], [226.2096990607011, 235.90308840002928], [226.2096990607011, 262.6511636941655], [232.20791598123856, 286.2868426234638], [243.11376492766777, 307.3760154360794], [257.8366610053482, 326.4845723801618], [275.2860193196383, 344.17840370386347], [294.3712549758911, 361.02339965533696], [314.0017830794657, 377.585450482735], [333.0870187357199, 394.4304464342091], [350.5363770500079, 412.1242777579095], [365.2592731276887, 431.2328347019926], [376.16512207411927, 452.3220075146087]] - - const delay = 0.5; - const previousSpeed = 0.1; + const incrementX = 0.01; // Small step for x + let x = startX; + let y = startY; - let commands2 = followLine(line0, delay, previousSpeed); + const newSegment = []; + while (y < targetY) { + newSegment.push([x, y]); + y += incrementX; // Increment y based on slope + } - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = 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); + // 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]; - commands2 = followLine(line1, delay, previousSpeed); + const beforeLine = this.prependUntilTarget(lineData[0]); - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = command; - await this.motorCommand( - "steps", - { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, - { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } - ); + let { motorCommands, bezierPoints, line } = followLine(beforeLine, lineData[0], delay, previousSpeed, []); + 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); - } - commands2 = followLine(line2, delay, previousSpeed); - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = command; - await this.motorCommand( - "steps", - { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, - { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } - ); + ({ 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); - - } - commands2 = followLine(line3, delay, previousSpeed); - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = command; - await this.motorCommand( - "steps", - { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, - { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } - ); + ({ 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); - } - commands2 = followLine(line4, delay, previousSpeed); - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = command; - await this.motorCommand( - "steps", - { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, - { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } - ); + ({ 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); - } - commands2 = followLine(line5, delay, previousSpeed); - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = command; - await this.motorCommand( - "steps", - { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, - { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } - ); + ({ 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); - } - commands2 = followLine(line6, delay, previousSpeed); - - for (const command of commands2) { - console.log(command); - const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = command; - await this.motorCommand( - "steps", - { steps: Math.round(leftWheelDistance), stepsPerSecond: Math.round(leftWheelSpeed) }, - { steps: Math.round(rightWheelDistance), stepsPerSecond: Math.round(rightWheelSpeed) } - ); + ({ 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); - } - // commands2 = followLine(line7, delay, previousSpeed); - - // for (const command of commands2) { - // console.log(command); - // const { leftWheelSpeed, rightWheelSpeed, leftWheelDistance, rightWheelDistance } = 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); - - // } - - } diff --git a/extensions/src/doodlebot/LineFollowing.ts b/extensions/src/doodlebot/LineFollowing.ts index 02153f3e8..43cd6b19d 100644 --- a/extensions/src/doodlebot/LineFollowing.ts +++ b/extensions/src/doodlebot/LineFollowing.ts @@ -309,10 +309,27 @@ function blendLines(entireLine: Point[], worldPoints: Point[], transitionLength: } -export function followLine(previousLine: Point[], pixels: Point[], delay: number, previousSpeed: number, previousCommands: {radius: number, angle: number}[]) { +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)); + 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; + 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}; for (const command of previousCommands) { @@ -341,11 +358,11 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number procrustesResult = procrustes(guessLine, worldPoints, 0.5); } else { // If the current frame doesn't contain that many points, just use previous guess - procrustesResult = { translation: [0, 0], rotation: 0 }; + procrustesResult = { translation: [0, 0], rotation: 0, distance: 0 }; } // Correct the guess of the previous line - let line = rotateCurve(guessLine.map((point: Point) => ({x: point[0], y: point[1]})), procrustesResult.rotation).map((point: number) => [point.x, point.y]); + 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); @@ -375,7 +392,8 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number const spline = new Spline.default(ys, xs); // Switch x and y so we no overlapping 'x' values // Find the end point for the Bezier curve - const distance = previousSpeed*delay + lookahead; + //const distance = previousSpeed*delay + lookahead; + const distance = distanceTest*0.9; const x1 = findPointAtDistanceWithIncrements(spline, 0.001, distance - .01); const x2 = findPointAtDistanceWithIncrements(spline, 0.001, distance); const point1 = {x: spline.at(x1), y: x1} diff --git a/extensions/src/doodlebot/LineHelper.ts b/extensions/src/doodlebot/LineHelper.ts index f2c104576..cdd48d2a2 100644 --- a/extensions/src/doodlebot/LineHelper.ts +++ b/extensions/src/doodlebot/LineHelper.ts @@ -1,7 +1,7 @@ export type Command = { radius: number, angle: number }; export type Point = number[]; export type RobotPosition = { x: number, y: number, angle: number }; -export type ProcrustesResult = { rotation: number, translation: number[] }; +export type ProcrustesResult = { rotation: number, translation: number[], distance: number }; export function distanceBetweenPoints(p1: Point, p2: Point): number { return Math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2); diff --git a/extensions/src/doodlebot/Procrustes.ts b/extensions/src/doodlebot/Procrustes.ts index b83930eb9..7db140142 100644 --- a/extensions/src/doodlebot/Procrustes.ts +++ b/extensions/src/doodlebot/Procrustes.ts @@ -7,7 +7,7 @@ import { rotateCurve } from 'curve-matcher'; -import { type Point, type ProcrustesResult, applyTranslation, distanceBetweenPoints } from './LineHelper'; +import { type Point, type ProcrustesResult, applyTranslation, distanceBetweenPoints, cutOffLineAtOverlap } from './LineHelper'; function calculateCentroid(line: Point[]) { @@ -141,8 +141,12 @@ export function procrustes(line1: Point[], line2: Point[], ratio=0.5): Procruste // Calculate the error between the most similar segments const { rotation, translation } = getError(maxLine, line2Filtered); + let rotatedCurve1 = rotateCurve(mapLine(line1), rotation); + let translatedLine1 = applyTranslation(mapCurve(rotatedCurve1), translation); - return {rotation, translation }; + const end = cutOffLineAtOverlap(translatedLine1, line2Filtered); + + return {rotation, translation, distance: end.distance }; }