Reasoning for robot that draws a circle

[0_erwan_0]

Hi,
I write on this forum to ask for help, I have a robot with a wheel on each side at the back controlled by two motors (one motor per wheel) and a pen as a central wheel at the front.

My goal is to draw a circle but not with the classical method (one wheel turning in one direction and the other in the other). I need to draw a circle from 2 centimeters radius to 20 by making a circle with the pen not with the robot.
It's hard to explain so here is the different steps to draw the circle :

I can’t understand how to make a move like this, is it a mathematical expression, a logic that I don’t have, ...
I am using an ESP32 and motor with integrated encoder. Here is a simple code that make the robot move :

// ==== Libraries ====
#include <WiFi.h>

// ==== Wi-Fi Network ====
const char* ssid = "leboss";
const char* password = "1234567890";
WiFiServer server(80);

// ==== Encoder Pins ====
#define ENC_L_CH_A 32
#define ENC_L_CH_B 33
#define ENC_R_CH_A 27
#define ENC_R_CH_B 14

// ==== Motor Pins ====
#define IN1_L 17
#define IN2_L 16
#define IN1_R 18
#define IN2_R 19
#define EN_L 4
#define EN_R 23

// ==== Constants ====
const float TICK_TO_CM = (28.274 / 4200.0) * 4.0;
const float WHEELBASE_CM = 9.0;
const int BASE_SPEED = 85;
const int BRAKE_PWM = 70;
const int BRAKE_DURATION = 50;
const int ROTATION_BRAKE_DURATION = 100;

// ==== Variables ====
volatile long tickL = 0;
volatile long tickR = 0;
bool motionComplete = true;
bool isRotating = false;
bool rotateLeft = true;
float targetDistance = 0;
long targetRotationTicks = 0;

// ==== PID Structure ====
struct PID {
  float Kp, Ki, Kd;
  float errorSum;
  float previousError;

  PID(float p, float i, float d) : Kp(p), Ki(i), Kd(d), errorSum(0), previousError(0) {}

  float compute(float error) {
    errorSum += error;
    float deltaError = error - previousError;
    previousError = error;
    return Kp * error + Ki * errorSum + Kd * deltaError;
  }
};

PID pidTrajectory(1.5, 0.0, 0.5);
PID pidRotation(2.0, 0.0, 0.8);

// ==== Interrupts ====
void IRAM_ATTR handleEncoderL() { tickL++; }
void IRAM_ATTR handleEncoderR() { tickR++; }

// ==== Motor Functions ====
void setMotors(int pwmL, int pwmR) {
  digitalWrite(IN1_R, HIGH); digitalWrite(IN2_R, LOW);
  digitalWrite(IN1_L, HIGH); digitalWrite(IN2_L, LOW);
  analogWrite(EN_R, constrain(pwmR, 0, 255));
  analogWrite(EN_L, constrain(pwmL, 0, 255));
}

void setRotation(bool left, int pwmL, int pwmR) {
  if (left) {
    digitalWrite(IN1_R, HIGH); digitalWrite(IN2_R, LOW);
    digitalWrite(IN1_L, LOW); digitalWrite(IN2_L, HIGH);
  } else {
    digitalWrite(IN1_R, LOW); digitalWrite(IN2_R, HIGH);
    digitalWrite(IN1_L, HIGH); digitalWrite(IN2_L, LOW);
  }
  analogWrite(EN_R, constrain(pwmR, 0, 255));
  analogWrite(EN_L, constrain(pwmL, 0, 255));
}

void activeBrake() {
  digitalWrite(IN1_R, LOW); digitalWrite(IN2_R, HIGH);
  digitalWrite(IN1_L, LOW); digitalWrite(IN2_L, HIGH);
  analogWrite(EN_R, BRAKE_PWM);
  analogWrite(EN_L, BRAKE_PWM);
  delay(BRAKE_DURATION);
}

void activeBrakeRotation() {
  digitalWrite(IN1_R, LOW); digitalWrite(IN2_R, LOW);
  digitalWrite(IN1_L, LOW); digitalWrite(IN2_L, LOW);
  analogWrite(EN_R, 0);
  analogWrite(EN_L, 0);
  delay(ROTATION_BRAKE_DURATION);
}

void stopMotors() {
  digitalWrite(IN1_R, LOW); digitalWrite(IN2_R, LOW);
  digitalWrite(IN1_L, LOW); digitalWrite(IN2_L, LOW);
  analogWrite(EN_R, 0); analogWrite(EN_L, 0);
}

void resetTicks() {
  tickL = 0;
  tickR = 0;
}

void moveForward(float distance_cm) {
  resetTicks();
  targetDistance = distance_cm;
  motionComplete = false;
  isRotating = false;
  while (!motionComplete) {
    float averageDist = ((float)tickL + (float)tickR) / 2.0 * TICK_TO_CM;
    float tickError = (float)tickL - (float)tickR;
    float correction = pidTrajectory.compute(tickError);
    int pwmL = BASE_SPEED - correction;
    int pwmR = BASE_SPEED + correction;
    setMotors(pwmL, pwmR);
    if (averageDist >= targetDistance) {
      activeBrake();
      stopMotors();
      motionComplete = true;
    }
  }
}

