2DOF Arm

The arm is controlled through two channels of the virtual PWM peripheral.

PWM Channels

PWM Channel	Joint	Base Address
PWM0	Joint 1	0x40004000
PWM1	Joint 2	0x40004040

Register Map

PWM0 — Joint 1

Address	Register	Description
0x40004000	Period	PWM period
0x40004004	Duty	PWM duty value
0x40004008	Enable	Enables the PWM output

PWM1 — Joint 2

Address	Register	Description
0x40004040	Period	PWM period
0x40004044	Duty	PWM duty value
0x40004048	Enable	Enables the PWM output

PWM Configuration

The PWM period used by the model is:

800000

The example position mapping uses the following duty values:

0°   → 40000
180° → 80000

The relationship is linear:

duty = 40000 + (angle × 40000) / 180

The exact PWM configuration should be set according to the requirements of the application firmware.

The two joints can be controlled independently by applying the appropriate PWM values to their respective channels.

Joint Control

Each joint has a position range of 0° to 180°.

Joint	PWM Channel	Position Range
Joint 1	PWM0	0–180°
Joint 2	PWM1	0–180°

Changing the PWM duty value changes the position of the corresponding joint in the simulation.

End-Effector Position

The position of the end effector depends on the two joint angles and the lengths of the two links.

L1 = 10 cm
L2 = 10 cm

For joint angles θ1 and θ2, the end-effector position can be calculated using forward kinematics:

x = L1 cos(θ1) + L2 cos(θ1 + θ2)

y = L1 sin(θ1) + L2 sin(θ1 + θ2)

Inverse Kinematics

For a target end-effector position (x, y), the corresponding joint angles can be calculated using inverse kinematics.

d = (x² + y² - L1² - L2²) / (2L1L2)

θ2 = acos(d)

θ1 = atan2(y, x)
     - atan2(
         L2 sin(θ2),
         L1 + L2 cos(θ2)
       )

The calculated angles can then be converted into the appropriate PWM values for the two joints.

Motion

The joints can be controlled individually or continuously to produce a trajectory.

A trajectory can be represented as a sequence of joint positions or end-effector target positions.

This allows the virtual arm to perform movements such as:

• Point-to-point movement
• Linear movement
• Geometric trajectories
• Continuous joint movement

The trajectory itself is determined by the application firmware.

The following Embedded C program demonstrates one way of controlling the virtual arm.

The example:

• Initializes the two PWM channels
• Converts joint angles into PWM duty values
• Calculates inverse kinematics
• Moves the end effector to target positions
• Generates linear trajectories
• Draws a rectangular trajectory

This is an example application. Other firmware can use the same hardware interface to implement different control strategies.

#include "halo.h"
#include <stdio.h>
#include <math.h>

#define PI 3.141592653589793
#define RAD_TO_DEG(rad) ((rad) * 180.0 / PI)

#define L1 10.0
#define L2 10.0

unsigned int angle_to_duty_us(unsigned int angle)
{
    if (angle > 180)
        angle = 180;

    return 40000 + (angle * 40000) / 180;
}

void inverse_kinematics(float x, float y, float* t1, float* t2)
{
    float d =
        (x*x + y*y - L1*L1 - L2*L2) /
        (2*L1*L2);

    if (d > 1.0)
        d = 1.0;

    if (d < -1.0)
        d = -1.0;

    float theta2 = acos(d);

    float theta1 =
        atan2(y, x) -
        atan2(
            L2 * sin(theta2),
            L1 + L2 * cos(theta2)
        );

    *t1 = RAD_TO_DEG(theta1);
    *t2 = RAD_TO_DEG(theta2);
}

void move_to(float x, float y)
{
    float t1, t2;

    inverse_kinematics(x, y, &t1, &t2);

    if (t1 < 0)
        t1 = 0;

    if (t1 > 180)
        t1 = 180;

    if (t2 < 0)
        t2 = 0;

    if (t2 > 180)
        t2 = 180;

    WRITE_REGISTER(
        0x40004004,
        angle_to_duty_us((unsigned int)t1)
    );

    WRITE_REGISTER(
        0x40004044,
        angle_to_duty_us((unsigned int)t2)
    );

    delay_us(200);
}

void initialize_pwm(void)
{
    WRITE_REGISTER(0x40004000, 800000);
    WRITE_REGISTER(0x40004040, 800000);

    WRITE_REGISTER(0x40004008, 0x01);
    WRITE_REGISTER(0x40004048, 0x01);
}

void move_line(
    float xs,
    float ys,
    float xe,
    float ye,
    int steps
)
{
    for (int i = 0; i <= steps; i++)
    {
        float x =
            xs + i * (xe - xs) / steps;

        float y =
            ys + i * (ye - ys) / steps;

        move_to(x, y);
    }
}

void draw_rectangle(void)
{
    int steps = 25;

    float x0 = -10.0, y0 = 15.0;
    float x1 = -5.0,  y1 = 15.0;
    float x2 = -5.0,  y2 = 10.0;
    float x3 = -10.0, y3 = 10.0;

    move_line(x0, y0, x1, y1, steps);
    move_line(x1, y1, x2, y2, steps);
    move_line(x2, y2, x3, y3, steps);
    move_line(x3, y3, x0, y0, steps);
}

void fw_main(void)
{
    initialize_pwm();

    while (1)
    {
        draw_rectangle();
    }
}

Example behavior

The example continuously generates a rectangular trajectory by moving the end effector through four target points.

The simulation visualizes the resulting joint and arm movement.

The application can be modified to implement other trajectories or control strategies while using the same virtual PWM interface.