Learn. Create. Enjoy.
Learn how to assemble the Digger add-on for Stackr and program two servo motors that control the parallelogram scoop mechanism.
The easiest way to assemble the Digger is by following the instructions in the video below.
This program runs through several cycles to move the arms of the digger and tilt the scoop. Servo 1 on D10 drives the arm; servo 2 on D9 tilts the scoop. A control_mode state machine steps each servo slowly between 45° and 135°, then returns both to center.
#include <ESP32Servo.h>
Servo servo1;
Servo servo2;
int angle1 = 90;
int angle2 = 90;
int control_mode = 0;
void setup()
{
ESP32PWM::allocateTimer(0);
ESP32PWM::allocateTimer(1);
ESP32PWM::allocateTimer(2);
ESP32PWM::allocateTimer(3);
//Initialize servos at 90 degrees
servo1.setPeriodHertz(50);
servo1.attach(D10);
servo1.write(angle1);
servo2.setPeriodHertz(50);
servo2.attach(D9);
servo2.write(angle2);
}
void loop()
{
if (control_mode == 0) {
if (angle1 < 135) {
angle1++;
servo1.write(angle1);
delay(50);
} else {
control_mode = 1;
}
}
else if (control_mode == 1) {
if (angle1 > 45) {
angle1--;
servo1.write(angle1);
delay(50);
} else {
control_mode = 2;
}
}
else if (control_mode == 2) {
if (angle2 < 135) {
angle2++;
servo2.write(angle2);
delay(50);
} else {
control_mode = 3;
}
}
else if (control_mode == 3) {
if (angle2 > 45) {
angle2--;
servo2.write(angle2);
delay(50);
} else {
control_mode = 4;
}
}
else if (control_mode == 4) {
if (angle2 < 90 || angle1 < 90) {
angle1++;
angle2++;
servo1.write(angle1);
servo2.write(angle2);
delay(50);
} else {
control_mode = 0; // loop back
}
}
}Library and Servos: ESP32Servo drives two servos—servo1 (arm, D10) and servo2 (scoop, D9)—both starting at 90°.
#include <ESP32Servo.h>
Servo servo1;
Servo servo2;
int angle1 = 90;
int angle2 = 90;
int control_mode = 0;Mode 0 — Raise arm: angle1 steps from 90° up to 135°.
Mode 1 — Lower arm: angle1 steps from 135° down to 45°.
Mode 2 — Tilt scoop up: angle2 steps from 90° up to 135°.
Mode 3 — Tilt scoop down: angle2 steps from 135° down to 45°.
Mode 4 — Return to center: Both angles increment until they reach 90°, then the cycle restarts at mode 0.
else if (control_mode == 4) {
if (angle2 < 90 || angle1 < 90) {
angle1++;
angle2++;
servo1.write(angle1);
servo2.write(angle2);
delay(50);
} else {
control_mode = 0;
}
}Install the ESP32Servo library from the Arduino Library Manager (Sketch → Include Library → Manage Libraries, then search for “ESP32Servo”).