README.md

@@ -5,9 +5,6 @@ Code de la tourelle de pointage laser du projet bouchons EDF.
 > [!WARNING]
 > Code de debuggage uniquement.
 
-> [!IMPORTANT]
-> Nouvelle méca, les axes H/x et V/y sont découplés.
-
 # Usage
 
 Compile through Arduino IDE or equivalent.

config.h

@@ -14,15 +14,15 @@
 #define PIN_PD2 (2)
 #define PIN_MOSI (11)
 
-#define PIN_Y_PULSE (PIN_PB0)
+#define PIN_X_PULSE (PIN_PB0)
-#define PIN_Y_ENABLE (PIN_PD6)
+#define PIN_X_ENABLE (PIN_PD6)
-#define PIN_Y_DIRECTION (PIN_PD7)
+#define PIN_X_DIRECTION (PIN_PD7)
-#define PIN_Y_HOME (PIN_PD3)
+#define PIN_X_HOME (PIN_PD3)
 
-#define PIN_X_PULSE (PIN_PB2)
+#define PIN_Y_PULSE (PIN_PB2)
-#define PIN_X_ENABLE (PIN_PB1)
+#define PIN_Y_ENABLE (PIN_PB1)
-#define PIN_X_DIRECTION (PIN_MOSI)
+#define PIN_Y_DIRECTION (PIN_MOSI)
-#define PIN_X_HOME (PIN_PD4)
+#define PIN_Y_HOME (PIN_PD4)
 
 #define PIN_LASER (PIN_PD2)
 

maths.cpp

@@ -1,16 +1,24 @@
 #include "math.h"
 
+// rollH atan((22.5 - 18.5) / 236) ~0.97°
+// rollV ~1°
+
 double degToRad(double deg) { return deg * M_PI / 180.0; }
 
 double radToDeg(double rad) { return rad * 180.0 / M_PI; }
 
 void angleToStep(long &stepX, long &stepY, double angleX, double angleY) {
   // TODO remove magic numbers
-  double stepRatioX = 6000 / M_PI;
+  double stepRatioX = M_PI / ((51.2 / 15.0) * (100.0 * 64.0));
-  double stepRatioY = 6500 / M_PI;
+  double stepRatioY = M_PI / ((48.0 / 15.0) * (100.0 * 64.0));
 
-  stepX = angleX * stepRatioX;
+  stepX = angleX / stepRatioX;
-  stepY = angleY * stepRatioY;
+  stepY = angleY / stepRatioY;
+}
+
+double rollCorrection(double x, double max, double width) {
+  double correction = max * (1.0 - cos((10.0 * x) / (width * M_PI))) / 2.0;
+  return x - correction;
 }
 
 void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
@@ -20,14 +28,21 @@ void cartesianToPolar(long &stepX, long &stepY, double zeroX, double zeroY,
   double offsetY = 21.0;   // laser to stand
   double offsetX = 0.0;    // unused
 
-  // x = -x; // natural axis direction
+  x = -x; // natural axis direction
-  y = -y; // natural axis direction
   double dX = x + offsetX;
-  double dY = y + offsetY;
+  double dY = rollCorrection(y + offsetY, 0.2, 400.0);
   double dZ = z + distance;
 
-  double angleY = atan2(dZ, dY) - M_PI / 2;
+  double roll = atan2(4.0, 236.0);
-  double angleX = atan2(dZ, dX) - M_PI / 2;
+  double rollY = dX * tan(roll);
+  double rollX = dY * tan(roll);
+
+  double dXr = dX + rollX;
+  double dYr = dY + rollY;
+
+  double rho = sqrt(pow(dXr, 2.0) + pow(dYr, 2.0) + pow(dZ, 2.0));
+  double angleY = M_PI / 2.0 - acos(dYr / rho);
+  double angleX = atan2(dXr, dZ);
 
   angleToStep(stepX, stepY, angleX, angleY);
 }

