diff --git a/public/calculateActions.js b/public/calculateActions.js index 3580f41..6cec0c3 100755 --- a/public/calculateActions.js +++ b/public/calculateActions.js @@ -50,7 +50,7 @@ async function readValues( data, headers, rows ){ } -async function calculateAngleFromPosition(row, axisY, axisZ, deltaYangle0, deltaZangle0 = 0) { +function calculateAngleFromPosition(row, axisY, axisZ, deltaYangle0, deltaZangle0 = 0) { let y = parseFloat(row.y_mm); let z = parseFloat(row.z_mm); @@ -66,10 +66,26 @@ async function calculateAngleFromPosition(row, axisY, axisZ, deltaYangle0, delta angleDeg = angleRad * (180 / Math.PI); appendToAnalysis(`(yMotor = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad ) aus Position von ID = ${row.id}`); + return angleRad; } -async function calculateAngleFromRollColumn(row, roll0 = 0, pitch0 = 0, yaw0 = 0) { + +function calculateAngleFromRollColumn(row, roll0 = 0, pitch0 = 0, yaw0 = 0, strMotor = "yMotor") { let roll = -parseFloat(row.roll_deg) + roll0; - appendToAnalysis(`(yMotor = ${roll.toFixed(2)}° = ${(roll * Math.PI / 180).toFixed(4)} rad) aus roll_deg von ID = ${row.id}`); + appendToAnalysis(`(${strMotor} = ${roll.toFixed(2)}° = ${(roll * Math.PI / 180).toFixed(4)} rad) aus roll_deg von ID = ${row.id}`); + return roll*Math.PI / 180; +} + +function calculateAngleFromRelativePosition(row1, row2, strMotor = "zMotor") { + let y1 = parseFloat(row1.y_mm); + let z1 = parseFloat(row1.z_mm); + let y2 = parseFloat(row2.y_mm); + let z2 = parseFloat(row2.z_mm); + let dy = y1 - y2; + let dz = z1 - z2; + let angleRad = -Math.atan(dz/dy); + let angleDeg = angleRad * (180 / Math.PI); + appendToAnalysis(`(${strMotor} = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad) aus relativer Position von ID = ${row1.id} und ID = ${row2.id}`); + return angleRad; } async function calculate() { @@ -92,21 +108,64 @@ async function calculate() { } - const row243 = rows.find(r => r.id == 243) + // Oberarm: + var angleY = 0; + var angleZ = 0; + var angleYcount = 0; + var angleZcount = 0; + + // 243 damit 35mm weiter außen (250+35) und 0mm höher als Schulterachse + const row243 = rows.find(r => r.id == 243 && r.seen_by == 3) if(row243){ - const angle = await calculateAngleFromPosition(row243, shoulderAxisY, shoulderAxisZ, 250+35, 0); - calculateAngleFromRollColumn(row243, 90, 0, 0); + angleY += await calculateAngleFromPosition(row243, shoulderAxisY, shoulderAxisZ, 250+35, 0); + angleY += await calculateAngleFromRollColumn(row243, 90, 0, 0); + angleYcount+=2; } - const row229 = rows.find(r => r.id == 229) + const row229 = rows.find(r => r.id == 229 && r.seen_by == 3) if(row229){ - const angle = await calculateAngleFromPosition(row229, shoulderAxisY, shoulderAxisZ, 250, 35); - calculateAngleFromRollColumn(row229, 0, 0, 0); + angleY += await calculateAngleFromPosition(row229, shoulderAxisY, shoulderAxisZ, 250, 35); + angleY +=calculateAngleFromRollColumn(row229, 0, 0, 0); + angleYcount+=2; + } + console.log(angleY, angleYcount); + const row198 = rows.find(r => r.id == 198 && r.seen_by == 3) + if(row198){ + angleY += await calculateAngleFromPosition(row198, shoulderAxisY, shoulderAxisZ, 165, 35); + //angleY +=calculateAngleFromRollColumn(row198, 180, 0, 0); // ist ungenau + angleYcount++; } - const row203 = rows.find(r => r.id == 203) - if(row203){ - const angle = await calculateAngleFromPosition(row203, shoulderAxisY, shoulderAxisZ, 165, 35); - calculateAngleFromRollColumn(row203, 0, 0, 0); + if(row198 && row229){ + angleY += 3*(calculateAngleFromRelativePosition(row198, row229, "yMotor")); + angleYcount += 3; + } + + // Unterarm: + + // 218 und 219, wenn die sichtbar sind, ist auch die Schulter eindeutig definiert + const row218 = rows.find(r => r.id == 218 && r.seen_by == 3) + const row219 = rows.find(r => r.id == 219 && r.seen_by == 3) + if(row218 && row219){ + const lowerArmAngle = calculateAngleFromRelativePosition(row218, row219, "zMotor"); + angleZ += lowerArmAngle; + angleZcount++; + } + console.log("z", angleZ); + + const row226 = rows.find(r => r.id == 226 && r.seen_by == 3) + if(row226){ + angleZ += calculateAngleFromRollColumn(row226, 0, 0, 0, "zMotor"); + angleZcount++; + } + + console.log("z", angleZ); + + if(angleYcount > 0 && angleYcount > 0){ + // appendToAnalysis(`Suggestion: G92 y${(angleY/angleYcount).toFixed(3)} z${(angleZ/angleZcount).toFixed(3)} (Set to coordinats. Angles in rad)`); + strActionOptionA = `G92 y${(angleY*180/(angleYcount*Math.PI)).toFixed(1)} z${(angleZ*180/(Math.PI*angleZcount)).toFixed(1)} (Set Coord. Angles in deg)`; + appendToAnalysis(`Suggestion: ${strActionOptionA}`); + + // ToDo: Change } }