adjust speed dynamically based on rho coordinate

firmware/arduino_code/arduino_code.ino

@@ -345,6 +345,29 @@ void homing()
 
 void movePolar(float theta, float rho)
 {
+    // Define different scaling factors for rho <= 0.5 and rho > 0.5
+    float speedScalingFactorLow = 0.7;  // Scaling factor for rho <= 0.5
+    float speedScalingFactorHigh = 0.4; // Scaling factor for rho > 0.5
+
+    float speedScalingFactor;
+
+    // Determine which factor to use
+    if (rho <= 0.5) {
+        speedScalingFactor = speedScalingFactorLow;
+    } else {
+        speedScalingFactor = speedScalingFactorHigh;
+    }
+
+    // Scale speed dynamically based on rho
+    long dynamicSpeed = maxSpeed * (1.0 + speedScalingFactor * (1.0 - 2.0 * rho));
+
+    // Ensure the speed is within a valid range
+    dynamicSpeed = constrain(dynamicSpeed, 1, maxSpeed);
+
+    // Set stepper speeds dynamically
+    rotStepper.setMaxSpeed(dynamicSpeed);
+    inOutStepper.setMaxSpeed(dynamicSpeed);
+
     // Convert polar coordinates to motor steps
     long rotSteps = theta * (rot_total_steps / (2.0 * M_PI)); // Steps for rot axis
     long inOutSteps = rho * inOut_total_steps;                // Steps for in-out axis

firmware/arduino_code_TMC2209/arduino_code_TMC2209.ino

@@ -346,6 +346,29 @@ void homing()
 
 void movePolar(float theta, float rho)
 {
+    // Define different scaling factors for rho <= 0.5 and rho > 0.5
+    float speedScalingFactorLow = 0.7;  // Scaling factor for rho <= 0.5
+    float speedScalingFactorHigh = 0.4; // Scaling factor for rho > 0.5
+
+    float speedScalingFactor;
+
+    // Determine which factor to use
+    if (rho <= 0.5) {
+        speedScalingFactor = speedScalingFactorLow;
+    } else {
+        speedScalingFactor = speedScalingFactorHigh;
+    }
+
+    // Scale speed dynamically based on rho
+    long dynamicSpeed = maxSpeed * (1.0 + speedScalingFactor * (1.0 - 2.0 * rho));
+
+    // Ensure the speed is within a valid range
+    dynamicSpeed = constrain(dynamicSpeed, 1, maxSpeed);
+
+    // Set stepper speeds dynamically
+    rotStepper.setMaxSpeed(dynamicSpeed);
+    inOutStepper.setMaxSpeed(dynamicSpeed);
+
     // Convert polar coordinates to motor steps
     long rotSteps = theta * (rot_total_steps / (2.0 * M_PI)); // Steps for rot axis
     long inOutSteps = rho * inOut_total_steps;                // Steps for in-out axis

firmware/esp32/esp32.ino

@@ -248,6 +248,29 @@ void homing()
 
 void movePolar(double theta, double rho)
 {
+    // Define different scaling factors for rho <= 0.5 and rho > 0.5
+    float speedScalingFactorLow = 0.7;  // Scaling factor for rho <= 0.5
+    float speedScalingFactorHigh = 0.4; // Scaling factor for rho > 0.5
+
+    float speedScalingFactor;
+
+    // Determine which factor to use
+    if (rho <= 0.5) {
+        speedScalingFactor = speedScalingFactorLow;
+    } else {
+        speedScalingFactor = speedScalingFactorHigh;
+    }
+
+    // Scale speed dynamically based on rho
+    long dynamicSpeed = maxSpeed * (1.0 + speedScalingFactor * (1.0 - 2.0 * rho));
+
+    // Ensure the speed is within a valid range
+    dynamicSpeed = constrain(dynamicSpeed, 1, maxSpeed);
+
+    // Set stepper speeds dynamically
+    rotStepper.setMaxSpeed(dynamicSpeed);
+    inOutStepper.setMaxSpeed(dynamicSpeed);
+
     long rotSteps = lround(theta * (rot_total_steps / (2.0f * M_PI)));
     double revolutions = theta / (2.0 * M_PI);
     long offsetSteps = lround(revolutions * (rot_total_steps / gearRatio));

firmware/esp32_TMC2209/esp32_TMC2209.ino

@@ -237,13 +237,36 @@ void homing()
 
 void movePolar(float theta, float rho)
 {
+    // Define different scaling factors for rho <= 0.5 and rho > 0.5
+    float speedScalingFactorLow = 0.7;  // Scaling factor for rho <= 0.5
+    float speedScalingFactorHigh = 0.4; // Scaling factor for rho > 0.5
+
+    float speedScalingFactor;
+
+    // Determine which factor to use
+    if (rho <= 0.5) {
+        speedScalingFactor = speedScalingFactorLow;
+    } else {
+        speedScalingFactor = speedScalingFactorHigh;
+    }
+
+    // Scale speed dynamically based on rho
+    long dynamicSpeed = maxSpeed * (1.0 + speedScalingFactor * (1.0 - 2.0 * rho));
+
+    // Ensure the speed is within a valid range
+    dynamicSpeed = constrain(dynamicSpeed, 1, maxSpeed);
+
+    // Set stepper speeds dynamically
+    rotStepper.setMaxSpeed(dynamicSpeed);
+    inOutStepper.setMaxSpeed(dynamicSpeed);
+
     // Convert polar coordinates to motor steps
     long rotSteps = theta * (rot_total_steps / (2.0 * M_PI)); // Steps for rot axis
     long inOutSteps = rho * inOut_total_steps;                // Steps for in-out axis
 
     // Calculate offset for inOut axis
     float revolutions = theta / (2.0 * M_PI); // Fractional revolutions (can be positive or negative)
-    long offsetSteps = revolutions * rot_total_steps / gearRatio;    // 1600 steps inward or outward per revolution
+    long offsetSteps = revolutions * rot_total_steps / gearRatio;    // Steps inward or outward per revolution
 
     // Update the total revolutions to keep track of the offset history
     totalRevolutions += (theta - currentTheta) / (2.0 * M_PI);