void rotate(bool left) {
  resetTicks();
  targetRotationTicks = (WHEELBASE_CM * PI / 4.0) / TICK_TO_CM;
  motionComplete = false;
  isRotating = true;
  while (!motionComplete) {
    long error = tickR + tickL;
    float correction = pidRotation.compute(error);
    int pwmL = BASE_SPEED - correction;
    int pwmR = BASE_SPEED - correction;
    setRotation(left, pwmL, pwmR);

    long avgTicks = (abs(tickL) + abs(tickR)) / 2;
    if (avgTicks >= targetRotationTicks) {
      activeBrakeRotation();
      stopMotors();
      motionComplete = true;
      isRotating = false;
    }
  }
}

void turnLeft90() {
  rotate(true);
}

void turnRight90() {
  rotate(false);
}

void runStairPath() {
  moveForward(20);
  turnLeft90();
  moveForward(10);
  turnRight90();
  moveForward(40);
}

// ==== Setup ====
void setup() {
  Serial.begin(115200);
  pinMode(IN1_R, OUTPUT); pinMode(IN2_R, OUTPUT);
  pinMode(IN1_L, OUTPUT); pinMode(IN2_L, OUTPUT);
  pinMode(EN_R, OUTPUT); pinMode(EN_L, OUTPUT);
  pinMode(ENC_L_CH_A, INPUT); pinMode(ENC_L_CH_B, INPUT);
  pinMode(ENC_R_CH_A, INPUT); pinMode(ENC_R_CH_B, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENC_L_CH_A), handleEncoderL, RISING);
  attachInterrupt(digitalPinToInterrupt(ENC_R_CH_A), handleEncoderR, RISING);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) delay(500);
  server.begin();
  Serial.println(WiFi.localIP());
}

// ==== Loop ====
void loop() {
  WiFiClient client = server.available();
  if (client) {
    String request = client.readStringUntil('\r');
    client.flush();

    if (request.indexOf("/FORWARD") != -1) {
      int index = request.indexOf("distance=");
      if (index != -1) {
        String param = request.substring(index + 9);
        int cm = param.toInt();
        if (cm > 0 && cm < 1000) {
          moveForward(cm);
        }
      }
    }

    if (request.indexOf("/LEFT") != -1) {
      turnLeft90();
    }

    if (request.indexOf("/RIGHT") != -1) {
      turnRight90();
    }

    if (request.indexOf("/STAIR") != -1) {
      runStairPath();
    }

    client.println("HTTP/1.1 200 OK");
    client.println("Content-type:text/html\n");
    client.println();
    client.println("<!DOCTYPE html><html><head><meta charset='UTF-8'><title>PID Robot</title></head><body>");
    client.println("<h2>PID Wi-Fi Robot</h2>");
    client.println("<form action='/FORWARD' method='GET'>");
    client.println("<label>Distance (cm): </label><input type='number' name='distance' min='1' max='1000'>");
    client.println("<input type='submit' value='Forward'>");
    client.println("</form>");
    client.println("<form action='/LEFT' method='GET'>");
    client.println("<input type='submit' value='Turn 90° Left'>");
    client.println("</form>");
    client.println("<form action='/RIGHT' method='GET'>");
    client.println("<input type='submit' value='Turn 90° Right'>");
    client.println("</form>");
    client.println("<form action='/STAIR' method='GET'>");
    client.println("<input type='submit' value='Stair Path'>");
    client.println("</form>");
    client.println("</body></html>");
    client.stop();
  }
}

I would really appreciate your help to get this circle drawn

[Paul_KD7HB]

Begin by telling us what is the MINIMUM movement you can get from each wheel and is it consistently the same distance. If you can't do that each and every time, then you cannot draw any known curve.

[0_erwan_0]

If I understand correctly what you're saying, the minimum movement I can achieve per wheel is 3 millimeters, and the distance is very precise and consistent.

[Paul_KD7HB]

Ok, and can you write a program to move both wheels different distances at the exact same time? If not, then any circle you can draw will be a series of connected straight lines with a minimum length of 3mm.

[gcjr]

as mentioned, the circle is a series of lines connecting points

you'll need to determine a new postion for each wheel depending on its existing position such that the pen is now at the next point on the circle

relative to the current direction of the robot, the next point is at some delta x (horizontal) and delta y (vertical) distance that results when each wheel if moved forward or backward some amount

its seem the delta y is the sum of the forward movements of the wheels. The delta x is the ratio of the distance of the pen from the center-line thru the wheels and half the distance between the wheels multipied by the difference between the forward/backward movements of the wheels. the distance for each wheel is the the sum of the two movements

i suggest working out the geometry and code on a laptop, viewing the result graphically (e.g. Xgraph)

[xfpd]