turret.cpp

@@ -74,8 +74,8 @@ Turret &Turret::gotoHome() {
 
 Turret &Turret::gotoZero() {
   // TODO remove magic numbers
-  _stepperX.prepareMove(_homeX + 0);
+  _stepperX.prepareMove(_homeX + 9300);
-  _stepperY.prepareMove(_homeY + 2500);
+  _stepperY.prepareMove(_homeY + 3500);
 
   bool xStop = false;
   bool yStop = false;

turret_debug.ino

@@ -10,7 +10,8 @@ void setup() {
 
   Serial.begin(BAUDRATE);
 
-  turret.laserOn().calibrate().moveTo(0, 0, 0);
+  turret.laserOn().calibrate();
+  turret.moveTo(0, 0, 0);
   delay(5000);
 
   // test pointage
@@ -19,22 +20,74 @@ void setup() {
   int panelHCount = 3;
   int panelVCount = 2;
 
-  double zeroOffsetV = -21;
+  // double zeroOffsetV = -31.5;
-  double zeroOffsetH = -163;
+  double zeroOffsetV = -20.8;
-  // double zeroOffsetH = -147;
+  // double zeroOffsetH = -184.5;
+  double zeroOffsetH = -195.5;
 
   // Test constant step h,v
-  for (double step = 0; step < 160; step += 10) {
+  Serial.println("Start demo_0");
+  for (int i = 0; i < 20; i++) {
+    Serial.print("> ");
+    Serial.println(i);
+    for (double step = -198.5; step < 176.0; step += 10) {
       Serial.print("[step]: ");
       Serial.println(step);
-    turret.moveTo(step, 0, 0);
+      turret.moveTo(step, 87, 0);
+      // turret.moveBy(20.0, 0.0, 0.0);
+      delay(500);
+    }
+
+    for (double step = 0; step < 200.0; step += 10) {
+      Serial.print("[step]: ");
+      Serial.println(step);
+      turret.moveTo(0, step, 0);
+      // turret.moveBy(20.0, 0.0, 0.0);
+      delay(500);
+    }
+
+    delay(5000);
+    turret.moveTo(0, 0, 0);
     delay(5000);
   }
-  for (double step = 0; step > -160; step -= 10) {
+
-    Serial.print("[step]: ");
+  // Test rollX
-    Serial.println(step);
+  turret.moveTo(0 * panelWidth + zeroOffsetH, 0, 0);
-    turret.moveTo(step, 0, 0);
   delay(5000);
+  turret.moveTo((panelHCount * panelWidth) / 2 + zeroOffsetH, 0, 0);
+  delay(5000);
+  turret.moveTo(panelHCount * panelWidth + zeroOffsetH, 0, 0);
+  delay(5000);
+  turret.gotoZero();
+
+  // Test rollY
+  turret.moveTo(0, 0 * panelHeight + zeroOffsetV, 0);
+  delay(5000);
+  turret.moveTo(0, (panelVCount * panelHeight) / 2 + zeroOffsetV, 0);
+  delay(5000);
+  turret.moveTo(0, panelVCount * panelHeight + zeroOffsetV, 0);
+  delay(5000);
+  turret.gotoZero();
+
+  // Test align to panels
+  for (double panelH = 0; panelH < panelHCount + 1; panelH += 0.5) {
+    for (double panelV = 0; panelV < panelVCount + 1; panelV += 0.5) {
+      double x = panelH * panelWidth + zeroOffsetH;
+      double y = panelV * panelHeight + zeroOffsetV;
+      double z = 0;
+
+      Serial.print("Goto (x, y, z) => (");
+      Serial.print(x);
+      Serial.print(", ");
+      Serial.print(y);
+      Serial.print(", ");
+      Serial.print(z);
+      Serial.println(")");
+
+      turret.moveTo(x, y, z);
+
+      delay(2000);
+    }
   }
 
   turret.gotoZero();