Zum Inhalt

arm_servos.tc

Six hobby servos (MG996R) of a 6-DOF robot arm on a PCA9685: one slider per joint, a slow ramp, "centre all" and a master switch.

Source on GitHub

// @name: ARM SERVOS
// Six hobby servos (MG996R) of a 6-DOF robot arm on a PCA9685: one slider per joint, a slow ramp, "centre all" and a master switch.
// For the first assembly step: put EVERY servo in its centre position (1500 us) BEFORE the horns and brackets are screwed on.
//
//   http://<ip>/   main page:  Servos on / off, six sliders (500..2500 us per joint), "Centre all"
//   console:  ArmOn, ArmOff, ArmCenter, ArmUs <joint 1..6> <us>, ArmLimit <joint 1..6> <min> <max>, ArmSpeed <us per 20 ms>
//
// SAFETY
//  1. Nothing moves at start: the outputs carry no pulse until "Servos on". After "Servos off" the outputs are released again (the
//     servos can be turned by hand; an arm will fall under its own weight).
//  2. A slow ramp (ARM_SPEED us per 20 ms tick, default 6 = 300 us per second): a jump of the slider does not make the arm snap.
//     When the servos are switched on, the ramp starts at the slider values (centre after a reset): put the sliders where the
//     arm really is, or the first move is a step from there.
//  3. Every joint has its own limits (default ARM_MIN..ARM_MAX = 700..2300 us). ArmLimit narrows one joint; use it to keep the
//     arm away from its mechanical stops once you know them.
//  4. POWER: six MG996R can draw several amps when they start or stall. A bench supply of 6 V and at least 5 A on the servo rail of the
//     PCA9685 (V+), NOT the 5 V of the ESP32 or USB. GND of the supply, PCA9685 and ESP32 has to be common. A large electrolytic
//     (>= 1000 uF) at the servo rail. Never power the servos from the 5 V rail of the rover.
// I2C: SDA/SCL of the ESP32 to the PCA9685 (3.3 V logic), address 0x40 (all address jumpers open); -DPCA_A0 for A0 closed (address 0x41).
// Run on 12.11.2026 on an ESP32-S3 with a PCA9685 and ONE servo (joint 1 = PCA channel 0): sweep 700..2300 us and back to centre, about 40 mA
// without load. The other five joints and the load of the real arm are not tested yet. Joints count 1..6 in the commands, PCA channels 0..15.

#include "pca9685.tc"

#define I2C_BUS    0
#ifdef PCA_A0
#define PCA_ADDR   0x41          // pushed with -DPCA_A0: the A0 jumper is closed (the arm on the Waveshare camera board)
#else
#define PCA_ADDR   0x40
#endif
#define NSRV       6
#define ARM_MIN    700
#define ARM_MAX    2300
#define ARM_SPEED  6             // us per 20 ms tick: how fast a joint may move
#define ARM_CENTRE 1500

int  on     = 0;
int  was_on = 0;
int  do_center = 0;
int  speed  = ARM_SPEED;
int  s0 = ARM_CENTRE;            // the slider values (targets), us
int  s1 = ARM_CENTRE;
int  s2 = ARM_CENTRE;
int  s3 = ARM_CENTRE;
int  s4 = ARM_CENTRE;
int  s5 = ARM_CENTRE;
int  cur[6];                     // what the servos are commanded to right now
int  lo[6];
int  hi[6];
int  sent[6];
char ob[96];

int arm_clamp(int v, int a, int b) {
    if (v < a) { return a; }
    if (v > b) { return b; }
    return v;
}

int arm_target(int j) {
    int v = ARM_CENTRE;
    if (j == 0) { v = s0; }
    if (j == 1) { v = s1; }
    if (j == 2) { v = s2; }
    if (j == 3) { v = s3; }
    if (j == 4) { v = s4; }
    if (j == 5) { v = s5; }
    return arm_clamp(v, lo[j], hi[j]);
}

void arm_set_slider(int j, int v) {
    if (j == 0) { s0 = v; }
    if (j == 1) { s1 = v; }
    if (j == 2) { s2 = v; }
    if (j == 3) { s3 = v; }
    if (j == 4) { s4 = v; }
    if (j == 5) { s5 = v; }
}

void arm_all_off() {
    int j = 0;
    pca_all_off();
    while (j < NSRV) { sent[j] = 0; j = j + 1; }
}

