move interpolation to arduino

arduino_code/arduino_code.ino

@@ -27,14 +27,17 @@ float buffer[BUFFER_SIZE][2];  // Store theta, rho pairs
 int bufferCount = 0;   // Number of pairs in the buffer
 bool batchComplete = false;
 
+// Track the current position in polar coordinates
+float currentTheta = 0.0;  // Current theta in radians
+float currentRho = 0.0;    // Current rho (0 to 1)
+
 void setup() {
     // Set maximum speed and acceleration
-    rotStepper.setMaxSpeed(2000);  // Adjust as needed
-    rotStepper.setAcceleration(500);  // Adjust as needed
+    rotStepper.setMaxSpeed(5000);  // Adjust as needed
+    rotStepper.setAcceleration(5000);  // Adjust as needed
 
-    inOutStepper.setMaxSpeed(2000);  // Adjust as needed
-    inOutStepper.setAcceleration(500);  // Adjust as needed
-    // inOutStepper.setPinsInverted(true, false, false);
+    inOutStepper.setMaxSpeed(5000);  // Adjust as needed
+    inOutStepper.setAcceleration(5000);  // Adjust as needed
 
     // Add steppers to MultiStepper
     multiStepper.addStepper(rotStepper);
@@ -92,9 +95,23 @@ void loop() {
 
     // Process the buffer if a batch is ready
     if (batchComplete && bufferCount > 0) {
+        // Start interpolation from the current position
+        float startTheta = currentTheta;
+        float startRho = currentRho;
+
         for (int i = 0; i < bufferCount; i++) {
-            movePolar(buffer[i][0], buffer[i][1]);
+            // Interpolate from the starting point to the next buffer point
+            interpolatePath(
+                startTheta, startRho,     // Start theta and rho
+                buffer[i][0], buffer[i][1],  // End theta and rho
+                0.001                      // Step size
+            );
+
+            // Update the starting point for the next segment
+            startTheta = buffer[i][0];
+            startRho = buffer[i][1];
         }
+
         bufferCount = 0;        // Clear buffer
         batchComplete = false;  // Reset batch flag
         Serial.println("READY");
@@ -113,6 +130,8 @@ void homing() {
         }
     }
     inOutStepper.setCurrentPosition(0);  // Set home position
+    currentTheta = 0.0;  // Reset polar coordinates
+    currentRho = 0.0;
     Serial.println("HOMED");
 }
 
@@ -136,4 +155,23 @@ void movePolar(float theta, float rho) {
     // Move both motors synchronously
     multiStepper.moveTo(targetPositions);
     multiStepper.runSpeedToPosition();  // Blocking call
+
+    // Update the current coordinates
+    currentTheta = theta;
+    currentRho = rho;
+}
+
+void interpolatePath(float startTheta, float startRho, float endTheta, float endRho, float stepSize) {
+    // Calculate the total distance in the polar coordinate system
+    float distance = sqrt(pow(endTheta - startTheta, 2) + pow(endRho - startRho, 2));
+    int numSteps = max(1, (int)(distance / stepSize));  // Ensure at least one step
+
+    for (int step = 0; step <= numSteps; step++) {
+        float t = (float)step / numSteps;  // Interpolation factor (0 to 1)
+        float interpolatedTheta = startTheta + t * (endTheta - startTheta);
+        float interpolatedRho = startRho + t * (endRho - startRho);
+
+        // Move to the interpolated theta-rho
+        movePolar(interpolatedTheta, interpolatedRho);
+    }
 }

theta_rho_app.py

@@ -82,24 +82,6 @@ def send_command(command):
                 break
         time.sleep(0.5)  # Small delay to avoid busy waiting
 
-def interpolate_path(start, end, step_size=0.001):
-    """Interpolate a straight path between two theta-rho points with a fixed step size."""
-    start_theta, start_rho = start
-    end_theta, end_rho = end
-
-    # Calculate the total distance in the polar coordinate system
-    distance = math.sqrt((end_theta - start_theta)**2 + (end_rho - start_rho)**2)
-    num_steps = max(1, int(distance / step_size))  # Ensure at least one step
-
-    interpolated_points = []
-    for step in range(num_steps + 1):
-        t = step / num_steps  # Interpolation factor (0 to 1)
-        theta = start_theta + t * (end_theta - start_theta)
-        rho = start_rho + t * (end_rho - start_rho)
-        interpolated_points.append((theta, rho))
-
-    return interpolated_points
-
 def run_theta_rho_file(file_path):
     """Run a theta-rho file by interpolating straight paths and sending data in optimized batches."""
     global stop_requested
@@ -110,20 +92,14 @@ def run_theta_rho_file(file_path):
         print("Not enough coordinates for interpolation.")
         return
 
-    # Interpolate paths between points with fine resolution
-    step_size = 0.01  # Smaller values create finer steps for smoother movement
-    interpolated_coordinates = []
-    for i in range(len(coordinates) - 1):
-        interpolated_coordinates.extend(interpolate_path(coordinates[i], coordinates[i + 1], step_size=step_size))
-
     # Optimize batch size for smoother execution
-    batch_size = 8  # Smaller batches may smooth movement further
-    for i in range(0, len(interpolated_coordinates), batch_size):
+    batch_size = 10  # Smaller batches may smooth movement further
+    for i in range(0, len(coordinates), batch_size):
         if stop_requested:
             print("Execution stopped by user.")
             break
 
-        batch = interpolated_coordinates[i:i + batch_size]
+        batch = coordinates[i:i + batch_size]
         while True:
             if ser.in_waiting > 0:
                 response = ser.readline().decode().strip()