Manipulate the dimensions of the red triangle to move the apex of the blue triangle in the equation

(x - h)^2 + (y - k)^2 = r^2

[GoForSmoke]

The cart should move the pen always tangent to the circle with the wheels behind. If one wheel moves faster than the other while both move smoothly forward at steady rates, you will get a circle. How far off you are will show when the cart goes around once and the ends don't meet.

[0_erwan_0]

Does this logic seem to be correct for calculating the distances of the left and right wheels following a circle represented by many points?

float R = 5.0;              // radius of the circle in cm
int N = 100;                // number of points on the circle
float B = 9.0;              // distance between wheels in cm

struct Pose {
  float x;
  float y;
  float angle;              // orientation in radians
};

Pose robot_pose = {0.0, 0.0, 0.0};  // initial robot position

for (int i = 1; i <= N; i++) {
  float theta = (i * 2.0 * PI) / N;   // target angle around the circle

  // 1. Next point on the circle
  float x_next = R * cos(theta);
  float y_next = R * sin(theta);

  // 2. Displacement vector
  float dx = x_next - robot_pose.x;
  float dy = y_next - robot_pose.y;

  // 3. Distance to move forward
  float d = sqrt(dx * dx + dy * dy);

  // 4. Target orientation
  float target_angle = atan2(dy, dx);
  float delta_theta = target_angle - robot_pose.angle;

  // Normalize delta_theta to [-PI, PI]
  while (delta_theta > PI) delta_theta -= 2 * PI;
  while (delta_theta < -PI) delta_theta += 2 * PI;

  // 5. Compute left and right wheel distances
  float dL = d - (B / 2.0) * delta_theta;
  float dR = d + (B / 2.0) * delta_theta;

  // 6. Move the robot with those distances (to be implemented)
  move_wheels(dL, dR);

  // 7. Update current robot pose
  robot_pose.x = x_next;
  robot_pose.y = y_next;
  robot_pose.angle = target_angle;
}

[DrDiettrich]

@0_erwan_0

the minimum movement I can achieve per wheel is 3 millimeters

Then you have to live with circles of 3 mm stair case steps.

For the code look at Bresenham's algorithms.

[gcjr]

@0_erwan_0

Does this logic seem to be correct for calculating the distances of the left and right wheels

below is a plot of dL and dR and certainly doesn't look correct

you might start with how to do this if the pen were simply between the wheels

[xfpd]

This would be a great time to employ chatGPT. Translate linear, reciprocating motion to circular motion.

[alto777]

This:

@GoForSmoke

If one wheel moves faster than the other while both move smoothly forward at steady rates, you will get a circle.

Make the circle very larger, and it becomes obvious.

The problem is calculating the speed at which each each wheel must turn. I don't think you can do any better than making one wheel go faster than the other, down to the smallest circle you could reasonably expect it to work at all.

a7

[GoForSmoke]

@alto777

The problem is calculating the speed at which each each wheel must turn.

LOL! I'd get the thing making circles and measure good ones!

What I saw of that code includes "normalization while's" that could block execution. It may take a bit just getting circles instead of polygons.

I did like the idea of making it mimic linear to circular motion.

Ralph Steiner Mechanical Principles 1933

[xfpd]

@alto777

calculating the speed

Something less complicated than "facing north" as post #1 suggests: There will be three radii. The pen (r), the outer wheel (r+(wheelbase/2)), the inner wheel (r-(wheelbase/2)). If the drive proportion of the outer wheel to the inner wheel is constant, the pen should draw a circle.

The example movement in Post #1 seems unrealistic. Red = left wheel, Blue = right wheel.

The original drawing is a pendulum (the triangle) on a moving wheel (the yellow ring). The wheels are not driving the triangle/pen to draw the circle.

[0_erwan_0]

YES, that’s exactly what I wanted to do, because the robot pen is the point of contact between the arm and the wheel in your GIF. But chatgpt has a hard time understanding this.

[0_erwan_0]

I think I have something, my simulation is not amazing but I can see the trajectory of the robot which is exactly what I want to do, just need to test on my robot to see if it really works

[GoForSmoke]

@0_erwan_0

But chatgpt has a hard time understanding this.

That makes two of you. I'm gone.

[xfpd]

Wheels do not roll sideways, as you can see in this animation. chatGPT does not understand that. The images in post #1 are treating the yellow circle as a wheel with the vehicle attached at the pen. Make your own "triangle car" and do your own physical experiments with moving the "pen" around the circle.

[alto777]

OK, so there's more to this than I will ever understand.

It reminds me of the 3D printers that have non-XYZ motion controlled by three actuators, or this

Elektor Sand Clock for Raspberry Pi Pico

where a path is figured out given constraints and linkages. There must be an entire field of inquiry on how to calculate the net motion of the pen.

How did you make the animation demonstration?

a7

[xfpd]

Etch-a-sketch has vertical and horizontal movement. The wheels on the OP bus go around and around. (an "xy problem?", "pir2 problem?")