Entrega 3

 

🧩  Inverse Kinematics and Controller Design

🎯 Objective

This report summarizes the key steps and methodologies used in the design and implementation of the inverse kinematics and discrete-time  controller for a 3-DOF SCARA manipulator robot, as part of the industrial Robotics and Digital Control project.

---

🤖 Inverse Kinematics Design

💡 Approach

A 3-DOF SCARA robot with two rotational joints and one prismatic joint was modeled. The inverse kinematics were derived analytically by using the DH method, considering the planar configuration of the first two joints and the linear movement of the third.

🧮 Robot Workspace simulation

%% Parámetros

r1 = 14.5; % long. barra 1 [cm]

r2 = 12.5; % long. barra 2 [cm]

ang1_antebrazo = -70;

ang2_antebrazo = 70;

ang1_brazo = -128;

ang2_brazo = 128;

pasos = 50;

Th1 = linspace(deg2rad(ang1_antebrazo), deg2rad(ang2_antebrazo), pasos);

Th2 = linspace(deg2rad(ang1_brazo), deg2rad(ang2_brazo), pasos);

% Cálculo posicion articulaciones y animación

% Creación Figura

fig1 = figure(1);

hb1 = plot(0,0,'linewidth',2);

hold on

hb2 = plot(0,0,'linewidth',2);

hold on

hb4 = plot(0,0, 'b', 'linewidth',8); % trayectoria

hold on

axis equal

axis([-1.25*(r1+r2) 1.25*(r1+r2) -1.0*(r1+r2) 1.25*(r1+r2)])

grid on

xlabel('X [cm]')

ylabel('Y [cm]')

title('Workspace');

% Cálculo de posición manipulador

lx_hist = []; % Para almacenar la trayectoria

ly_hist = [];

for i = 1:pasos

    % Añadir un "corte" en la trayectoria para evitar líneas rectas

    lx_hist = [lx_hist, NaN];

    ly_hist = [ly_hist, NaN];

    for j = 1:pasos

        th1 = Th1(i);

        th2 = Th2(j);

        % Cálculo de articulaciones

        p0x = 0.0; p0y = 0.0; % Tierra

        p1x = p0x + r1 * cos(th1);

        p1y = p0y + r1 * sin(th1);

        p2x = p1x + r2 * cos(th1 + th2);

        p2y = p1y + r2 * sin(th1 + th2);

        % Animación

        set(hb1,'xdata',[p0x, p1x],'ydata',[p0y, p1y]);

        set(hb2,'xdata',[p1x, p2x],'ydata',[p1y, p2y]);

        % Guardar la trayectoria

        lx_hist = [lx_hist, p2x];

        ly_hist = [ly_hist, p2y];

        set(hb4,'xdata', lx_hist, 'ydata', ly_hist);

        pause(0.005); % tiempo en s entre pasos

    end

end

The following is the representation of the robot workspace, based on the length of both of its arms.

$$\mathrm{<br>}$$

$$\mathrm{<span\; style="font-weight:\; 700;">🧮\; Digital\; twin</span>}$$

$$$$

🧪 Implementation

🧩 1. Pin and variables set.

#define PIN_PWM1 14
#define PIN_PWM2 15
#define PIN_DIR1 12
#define PIN_DIR2 13
#define encoderA_1 2
#define encoderB_1 3
#define encoderA_2 6
#define encoderB_2 7
#define FINAL1 10
#define FINAL2 11

float x = 0, y = 0;
float theta1 = 0, theta2 = 0;

---

🔧 2. Class Motor

class Motor {
public:
    int pin_pwm, pin_dir;
    int encoderA, encoderB;
    volatile long encoderCount = 0;

    Motor(int pwm, int dir, int encA, int encB) {
        pin_pwm = pwm;
        pin_dir = dir;
        encoderA = encA;
        encoderB = encB;
    }

    void begin() {
        pinMode(pin_pwm, OUTPUT);
        pinMode(pin_dir, OUTPUT);
        pinMode(encoderA, INPUT);
        pinMode(encoderB, INPUT);
        attachInterrupt(digitalPinToInterrupt(encoderA), isrEncoder, RISING);
    }

    void setSpeed(int speed) {
        digitalWrite(pin_dir, speed >= 0 ? HIGH : LOW);
        analogWrite(pin_pwm, abs(speed));
    }
};

---

🔄 3. Manual control and  "Home" routine