void TaskLoop() {
    int j = 0;
    int t = 0;
    int c = 0;
    if (do_center == 1) {
        do_center = 0;
        j = 0;
        while (j < NSRV) { arm_set_slider(j, ARM_CENTRE); j = j + 1; }
    }
    if (on == 1 && was_on == 0) {                // switching on: the ramp starts at the sliders, so the first command is no jump
        j = 0;
        while (j < NSRV) { cur[j] = arm_target(j); sent[j] = 0; j = j + 1; }
        was_on = 1;
        addLog("ARM: servos on");
    }
    if (on == 0 && was_on == 1) {
        arm_all_off();
        was_on = 0;
        addLog("ARM: servos released (no pulse)");
    }
    if (on == 1) {
        j = 0;
        while (j < NSRV) {
            t = arm_target(j);
            c = cur[j];
            if (t > c) { c = c + speed; if (c > t) { c = t; } }
            if (t < c) { c = c - speed; if (c < t) { c = t; } }
            cur[j] = c;
            if (c != sent[j]) {
                pca_us(j, c);
                sent[j] = c;
            }
            j = j + 1;
        }
    }
    delay(20);
}

void WebCall() {
    webToggle(on, "Servos on|Servos off");
    webSlider(s0, ARM_MIN, ARM_MAX, "1 Base");
    webSlider(s1, ARM_MIN, ARM_MAX, "2 Shoulder");
    webSlider(s2, ARM_MIN, ARM_MAX, "3 Elbow");
    webSlider(s3, ARM_MIN, ARM_MAX, "4 Wrist");
    webSlider(s4, ARM_MIN, ARM_MAX, "5 Wrist roll");
    webSlider(s5, ARM_MIN, ARM_MAX, "6 Gripper");
    webButton(do_center, "Centre all (1500 us)");
    if (pca_ok == 0) { webSend("{s}PCA9685{m}not found{e}"); }
}

void Command(char cmd[]) {
    char a[12];
    char b[12];
    char c[12];
    int j = 0;
    if (strcmp(cmd, "ON") == 0) {
        on = 1;
        responseCmnd("on");
    } else if (strcmp(cmd, "OFF") == 0) {
        on = 0;
        responseCmnd("released");
    } else if (strcmp(cmd, "CENTER") == 0) {
        do_center = 1;
        responseCmnd("centring");
    } else if (strStartsWith(cmd, "US")) {
        strToken(a, cmd, ' ', 2);
        strToken(b, cmd, ' ', 3);
        j = strToInt(a) - 1;
        if (j >= 0 && j < NSRV && strlen(b) > 0) {
            arm_set_slider(j, strToInt(b));
            responseCmnd("ok");
        } else {
            responseCmnd("ArmUs <joint 1..6> <us>");
        }
    } else if (strStartsWith(cmd, "LIMIT")) {
        strToken(a, cmd, ' ', 2);
        strToken(b, cmd, ' ', 3);
        strToken(c, cmd, ' ', 4);
        j = strToInt(a) - 1;
        if (j >= 0 && j < NSRV && strlen(c) > 0) {
            lo[j] = arm_clamp(strToInt(b), 500, 2500);
            hi[j] = arm_clamp(strToInt(c), 500, 2500);
            sprintf(ob, "joint %d limited to %d..%d us", j + 1, lo[j], hi[j]);
            responseCmnd(ob);
        } else {
            responseCmnd("ArmLimit <joint 1..6> <min> <max>");
        }
    } else if (strStartsWith(cmd, "SPEED")) {
        strToken(a, cmd, ' ', 2);
        if (strlen(a) > 0) { speed = arm_clamp(strToInt(a), 1, 200); }
        sprintf(ob, "%d us per 20 ms", speed);
        responseCmnd(ob);
    } else {
        responseCmnd("ArmOn, ArmOff, ArmCenter, ArmUs, ArmLimit, ArmSpeed");
    }
}

// OnExit = program stopped or replaced (CleanUp only runs at a restart): give the I2C address back and release the servos
void OnExit() {
    pca_all_off();
    pca_release();
}

void CleanUp() {
    pca_release();
}

int main() {
    int j = 0;
    while (j < NSRV) { lo[j] = ARM_MIN; hi[j] = ARM_MAX; cur[j] = ARM_CENTRE; sent[j] = 0; j = j + 1; }
    addCommand("Arm");
    if (pca_init(PCA_ADDR, I2C_BUS) == 0) { addLog("ARM: no PCA9685 found"); }
    return 0;
}