diff --git a/extensions/src/doodlebot/Doodlebot.ts b/extensions/src/doodlebot/Doodlebot.ts index 07b30336a..e25f4cdc6 100644 --- a/extensions/src/doodlebot/Doodlebot.ts +++ b/extensions/src/doodlebot/Doodlebot.ts @@ -613,102 +613,25 @@ export default class Doodlebot { async followLine() { - const line = [ - [320.0, 0.0], - [321.18093016474705, 85.93089371628516], - [324.46129173348885, 155.591194051611], - [329.8474413179264, 210.9928561014725], - [337.7368075299604, 254.1478349613646], - [348.4625309811926, 287.0680857267824], - [361.20418428371824, 311.7655634932206], - [374.5270520484066, 330.2522233561744], - [387.18749088738452, 344.5400204111386], - [399.34185741311743, 356.6409097536084], - ].map(([x, y]) => [x, y * 1.5]); - - const line2 = [[0.0, 0.0], - [6.750959988805026, 28.656715258676463], - [16.80684538467081, 51.52899671303742], - [29.27087024849043, 69.52656205250135], - [43.24624864115695, 83.55912896648654], - [57.83619462356339, 94.53641514441152], - [72.14392225660285, 103.36813827569464], - [85.27264560116842, 110.96401604975448], - [96.3255787181531, 118.23376615600927], - [104.40593566845, 126.0871062838775]].map(([x, y]) => [ - x + 320, // Add 320 to x - y * 3 // Multiply y by 2 - ]); - - const line3 = [[0.0, 0.0], - [6.268865158534559, 20.97535705484649], - [15.20407690140092, 38.649805462755155], - [25.67096258758057, 53.6101410082584], - [36.53484957605513, 66.44315947588885], - [46.661065225806075, 77.73565665017801], - [54.91493689581503, 88.07442806521303], - [60.16179194506348, 98.04627025686385], - [61.26695773253297, 108.23797825832502], - [57.09576161720508, 119.2363481045747]].map(([x, y]) => [ - x + 320, // Add 320 to x - y * 3 // Multiply y by 2 - ]); - - const line4 = [[0.0, 0.0], - [4.263741167481272, 17.75353054629771], - [9.675681926862309, 33.672742232365366], - [14.965101388327154, 47.88832444264693], - [18.861278662060074, 60.53096656158614], - [20.093492858245157, 71.7313579736269], - [17.391023087066518, 81.62018806321313], - [9.48314845870837, 90.32814621478864], - [-4.900851916645138, 97.98592181279739], - [-27.0316989288099, 104.72420424168314]].map(([x, y]) => [ - x + 320, // Add 320 to x - y * 3 // Multiply y by 2 - ]); - - const line5 = [[0.0, 0.0], - [-2.458546532762881, 10.996484372490875], - [-8.215642996982728, 19.47870953494133], - [-18.51592857487068, 25.15912618527381], - [-34.604042448638, 27.750185021410845], - [-57.72462380049591, 26.964336741274786], - [-89.1223118126556, 22.514032042788193], - [-130.04174566732827, 14.111721623873494], - [-181.72756454672538, 1.4698561824531832], - [-245.42440763305805, 0.699113583550306]].map(([x, y]) => [ - x + 320, // Add 320 to x - y * 3 // Multiply y by 2 - ]); - - const line6 = [ - [0.0, 0.0], - [0.09236843533610564, -0.07283113197185509], - [0.1787644355172215, -0.21099708488163482], - [0.2613754803283964, -0.42708340844685555], - [0.34238904955468125, -0.7336756523850327], - [0.4239926229811255, -1.1433593664136825], - [0.5083736803927803, -1.6687201002503227], - [0.597719701574695, -2.3223434036124684] - - ].map(([x, y]) => [ - y * 100 + 320, // Add 320 to x - x * 100 * 3 // Multiply y by 2 - ]); - - const line7 = [[0.0, 0.0], [0.09236843533610564, -0.07283113197185509], [0.1787644355172215, -0.21099708488163482], [0.2613754803283964, -0.42708340844685555], [0.34238904955468125, -0.7336756523850327], [0.4239926229811255, -1.1433593664136825], [0.5083736803927803, -1.6687201002503227], [0.597719701574695, -2.3223434036124684]].map(([x, y]) => [ - y * 100 + 320, // Add 320 to x - x * 100 * 3 // Multiply y by 2 - ]) - - + let line0; + let line1; + let line2; + let line3; + let line4; + let line5; + let line6; + line0=[[320, 0], [320, 33.3], [320, 66.6], [320.0, 100.0], [290.55547455897363, 121.01450702944364], [270.9257909316227, 140.13821690036886], [259.9562618457501, 157.8072022603516], [256.49220002915877, 174.45753575696776], [259.3789182096516, 190.525290037793], [267.46172911503135, 206.44653775040322], [279.585945473101, 222.6573515423743], [294.5968800116634, 239.59380406128207], [311.33984545852167, 257.6919679547024], [328.66015454147833, 277.387915870211], [345.40311998833647, 299.11772045538373], [360.41405452689895, 323.3174543577966], [372.5382708849686, 350.42319022502517], [380.62108179034846, 380.87100070464555], [383.50779997084123, 415.09695844423345]] + line1=[[320.0, 0.0], [320.0, 33.400000000000006], [290.55547455897363, 54.41450702944364], [270.9257909316227, 73.53821690036887], [259.9562618457501, 91.20720226035161], [256.49220002915877, 107.85753575696776], [259.3789182096516, 123.92529003779302], [267.46172911503135, 139.84653775040323], [279.585945473101, 156.0573515423743], [294.5968800116634, 172.99380406128208], [311.33984545852167, 191.09196795470243], [328.66015454147833, 210.78791587021098], [345.40311998833647, 232.51772045538374], [360.41405452689895, 256.71745435779656], [372.5382708849686, 283.8231902250252], [380.62108179034846, 314.27100070464553], [383.50779997084123, 348.4969584442334], [380.0437381542499, 386.93713609136466], [369.07420906837734, 430.0276062936151]] + line2=[[320.0, 0.0], [311.8369232127996, 26.161134071432784], [310.5980932335586, 46.92140652387483], [315.47549899916805, 63.213866215870496], [325.6611294465186, 75.97156200596407], [340.346973512501, 86.12754275270002], [358.7250201340062, 94.61485731462274], [379.98725824792496, 102.36655455027662], [403.3256767911483, 110.315683318206], [427.9322647005667, 119.39529247695522], [452.9990109130711, 130.53843088506878], [477.71790436555244, 144.67814740109097], [501.2809339949014, 162.74749088356626], [522.8800887380089, 185.67951019103896], [541.7073575317658, 214.40725418205352], [556.9547293130627, 249.86377171515434], [567.8141930187907, 292.9821116488858], [573.4777375858403, 344.6953228417921]] + line3=[[320.0, 0.0], [327.9833280658778, 15.016650851980016], [340.4769463262831, 25.524522770015245], [356.8719083941727, 32.59722944079429], [376.5592678825036, 37.30838455100564], [398.93007840423263, 40.73160178733782], [423.3753935723168, 43.94049483647929], [449.28626699971255, 48.00867738511856], [476.05375229937704, 54.00976311994425], [503.06890308426705, 63.01736572764473], [529.7227729673393, 76.10509889490865], [555.4064155615508, 94.34657630842443], [579.5108844798585, 118.81541165488063], [601.4272333352191, 150.5852186209658], [620.5465157405895, 190.72961089336843], [636.2597853089262, 240.3222021587769]] + line4=[[320.0, 0.0], [328.3624555126027, 15.776162355216856], [340.77079630514635, 31.77054214536007], [356.078208581285, 48.439516590554405], [373.1378785446733, 66.23946291092469], [390.80299239896493, 85.62675832659549], [407.9267363478144, 107.05778005769183], [423.362296594876, 130.98890532433836], [435.96285934380376, 157.8765113466599], [444.5816107982521, 188.17697534478123], [448.07173716187526, 222.34667453882724], [445.28642463832733, 260.8419861489226], [435.07885943126274, 304.11928739519226], [416.3022277443355, 352.63495549776053]] + line5=[[320.0, 0.0], [323.69086129808085, 22.328213599044407], [328.2294799544362, 46.561896128944376], [332.4062433387309, 72.45548836044426], [335.01153882063034, 99.7634310642892], [334.83575376979974, 128.24016501122384], [330.6692755559043, 157.64013097199293], [321.30249154860934, 187.71776971734135], [305.52578911758, 218.227522018014], [282.1295556324815, 248.92382864475547], [249.9041784629792, 279.5611303683108], [207.64004497873827, 309.89386795942426]] + line6=[[320.0, 0.0], [319.6063583708792, 26.22534112814168], [317.41907209558354, 53.569939827233725], [312.2897285040425, 81.58145729752641], [303.0699149261853, 109.80755473927005], [288.6112186919413, 137.79589335271513], [267.7652271312397, 165.09413433811207], [239.38352757400992, 191.24993889571107], [202.31770735018122, 215.81096822576276], [155.4193537896831, 238.32488352851706]] const delay = 0.5; const previousSpeed = 0.1; - - let commands2 = followLine(line, delay, previousSpeed); + let commands2 = followLine(line0, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -723,7 +646,8 @@ export default class Doodlebot { } - commands2 = followLine(line2, delay, previousSpeed); + + commands2 = followLine(line1, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -736,9 +660,9 @@ export default class Doodlebot { console.log("command"); console.log(command); - } - commands2 = followLine(line3, delay, previousSpeed); + + commands2 = followLine(line2, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -751,8 +675,9 @@ export default class Doodlebot { console.log("command"); console.log(command); + } - commands2 = followLine(line4, delay, previousSpeed); + commands2 = followLine(line3, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -766,7 +691,7 @@ export default class Doodlebot { console.log(command); } - commands2 = followLine(line5, delay, previousSpeed); + commands2 = followLine(line4, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -780,8 +705,7 @@ export default class Doodlebot { console.log(command); } - - commands2 = followLine(line6, delay, previousSpeed); + commands2 = followLine(line5, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -796,7 +720,7 @@ export default class Doodlebot { } - commands2 = followLine(line7, delay, previousSpeed); + commands2 = followLine(line6, delay, previousSpeed); for (const command of commands2) { console.log(command); @@ -811,6 +735,21 @@ export default class Doodlebot { } + // 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 fdd6d1cef..4263975b3 100644 --- a/extensions/src/doodlebot/LineFollowing.ts +++ b/extensions/src/doodlebot/LineFollowing.ts @@ -9,48 +9,62 @@ const cameraMatrix = [ [0, 0, 1] // 0, 0, 1 ]; -const wheelBase = 0.0816; -const bezierSamples = 4; -const bezierIncrement = .05; +const wheelBase = 0.0716; +const bezierSamples = 3; +const bezierIncrement = .01; const linearSpeed = .1; +const delay = 0.5; +const previousSpeed = 0.1; -export function followLine(linePixels: number[][], delay: number, previousSpeed: number) { + +export function followLine(linePixels: number[][], delay1: number, previousSpeed1: number) { let increasing = true; - const filteredLinePixels = linePixels.filter((pixel, index, array) => { - // Skip the first element, as there's no previous element to compare with - if (index === 0) return true; - - const prevY = array[index - 1][1]; - const currentY = pixel[1]; - if (currentY < prevY) { - increasing = false; - } - // Keep the pixel if the y-value is increasing - return increasing; - }); - const xs = filteredLinePixels.map((point) => point[0]); - const ys = filteredLinePixels.map((point) => point[1]); + // const filteredLinePixels = linePixels.filter((pixel, index, array) => { + // // Skip the first element, as there's no previous element to compare with + // if (index === 0) return true; + + // const prevY = array[index - 1][1]; + // const currentY = pixel[1]; + // if (currentY < prevY) { + // increasing = false; + // } + // // Keep the pixel if the y-value is increasing + // return increasing; + // }); + const xs = linePixels.map((point) => point[0]); + const ys = linePixels.map((point) => point[1]); const spline = new Spline.default(ys, xs); // Opposite so we get the x values - const x1 = findPointAtDistanceWithIncrements(spline, 0.1, (previousSpeed * delay) - bezierIncrement - 0.05); - const x2 = findPointAtDistanceWithIncrements(spline, 0.1, (previousSpeed * delay)); + const x1 = findPointAtDistanceWithIncrements(spline, 0.01, (previousSpeed * delay * 2)*0.75); + const x2 = findPointAtDistanceWithIncrements(spline, 0.01, (previousSpeed * delay * 2)); + + const x3 = findPointAtDistanceWithIncrements(spline, 0.01, (previousSpeed * delay)); + - console.log(x1); - console.log(x2); - // console.log(spline); - const groundCoordinate1 = pixelToGroundCoordinates([spline.at(x1), x1]); - const groundCoordinate2 = pixelToGroundCoordinates([spline.at(x2), x2]); + + console.log("X"); + console.log(x1); + console.log(x2); + console.log((previousSpeed * delay)*0.5); + // console.log(spline); + // const groundCoordinate1 = pixelToGroundCoordinates([spline.at(x1), x1]); + // const groundCoordinate2 = pixelToGroundCoordinates([spline.at(x2), x2]); + // const baseCoordinate = pixelToGroundCoordinates([spline.at(x3), x3]) + const groundCoordinate1 = pixelToGroundCoordinates([spline.at(spline.xs[2]*0.75), spline.xs[2]*0.75]); + const groundCoordinate2 = pixelToGroundCoordinates([spline.at(spline.xs[2]), spline.xs[2]]); + const baseCoordinate = pixelToGroundCoordinates([spline.at(spline.xs[0]), spline.xs[0]]) console.log("ground coordinates"); - console.log(groundCoordinate1.x * 100, groundCoordinate1.y * 100); - console.log(groundCoordinate2.x * 100, groundCoordinate2.y * 100); + console.log(groundCoordinate1.x*100, groundCoordinate1.y*100); + console.log(groundCoordinate2.x*100, groundCoordinate2.y*100); + console.log(baseCoordinate); const bezier = new Bezier.Bezier( - { x: 0, y: 0 }, - { x: 0, y: bezierIncrement }, - groundCoordinate1, - groundCoordinate2 + { x: baseCoordinate.x, y: baseCoordinate.y }, + { x: baseCoordinate.x, y: baseCoordinate.y + bezierIncrement }, + groundCoordinate1, + groundCoordinate2 ); console.log("bezier") console.log(bezier); @@ -58,53 +72,42 @@ export function followLine(linePixels: number[][], delay: number, previousSpeed: const motorCommands = []; // TODO: Improve this function const bezierPoints = bezierCurvePoints(bezier, bezierSamples); - console.log(bezierPoints); + //console.log(bezierPoints); for (let i = 0; i < bezierPoints.length - 1; i++) { - const command = purePursuit(bezierPoints[i], bezierPoints[i + 1]); - motorCommands.push(command); + //console.log(bezierPoints[i], bezierPoints[i+1]); + const command = purePursuit(bezierPoints[i], bezierPoints[i+1]); + motorCommands.push(command); } return motorCommands; } -// function calculateDistanceOnCurve(curve: Spline, t0: number, t1: number) { -// const numSteps = 100; -// let distance = 0; -// let prevPoint = pixelToGroundCoordinates([curve.at(t0), t0]); -// // console.log('prev point'); -// // console.log(prevPoint); -// for (let i = 1; i <= numSteps; i++) { -// const t = t0 + (t1 - t0) * i / numSteps; -// const currentPoint = pixelToGroundCoordinates([curve.at(t), t]); -// // console.log("current point"); -// // console.log(currentPoint); -// distance += Math.sqrt( -// Math.pow(currentPoint.x - prevPoint.x, 2) + -// Math.pow(currentPoint.y - prevPoint.y, 2) -// ); -// prevPoint = currentPoint; -// console.log(currentPoint); -// console.log(distance); -// } -// return distance; -// } - -// Spline function that gives the y value for a given x value - // Function to calculate the Euclidean distance between two points function distanceBetweenPoints(x1, y1, x2, y2) { + //console.log(x1, x2, y1, y2); const ground1 = pixelToGroundCoordinates([x1, y1]); const ground2 = pixelToGroundCoordinates([x2, y2]); + // console.log("calulcating"); + // console.log(ground1); + // console.log(ground2); const dx = ground2.x - ground1.x; const dy = ground2.y - ground2.y; + if (isNaN(ground1.x) || isNaN(ground2.x) || isNaN(ground1.y) || isNaN(ground2.y)) { + console.log(ground1); + console.log(ground2); + console.log(x2, y2); + console.log("hmmm"); + + } return Math.sqrt(dx * dx + dy * dy); } -function findPointAtDistanceWithIncrements(spline, increment: number, desiredDistance: number): number { +function findPointAtDistanceWithIncrements(spline: Spline, increment: number, desiredDistance: number): number { let totalDistance = 0; const xValues = spline.xs; + console.log(xValues); // Iterate through each pair of xValues in the array for (let i = 0; i < xValues.length - 1; i++) { @@ -114,9 +117,10 @@ function findPointAtDistanceWithIncrements(spline, increment: number, desiredDis // Check the direction between currentX and nextX (allow for backtracking) let direction = nextX > currentX ? 1 : -1; + console.log(direction); // Step through each segment in increments, adjusting for direction while ((direction === 1 && currentX < nextX) || (direction === -1 && currentX > nextX)) { - + const nextXIncrement = currentX + direction * increment; // Increment or decrement by step size const currentY = spline.at(currentX); @@ -125,12 +129,22 @@ function findPointAtDistanceWithIncrements(spline, increment: number, desiredDis // console.log(currentX, currentY); // Calculate distance between current and next increment + if (isNaN(nextY)) { + console.log( + "MMM" + ); + console.log(totalDistance); + console.log(nextXIncrement); + } const distance = distanceBetweenPoints(currentX, currentY, nextXIncrement, nextY); totalDistance += distance; // Check if the accumulated distance is equal to or exceeds the desired distance + if (totalDistance >= desiredDistance) { + console.log("TOTAL DISTANCE"); + console.log(totalDistance); return nextXIncrement; } @@ -145,127 +159,71 @@ function findPointAtDistanceWithIncrements(spline, increment: number, desiredDis } // If the desired distance is beyond all xValues, return the last point + console.log("TOTAL DISTANCE"); + console.log(totalDistance); return xValues[xValues.length - 1]; } -// Example usage - - - -// function findPointOnCurve(curve: Spline, t0: number, desiredDistance) { -// let low = t0; -// let high = imageDimensions[1]; // Assuming y ranges from 0 to image height -// let mid: number; // y value - -// while (high - low > 0.0001) { -// mid = (low + high) / 2; -// const distance = calculateDistanceOnCurve(curve, t0, mid); - -// if (distance < desiredDistance) { -// low = mid; // Increase y -// } else { -// high = mid; // Decrease y -// } -// } - -// return mid; -// } -// function pixelToGroundCoordinates( -// pixelCoords: [number, number], -// ): { x: number, y: number, distance: number } { -// // Convert angle from degrees to radians -// const angleRad = tiltAngle * (Math.PI / 180); -// // Extract intrinsic matrix parameters -// const fx = cameraMatrix[0][0]; // Focal length in x -// const fy = cameraMatrix[1][1]; // Focal length in y -// const cx = cameraMatrix[0][2]; // Principal point x -// const cy = cameraMatrix[1][2]; // Principal point y -// // Pixel coordinates -// const [px, py] = pixelCoords; -// // Compute the depth of the pixel point in camera coordinates -// const z = cameraHeight / Math.sin(angleRad); - -// // Compute the normalized image coordinates -// const xNormalized = (px - cx) / fx; -// const yNormalized = (py - cy) / fy; - -// // Compute the ground coordinates relative to the camera -// const xGround = xNormalized * z; -// const yGround = yNormalized * z; - -// const perspectiveCorrection = Math.cos(angleRad); // The correction factor - -// // Compute the distances in x and y directions on the ground -// // The component here is used to adjust the distances based on the height and angle -// const xDistance = Math.sqrt(xGround/perspectiveCorrection ** 2 + (cameraHeight ** 2) / (Math.sin(angleRad) ** 2)); -// const yDistance = Math.sqrt(yGround/perspectiveCorrection ** 2 + (cameraHeight ** 2) / (Math.sin(angleRad) ** 2)); - -// // Compute the total distance to the ground point from the point directly below the camera -// const totalDistance = Math.sqrt(xDistance ** 2 + yDistance ** 2); - -// return { -// x: xDistance, -// y: yDistance, -// distance: totalDistance -// }; -// } - -const imageDimensions = [640, 480]; +const imageDimensions = [640,480]; const horizontalFOV = 53.5; const verticalFOV = 41.41; -const cameraHeight = 0.1; -const tiltAngle = 32.85331330197821; - -function pixelToGroundCoordinates( - pixelCoords: [number, number], +// const horizontalFOV = 50; +// const verticalFOV = 50; +const cameraHeight = 0.1; +//const tiltAngle = 52.85331330197821; +//const tiltAngle = 32.85331330197821; +const tiltAngle = 30; + +export function pixelToGroundCoordinates( + pixelCoords: [number, number], ): { x: number, y: number } { - 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); - const angleZ = 180 - angleP - angleY; - const sideA = solveForSide(tiltAngle, 90, sideB); - 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 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); - - return { x: horizontalOffset, y: verticalOffset }; + 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); + const angleZ = 180 - angleP - angleY; + const sideA = solveForSide(tiltAngle, 90, sideB); + 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 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); + + return { x: horizontalOffset, y: verticalOffset}; } function bezierCurvePoints(bezier: Bezier, n: number) { const points = []; for (let i = 0; i <= n; i++) { - const t = i / n; - const position = bezier.get(t); - const derivative = bezier.derivative(t); - const orientation = Math.atan2(derivative.y, derivative.x); - points.push({ x: position.x, y: position.y, theta: orientation }); + const t = i / n; + const position = bezier.get(t); + const derivative = bezier.derivative(t); + const orientation = Math.atan2(derivative.y, derivative.x); + points.push({ x: position.x, y: position.y, theta: orientation }); } - + return points; } -function solveForSide(angleA, angleB, sideB) { +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 = Math.sin(angleBRad) == 0 ? 0 : (sideB * Math.sin(angleARad)) / Math.sin(angleBRad); - + const sideA = (sideB * Math.sin(angleARad)) / Math.sin(angleBRad); + return sideA; } @@ -289,35 +247,35 @@ function metersToSteps(distanceMeters: number) { return steps; } -function purePursuit(currentPoint, lookaheadPoint) { +function purePursuit(currentPoint: {x: number, y: number, theta: number}, lookaheadPoint: {x: number, y: number, theta: number}) { if (!lookaheadPoint) { - return { leftWheelSpeed: 0, rightWheelSpeed: 0 }; // No valid lookahead point found + return { leftWheelSpeed: 0, rightWheelSpeed: 0 }; // No valid lookahead point found } const dx = lookaheadPoint.x - currentPoint.x; const dy = lookaheadPoint.y - currentPoint.y; - + const lookaheadAngle = lookaheadPoint.theta; const robotHeading = currentPoint.theta; // Robot's orientation (angle) - + const angleToLookahead = lookaheadAngle - robotHeading; const lookaheadDistanceToPoint = Math.sqrt(dx * dx + dy * dy); - + const curvature = (2 * Math.sin(angleToLookahead)) / lookaheadDistanceToPoint; const angularVelocity = linearSpeed * curvature; const leftWheelDistance = metersToSteps(lookaheadDistanceToPoint * (1 - (wheelBase * curvature) / 2)); const rightWheelDistance = metersToSteps(lookaheadDistanceToPoint * (1 + (wheelBase * curvature) / 2)); - + // Compute wheel speeds - const leftWheelSpeed = metersToSteps(linearSpeed - (wheelBase / 2) * angularVelocity); - const rightWheelSpeed = metersToSteps(linearSpeed + (wheelBase / 2) * angularVelocity); - + const leftWheelSpeed = Math.abs(metersToSteps(linearSpeed - (wheelBase / 2) * angularVelocity)); + const rightWheelSpeed = Math.abs(metersToSteps(linearSpeed + (wheelBase / 2) * angularVelocity)); + return { - leftWheelSpeed, - rightWheelSpeed, - leftWheelDistance, - rightWheelDistance + leftWheelSpeed, + rightWheelSpeed, + leftWheelDistance, + rightWheelDistance }; }