void rutina_home() {
    while (digitalRead(FINAL1) == HIGH) {
        motor1.setSpeed(-50);
    }
    motor1.setSpeed(0);
    delay(100);
    while (digitalRead(FINAL2) == HIGH) {
        motor2.setSpeed(-50);
    }
    motor2.setSpeed(0);
    delay(100);
}

void manual_control() {
    char input = Serial.read();
    if (input == 'w') motor1.setSpeed(100);
    else if (input == 's') motor1.setSpeed(-100);
    else if (input == 'e') motor2.setSpeed(100);
    else if (input == 'd') motor2.setSpeed(-100);
}

---

📐 4. Inverse kinematics

void cinematic_inv(float x, float y, float &th1, float &th2) {
    float r = sqrt(x * x + y * y);
    th2 = acos((r * r - L1 * L1 - L2 * L2) / (2 * L1 * L2));
    th1 = atan2(y, x) - atan2(L2 * sin(th2), L1 + L2 * cos(th2));
}

---

🔁 5. Discrete PID controller

float PID_Update_1(float setpoint, float measurement) {
    float error = setpoint - measurement;
    integral1 += error * Ts;
    float derivative = (error - prevError1) / Ts;
    float output = Kp1 * error + Ki1 * integral1 + Kd1 * derivative;
    prevError1 = error;
    return constrain(output, -255, 255);
}

---

🤖 6. Function moveTo()

void moveTo(float target1, float target2) {
    while (abs(current1 - target1) > tolerance || abs(current2 - target2) > tolerance) {
        int pwm1 = PID_Update_1(target1, current1);
        int pwm2 = PID_Update_2(target2, current2);
        motor1.setSpeed(pwm1);
        motor2.setSpeed(pwm2);
    }
    motor1.setSpeed(0);
    motor2.setSpeed(0);
}

---

🧠 7. Function main()

void loop() {
    Serial.println("1: Rutina home");
    Serial.println("2: Control manual");
    Serial.println("3: Cinemática directa");
    Serial.println("4: Cinemática inversa");

    while (Serial.available() == 0);
    char opcion = Serial.read();

    if (opcion == '1') rutina_home();
    else if (opcion == '2') manual_control();
    else if (opcion == '3') {
        // solicitar ángulos y mover
    }
    else if (opcion == '4') {
        // solicitar coordenadas (x, y), calcular θ1 y θ2, mover
    }
}

⚙️ Discrete-Time PID Controller Design

⚡ Control Variables

PID controllers were designed for the:

• Position 

• Speed 

🧰 Methodology

1. System Identification:

   • Obtained discrete-time model of the motor via step response analysis Model for motor speed.  Model for motor position

2. Controllers for motor speed:

        The following controllers are the result of a process of design and tuning 

• Ultimate Gain PID controller

  

  
  
  
  
  

• Reaction curve PID controller 

  

  
  
  
  
  

1. Controllers for motor position:

            The following controllers are the result of a process of design and tuning 

• Ultimate gain PID controller

  

  
  
• Reaction curve PID controller

  

📌 Frequency analysis

        Controllers Analyzed

• Non-tuned discrete controller:

  $$C_{nt}(z) = \frac{16.79z^2 - 14.51z + 3.895}{z^2 - z}$$

• Tuned discrete controller:

  $$C_{t}(z) = \frac{1.418z^2 - 1.773z + 0.3971}{z^2 - z}$$

• Plant (continuous time):

  $$G(s) = \frac{1.042}{0.0575s + 1}, \quad \text{Input Delay } = 0.0025 \,s$$

• Sampling time:
  $T_s = 0.0085 \,s$

---

📊 Bandwidth Results

Controller	Bandwidth (rad/s)
Non-tuned	730.1
Tuned	4.23

---

📡 Results

• The non-tuned controller provides a much higher bandwidth (~730 rad/s), which results in a very fast response, but it also increases the system's sensitivity to noise and high-frequency disturbances.

• In contrast, the tuned controller has a much lower bandwidth (~4.23 rad/s), which makes the system:

  • Less sensitive to noise.

  • More robust and stable.

  • Slightly slower, but with improved precision.

✅ Final Considerations

• The inverse kinematics system successfully maps W-space points to C-space joint values within a defined workspace.

• The selected PID controllers ensure smooth, precise, and stable movement of the SCARA joints.

• Although the non-tuned controller offers faster performance, its high bandwidth makes the system more vulnerable to external disturbances and sensor noise. The tuned controller, with reduced bandwidth, ensures better precision, noise immunity, and overall system stability, which are essential characteristics for applications like the SCARA manipulator.

---