Doug Domke
Published © GPL3+

Servo Motor Artwork

We use an Uno to control 36 servo motors and create digital animated artwork.

IntermediateFull instructions provided6,228
Servo Motor Artwork

Things used in this project

Hardware components

Arduino UNO & Genuino UNO
Arduino UNO & Genuino UNO
×1
Adafruit 16 Channel PWM Servo Motor Controller
×3
SG90 Micro-servo motor
SG90 Micro-servo motor
×36
5 volt 30 watt Switching Power Supply
×1
Servo Motor Cable Extensions
×1
24 in. x 48 in. Peg Board
×1
Popsicle Sticks, cut to 3 1/4 inches
×36
Ultrasonic Sensor - HC-SR04 (Generic)
Ultrasonic Sensor - HC-SR04 (Generic)
These components is optional - only required if you are adding the interactive feature.
×5

Software apps and online services

Arduino IDE
Arduino IDE

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
Hot glue gun (generic)
Hot glue gun (generic)

Story

Read more

Schematics

ServosArt Scematic

Shows how Uno and servos to connected through three 16-channel controllers

Optional Circuit for Interactive Mode

Connects 5 ultrasonic distance sensors to Uno to enable servos to track hand motion.

Code

ServosArt Software for Arduino Uno

C/C++
This is the code that controls the servos and provides a three minute repeating show.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
Adafruit_PWMServoDriver pwm3 = Adafruit_PWMServoDriver(0x42);
#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  588 // this is the 'maximum' pulse length count (out of 4096)

byte curPos [36];   // stores 50 - 196 repr pulselen/3 of curr pos
byte tarPos [36];   // stores 50 - 196 repr pulselen/3 of target pos

// defines pins numbers
const int trigPin5 = 9;
const int echoPin5 = 10;
const int trigPin4 = 7;
const int echoPin4 = 8;
const int trigPin3 = 5;
const int echoPin3 = 6;
const int trigPin2 = 3;
const int echoPin2 = 4;
const int trigPin1 = 11;
const int echoPin1 = 12;
long duration;
int distance1, distance2, distance3, distance4, distance5;
int mypos=3;
int counter=0;

// input is row, col
// this table provides the servo number
const byte findNum[8][8] = {
  {0, 36, 36, 36, 36, 36, 36, 36},
  {1, 2, 36, 36, 36, 36, 36, 36},
  {3, 4, 5, 36, 36, 36, 36, 36},
  {6, 7, 8, 9, 36, 36, 36, 36},
  {10, 11, 12, 13, 14, 36, 36, 36},
  {15, 16, 17, 18, 19, 20, 36, 36},
  {21, 22, 23, 24, 25, 26, 27, 36},
  {28, 29, 30, 31, 32, 33, 34, 35}
};

const byte track[8][5] = {
{175,150,125,90,65},
{165,145,125,95,70},
{155,140,125,100,75},
{150,137,125,105,83},
{145,135,125,108,90},
{140,133,125,112,97},
{135,130,125,115,105},
{130,127,125,118,110}
};

const byte positions[5][36]={
{184,174,180,162,169,177,150,162,170,175,140,153,162,169,174,131,144,154,162,168,172,125,136,154,155,162,167,171,119,130,140,149,156,162,167,171},
{171,153,169,136,155,167,125,143,157,166,117,132,147,158,166,111,125,138,149,158,165,107,119,138,142,151,159,165,105,114,125,135,144,153,159,165},
{125,109,140,102,125,147,99,115,134,150,96,109,125,140,153,95,105,118,131,144,149,94,102,118,125,136,147,155,93,100,109,119,130,140,149,157},
{78,80,96,82,94,113,83,92,106,125,83,91,102,117,132,84,91,100,111,125,138,84,90,100,107,119,130,142,84,90,96,105,114,125,135,144},
{65,69,75,72,78,87,74,79,87,99,75,80,87,96,109,77,81,87,95,105,118,78,82,87,94,102,113,125,78,82,87,93,100,109,119,130}
};

const byte spiral[36] = {0,1,3,6,10,15,21,28,29,30,31,32,33,34,35,27,20,14,9,5,2,4,7,11,16,22,23,24,25,26,19,13,8,12,17,18};

void setup() {
  pinMode(trigPin1, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin1, INPUT); // Sets the echoPin as an Input
  pinMode(trigPin2, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin2, INPUT); // Sets the echoPin as an Input
  pinMode(trigPin3, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin3, INPUT); // Sets the echoPin as an Input
  pinMode(trigPin4, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin4, INPUT); // Sets the echoPin as an Input
  pinMode(trigPin5, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin5, INPUT); // Sets the echoPin as an Input

  pwm.begin();
  pwm2.begin();
  pwm3.begin();

  pwm.setPWMFreq(60);
  pwm2.setPWMFreq(60);
  pwm3.setPWMFreq(60);
  delay(10);
  // set all servos to midpoint pos and intialize tables
  for (int myservo = 0; myservo < 36; myservo++) {
    if (myservo < 15)  pwm.setPWM(myservo, 0, 375);
    if (myservo > 14 && myservo < 28) pwm2.setPWM(myservo - 15, 0, 375);
    if (myservo > 27) pwm3.setPWM(myservo - 28, 0, 375);
    curPos[myservo] = 125;
    tarPos[myservo] = 125;
  }

}

void loop() {
  for (int count = 0; count < 5; count=count+2) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      setServo2(spiral[myservo], count);
      goToTargets(12, 2);
    }
 /*  for (int myservo  = 35; myservo > -1; myservo--) {
    if (count<4){
      setServo2(spiral[myservo], count+1);
     goToTargets(5, 3);
     } 
   }*/
  }
  for (int count = 3; count > 0; count=count-2) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      setServo2(spiral[myservo], count);
      goToTargets(12, 2);
    }
 /*  for (int myservo  = 35; myservo > -1; myservo--) {
    if (count>1){
      setServo2(spiral[myservo], count-1);
     goToTargets(5, 3);
     } 
   }*/
  }

  // interesting effects using the diagonals but with variable angles
  for (int count2 = 0; count2 < 2; count2++) {
    for (int count = 0; count < 8; count++) {
      rightDiag2(count + 1 , 14);
      goToTargets(20, 2 - count / 4);
    }
    delay(500);
    for (int count = 0; count < 8; count++) {
      leftDiag2(count + 1 , 15);
      goToTargets(20, 2 - count / 4);
    }
    delay(500);
  }
  for (int count2 = 0; count2 < 4; count2++) {
    for (int count = 0; count < 8; count++) {
      rightDiag2(count + 1 , 14);
    }
    goToTargets(15, 8);
    for (int count = 0; count < 8; count++) {
      leftDiag2(count + 1 , 15);
    }
    goToTargets(15, 8);
  }

  // now just using regular diagonls where all along diagonal are the same
  for (int count = 7; count > -1; count--) {
    rightDiag(count + 1 , 4);
    goToTargets(20, 2 - count / 4);
  }
  for (int count = 7; count > -1; count--) {
    rows(count + 1 , 1);
    goToTargets(20, 2 - count / 4);
  }
  for (int count = 7; count > -1; count--) {
    leftDiag(count + 1 , 0);
    goToTargets(20, 2 - count / 4);
  }
  for (int count = 7; count > -1; count--) {
    rows(count + 1 , 3);
    goToTargets(20, 2 - count / 4);
  }
  for (int count = 0; count < 8; count++) {
    leftDiag(count + 1 , 1);
    goToTargets(20, 2 - count / 4);
  }
  delay(300);
  for (int count = 0; count < 8; count++) {
    if (count % 2) rightDiag(count + 1 , 3);
    goToTargets(20, 2 - count / 4);
  }
  for (int myservo  = 0; myservo < 36; myservo++) {
    setTarget(myservo, 4);
  }
  // Wave routine
  for (int count = 0; count < 120; count++) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      if (curPos[myservo] < 51 && tarPos[myservo] == 50) tarPos[myservo] = 196;
      if (curPos[myservo] > 195 && tarPos[myservo] == 196) tarPos[myservo] = 50;
    }
    goToTargets(0, 0);
  }
  for (int myservo = 0; myservo < 36; myservo++) {
    setServo2(myservo, 2);
  }
  goToTargets(36, 0);

  //Setup for first wave
  for (int servoNum = 0; servoNum < 36; servoNum++) {
    tarPos[servoNum] = 50 + servoNum * 2;
  }
  goToTargets(36, 0);
  for (int myservo  = 0; myservo < 36; myservo++) {
    setTarget(myservo, 4);
  }
  // Wave routine
  for (int count = 0; count < 120; count++) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      if (curPos[myservo] < 51 && tarPos[myservo] == 50) tarPos[myservo] = 196;
      if (curPos[myservo] > 195 && tarPos[myservo] == 196) tarPos[myservo] = 50;
    }
    goToTargets(0, 0);
  }
  // set all to pos 0
  for (int myservo = 0; myservo < 36; myservo++) {
    setServo2(myservo, 0);
  }
  goToTargets(36, 0);
  for (int count = 0; count < 8; count++) {
    if (count % 2) rightDiag(count + 1 , 3);
    else rightDiag(count + 1 , 1);
    goToTargets(20, 2 - count / 4);
  }
  for (int myservo = 0; myservo < 36; myservo++) {
    setServo2(myservo, 4);
  }
  goToTargets(36, 0);
  for (int count = 0; count < 8; count++) {
    if (count % 2) leftDiag(count + 1 , 1);
    else leftDiag(count + 1 , 3);
    goToTargets(20, 2 - count / 4);
  }

  for (int myservo = 0; myservo < 36; myservo++) {
    setServo2(myservo, 0);
  }
  goToTargets(36, 0);
  for (int count = 0; count < 8; count++) {
    if (count % 2) rows(count + 1 , 1);
    else rows(count + 1 , 3);
    goToTargets(20, 2 - count / 4);
  }

  for (int count = 0; count < 8; count++) {
    if (count % 2) rows(count + 1 , 3);
    else rows(count + 1 , 1);
    goToTargets(20, 2 - count / 4);
  }

  for (int count = 0; count < 8; count++) {
    if (count % 2) rows(count + 1 , 1);
    else rows(count + 1 , 3);
    goToTargets(20, 2 - count / 4);
  }

  for (int count = 0; count < 8; count++) {
    if (count % 2) rows(count + 1 , 3);
    else rows(count + 1 , 1);
    goToTargets(20, 2 - count / 4);
  }
  piramid ();
  for (int count = 0; count < 8; count++) {
    rightDiag(count + 1 , 3);
    goToTargets(20, 2 - count / 4);
  }
  for (int count = 0; count < 8; count++) {
    leftDiag(count + 1 , 1);
    goToTargets(20, 2 - count / 4);
  }
  for (int count = 0; count < 8; count++) {
    rows(count + 1 , 0);
    goToTargets(20, 2 - count / 4);
  }
  // set up for the Ts wave
  for (int count = 0; count < 8; count++) {
    if (count % 2 == 0)rows(count + 1 , 4);
  }
  goToTargets(15, 3);
  //wave routine
  for (int count = 0; count < 240; count++) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      if (curPos[myservo] < 51 && tarPos[myservo] == 50) tarPos[myservo] = 196;
      if (curPos[myservo] > 195 && tarPos[myservo] == 196) tarPos[myservo] = 50;
    }
    goToTargets(0, 0);
  }
  setForWave();  // set up for second wave
  delay(300);
  //wave routine
  for (int count = 0; count < 240; count++) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      if (curPos[myservo] < 51 && tarPos[myservo] == 50) tarPos[myservo] = 196;
      if (curPos[myservo] > 195 && tarPos[myservo] == 196) tarPos[myservo] = 50;
    }
    goToTargets(0, 0);
  }
  setAllZero();
  delay(300);

  triangles();
  triangles();

  for (int count = 0; count < 36; count++) {
    setServo2(count, 3);
  }
  goToTargets(21, 15);
  for (int count = 0; count < 36; count++) {
    setServo2(count, 1);
  }
  goToTargets(36, 0);
  for (int count = 0; count < 36; count++) {
    setServo2(count, 3);
  }
  goToTargets(36, 0);
  for (int count = 0; count < 36; count++) {
    setServo2(count, 1);
  }
  goToTargets(20, 0);

  }

// this is the primary routine for moving the servos, movint them from there current position toward their target postions
void goToTargets(int reps, int mydelay) {
  for (int count = 0; count < 5 + 5 * reps; count++) {
    for (int myservo  = 0; myservo < 36; myservo++) {
      if (curPos[myservo] > tarPos[myservo]) curPos[myservo]--;
      if (curPos[myservo] < tarPos[myservo]) curPos[myservo]++;
      if (curPos[myservo] != tarPos[myservo]) {
        if (myservo < 15)  pwm.setPWM(myservo, 0, 3 * curPos[myservo]);
        if (myservo > 14 && myservo < 28) pwm2.setPWM(myservo - 15, 0, 3 * curPos[myservo]);
        if (myservo > 27) pwm3.setPWM(myservo - 28, 0, 3 * curPos[myservo]);
      }
    }
    delay(mydelay);
  }
  // this is where we check if hand is over the center distance sensor; if so we go into tracker mode
  digitalWrite(trigPin3, LOW);
  delayMicroseconds(2);
  // Sets the trigPin3 on HIGH state for 10 micro seconds
  digitalWrite(trigPin3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin3, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin3, HIGH, 2500);
  if (duration > 0) {
    counter = 0;
    trackRoutine();
  }
}
// this sets the target of a servo to one of five preset positions
void setTarget(int myservo, int pos) {
  tarPos[myservo] = pos * 38 + 50;
  if (tarPos[myservo] > 196)tarPos[myservo] = 196;
}
// this sets all servos to their minimum positions
void setAllZero() {
  for (int myservo  = 0; myservo < 36; myservo++) {
    tarPos[myservo] = 50;
  }
  for (int count = 0; count < 30; count++) {
    goToTargets(0, 0);
  }
}
// set target position of servo in rows 1-8 
// and coluumns 1-8, addressing them by row and column
// to one of the 5 preset positions
void setServo(int row, int col,  int myPos) {
  int servoNum = findNum[row - 1][col - 1];
  if (myPos == 0) tarPos[servoNum] = 50;
  else if (myPos == 4) tarPos[servoNum] = 196;
  else if (myPos == 2) tarPos[servoNum] = 125;
  else if (myPos == 1) tarPos[servoNum] = 93;
  else if (myPos == 3) tarPos[servoNum] = 153;
}
// set target position of servo by servo #
// this is a duplicate of setTarget
void setServo2(int servoNum, int myPos) {
  if (myPos == 0) tarPos[servoNum] = 50;
  else if (myPos == 4) tarPos[servoNum] = 196;
  else if (myPos == 2) tarPos[servoNum] = 125;
  else if (myPos == 1) tarPos[servoNum] = 93;
  else if (myPos == 3) tarPos[servoNum] = 153;
}

// set target position of servo in rows 1-8 and coluumns 1-8
// special version used by rightdiag2 and leftdiag2
void setServo3(int row, int col,  int num, int mult2) {
  int servoNum = findNum[row - 1][col - 1];
  if (mult2 % 2 == 0)tarPos[servoNum] = 180 - num * mult2;
  else tarPos[servoNum] = 60 + num * mult2;
}

// sets servo to point to target at row 0 and targetX
// this uses arctan to point servo at a preset position above the board
void setServo4(int row, int col, int targetX) {
  int servoNum = findNum[row - 1][col - 1];
  int trueCol = col + 8 - row;
  double angle = 1.57 + atan2(targetX - trueCol, row);
  tarPos[servoNum] = int(188 - 138 * angle / 3.14);
}

//setup for wave; not very efficient way to do it
void setForWave() {
  int mult = 50;
  tarPos[0] = 50 + 0;
  for (int myservo  = 1; myservo < 3; myservo++) {  // row 1
    tarPos[myservo] = 50 + (myservo - 1) * mult / 3;
  }
  for (int myservo  = 3; myservo < 6; myservo++) {  // row 2
    tarPos[myservo] = 50 + (myservo - 3) * mult / 3;
  }
  for (int myservo  = 6; myservo < 10; myservo++) {  // row 3
    tarPos[myservo] = 50 + (myservo - 6) * mult / 3;
  }
  for (int myservo  = 10; myservo < 15; myservo++) {  // row 4
    tarPos[myservo] = 50 + (myservo - 10) * mult / 3;
  }
  for (int myservo  = 15; myservo < 21; myservo++) {  // row 5
    tarPos[myservo] = 50 + (myservo - 15) * mult / 3;
  }
  for (int myservo  = 21; myservo < 28; myservo++) {  // row 6
    tarPos[myservo] = 50 + (myservo - 21) * mult / 3;
  }
  for (int myservo  = 28; myservo < 36; myservo++) { // row 7
    tarPos[myservo] = 50 + (myservo - 28) * mult / 3;
  }
  goToTargets(36, 0);
  for (int myservo  = 0; myservo < 36; myservo++) {
    tarPos[myservo] = 196;
  }
}
//  the triagle patterns
void triangles () {
  setServo2(0, 0);
  setServo2(1, 0);
  setServo2(2, 0);
  setServo2(3, 0);
  setServo2(4, 0);
  setServo2(5, 0);
  setServo2(6, 2);
  setServo2(7, 2);
  setServo2(8, 2);
  setServo2(9, 2);
  setServo2(10, 0);
  setServo2(11, 2);
  setServo2(12, 2);
  setServo2(13, 2);
  setServo2(14, 0);
  setServo2(15, 0);
  setServo2(16, 0);
  setServo2(17, 2);
  setServo2(18, 2);
  setServo2(19, 0);
  setServo2(20, 0);
  setServo2(21, 0);
  setServo2(22, 0);
  setServo2(23, 0);
  setServo2(24, 2);
  setServo2(25, 0);
  setServo2(26, 0);
  setServo2(27, 0);
  for (int count = 28; count < 36; count++) {
    setServo2(count, 0);
  }
  goToTargets(15, 0);
  delay(500);
  setServo2(0, 2);
  setServo2(1, 2);
  setServo2(2, 2);
  setServo2(3, 2);
  setServo2(4, 2);
  setServo2(5, 2);
  setServo2(6, 0);
  setServo2(7, 0);
  setServo2(8, 0);
  setServo2(9, 0);
  setServo2(10, 2);
  setServo2(11, 0);
  setServo2(12, 0);
  setServo2(13, 0);
  setServo2(14, 2);
  setServo2(15, 2);
  setServo2(16, 2);
  setServo2(17, 0);
  setServo2(18, 0);
  setServo2(19, 2);
  setServo2(20, 2);
  setServo2(21, 2);
  setServo2(22, 2);
  setServo2(23, 2);
  setServo2(24, 0);
  setServo2(25, 2);
  setServo2(26, 2);
  setServo2(27, 2);
  for (int count = 28; count < 36; count++) {
    setServo2(count, 2);
  }
  goToTargets(15, 0);
  delay(500);
}

// way to progressively do something along right diagonal
void rightDiag(int num, int pos) {
  if (num == 1) {
    setServo(8, 8, pos);
  }
  if (num == 2) {
    setServo(7, 7, pos);
    setServo(8, 7, pos);
  }
  if (num == 3) {
    setServo(6, 6, pos);
    setServo(7, 6, pos);
    setServo(8, 6, pos);
  }
  if (num == 4) {
    setServo(5, 5, pos);
    setServo(6, 5, pos);
    setServo(7, 5, pos);
    setServo(8, 5, pos);
  }
  if (num == 5) {
    setServo(4, 4, pos);
    setServo(5, 4, pos);
    setServo(6, 4, pos);
    setServo(7, 4, pos);
    setServo(8, 4, pos);
  }
  if (num == 6) {
    setServo(3, 3, pos);
    setServo(4, 3, pos);
    setServo(5, 3, pos);
    setServo(6, 3, pos);
    setServo(7, 3, pos);
    setServo(8, 3, pos);
  }
  if (num == 7) {
    setServo(2, 2, pos);
    setServo(3, 2, pos);
    setServo(4, 2, pos);
    setServo(5, 2, pos);
    setServo(6, 2, pos);
    setServo(7, 2, pos);
    setServo(8, 2, pos);
  }
  if (num == 8) {
    setServo(1, 1, pos);
    setServo(2, 1, pos);
    setServo(3, 1, pos);
    setServo(4, 1, pos);
    setServo(5, 1, pos);
    setServo(6, 1, pos);
    setServo(7, 1, pos);
    setServo(8, 1, pos);
  }
}

// way to progressively do something along the left diagonal
void leftDiag(int num, int pos) {
  if (num == 1) {
    setServo(8, 1, pos);
  }
  if (num == 2) {
    setServo(7, 1, pos);
    setServo(8, 2, pos);
  }
  if (num == 3) {
    setServo(6, 1, pos);
    setServo(7, 2, pos);
    setServo(8, 3, pos);
  }
  if (num == 4) {
    setServo(5, 1, pos);
    setServo(6, 2, pos);
    setServo(7, 3, pos);
    setServo(8, 4, pos);
  }
  if (num == 5) {
    setServo(4, 1, pos);
    setServo(5, 2, pos);
    setServo(6, 3, pos);
    setServo(7, 4, pos);
    setServo(8, 5, pos);
  }
  if (num == 6) {
    setServo(3, 1, pos);
    setServo(4, 2, pos);
    setServo(5, 3, pos);
    setServo(6, 4, pos);
    setServo(7, 5, pos);
    setServo(8, 6, pos);
  }
  if (num == 7) {
    setServo(2, 1, pos);
    setServo(3, 2, pos);
    setServo(4, 3, pos);
    setServo(5, 4, pos);
    setServo(6, 5, pos);
    setServo(7, 6, pos);
    setServo(8, 7, pos);
  }
  if (num == 8) {
    setServo(1, 1, pos);
    setServo(2, 2, pos);
    setServo(3, 3, pos);
    setServo(4, 4, pos);
    setServo(5, 5, pos);
    setServo(6, 6, pos);
    setServo(7, 7, pos);
    setServo(8, 8, pos);
  }
}
// like the diagonals except progressively through the rows
void rows(int num, int pos) {
  if (num == 1) {
    setServo(1, 1, pos);
  }
  if (num == 2) {
    setServo(2, 1, pos);
    setServo(2, 2, pos);
  }
  if (num == 3) {
    setServo(3, 1, pos);
    setServo(3, 2, pos);
    setServo(3, 3, pos);
  }
  if (num == 4) {
    setServo(4, 1, pos);
    setServo(4, 2, pos);
    setServo(4, 3, pos);
    setServo(4, 4, pos);
  }
  if (num == 5) {
    setServo(5, 1, pos);
    setServo(5, 2, pos);
    setServo(5, 3, pos);
    setServo(5, 4, pos);
    setServo(5, 5, pos);
  }
  if (num == 6) {
    setServo(6, 1, pos);
    setServo(6, 2, pos);
    setServo(6, 3, pos);
    setServo(6, 4, pos);
    setServo(6, 5, pos);
    setServo(6, 6, pos);
  }
  if (num == 7) {
    setServo(7, 1, pos);
    setServo(7, 2, pos);
    setServo(7, 3, pos);
    setServo(7, 4, pos);
    setServo(7, 5, pos);
    setServo(7, 6, pos);
    setServo(7, 7, pos);
  }
  if (num == 8) {
    setServo(8, 1, pos);
    setServo(8, 2, pos);
    setServo(8, 3, pos);
    setServo(8, 4, pos);
    setServo(8, 5, pos);
    setServo(8, 6, pos);
    setServo(8, 7, pos);
    setServo(8, 8, pos);
  }
}
//  a particular sequence of patterns
void piramid () {
  setServo(2, 1, 1);
  setServo(2, 2, 3);
  goToTargets(10, 3);
  setServo(3, 1, 1);
  setServo(3, 3, 3);
  goToTargets(10, 3);
  setServo(4, 1, 1);
  setServo(4, 3, 3);
  setServo(4, 2, 1);
  setServo(4, 4, 3);
  goToTargets(10, 3);
  setServo(5, 1, 1);
  setServo(5, 4, 3);
  setServo(5, 2, 1);
  setServo(5, 5, 3);
  goToTargets(10, 3);
  setServo(6, 1, 1);
  setServo(6, 2, 1);
  setServo(6, 3, 1);
  setServo(6, 4, 3);
  setServo(6, 5, 3);
  setServo(6, 6, 3);
  goToTargets(10, 3);
  setServo(7, 1, 1);
  setServo(7, 2, 1);
  setServo(7, 3, 1);
  setServo(7, 5, 3);
  setServo(7, 6, 3);
  setServo(7, 7, 3);
  goToTargets(10, 3);
  setServo(8, 1, 1);
  setServo(8, 2, 1);
  setServo(8, 3, 1);
  setServo(8, 4, 1);
  setServo(8, 5, 3);
  setServo(8, 6, 3);
  setServo(8, 7, 3);
  setServo(8, 8, 3);
  goToTargets(10, 3);
  delay (500);
  setServo(1, 1, 1);
  setServo(3, 2, 1);
  setServo(5, 3, 1);
  setServo(7, 4, 1);
  goToTargets(20, 3);
  setServo(1, 1, 3);
  setServo(3, 2, 3);
  setServo(5, 3, 3);
  setServo(7, 4, 3);
  goToTargets(20, 3);
  setServo(1, 1, 1);
  setServo(3, 2, 1);
  setServo(5, 3, 1);
  setServo(7, 4, 1);
  goToTargets(20, 3);
  setServo(4, 3, 1);
  setServo(5, 4, 1);
  setServo(6, 4, 1);
  setServo(7, 5, 1);
  setServo(8, 5, 1);
  goToTargets(20, 3);
  setServo(6, 5, 1);
  setServo(7, 6, 1);
  setServo(8, 6, 1);
  setServo(8, 7, 1);
  goToTargets(20, 3);
  setServo(2, 2, 1);
  setServo(3, 3, 1);
  setServo(4, 4, 1);
  setServo(5, 5, 1);
  setServo(6, 6, 1);
  setServo(7, 7, 1);
  setServo(8, 8, 1);
  goToTargets(20, 3);
}

void rightDiag2(int num, int mult) {
  if (num == 1) {
    setServo3(8, 8, 1, mult);
  }
  if (num == 2) {
    setServo3(7, 7, 1, mult);
    setServo3(8, 7, 2, mult);
  }
  if (num == 3) {
    setServo3(6, 6, 1, mult);
    setServo3(7, 6, 2, mult);
    setServo3(8, 6, 3, mult);
  }
  if (num == 4) {
    setServo3(5, 5, 1, mult);
    setServo3(6, 5, 2, mult);
    setServo3(7, 5, 3, mult);
    setServo3(8, 5, 4, mult);
  }
  if (num == 5) {
    setServo3(4, 4, 1, mult);
    setServo3(5, 4, 2, mult);
    setServo3(6, 4, 3, mult);
    setServo3(7, 4, 4, mult);
    setServo3(8, 4, 5, mult);
  }
  if (num == 6) {
    setServo3(3, 3, 1, mult);
    setServo3(4, 3, 2, mult);
    setServo3(5, 3, 3, mult);
    setServo3(6, 3, 4, mult);
    setServo3(7, 3, 5, mult);
    setServo3(8, 3, 6, mult);
  }
  if (num == 7) {
    setServo3(2, 2, 1, mult);
    setServo3(3, 2, 2, mult);
    setServo3(4, 2, 3, mult);
    setServo3(5, 2, 4, mult);
    setServo3(6, 2, 5, mult);
    setServo3(7, 2, 6, mult);
    setServo3(8, 2, 7, mult);
  }
  if (num == 8) {
    setServo3(1, 1, 1, mult);
    setServo3(2, 1, 2, mult);
    setServo3(3, 1, 3, mult);
    setServo3(4, 1, 4, mult);
    setServo3(5, 1, 5, mult);
    setServo3(6, 1, 6, mult);
    setServo3(7, 1, 7, mult);
    setServo3(8, 1, 8, mult);
  }
}
void leftDiag2(int num, int mult) {
  if (num == 1) {
    setServo3(8, 1, 1, mult);
  }
  if (num == 2) {
    setServo3(7, 1, 1, mult);
    setServo3(8, 2, 2, mult);
  }
  if (num == 3) {
    setServo3(6, 1, 1, mult);
    setServo3(7, 2, 2, mult);
    setServo3(8, 3, 3, mult);
  }
  if (num == 4) {
    setServo3(5, 1, 1, mult);
    setServo3(6, 2, 2, mult);
    setServo3(7, 3, 3, mult);
    setServo3(8, 4, 4, mult);
  }
  if (num == 5) {
    setServo3(4, 1, 1, mult);
    setServo3(5, 2, 2, mult);
    setServo3(6, 3, 3, mult);
    setServo3(7, 4, 4, mult);
    setServo3(8, 5, 5, mult);
  }
  if (num == 6) {
    setServo3(3, 1, 1, mult);
    setServo3(4, 2, 2, mult);
    setServo3(5, 3, 3, mult);
    setServo3(6, 4, 4, mult);
    setServo3(7, 5, 5, mult);
    setServo3(8, 6, 6, mult);
  }
  if (num == 7) {
    setServo3(2, 1, 1, mult);
    setServo3(3, 2, 2, mult);
    setServo3(4, 3, 3, mult);
    setServo3(5, 4, 4, mult);
    setServo3(6, 5, 5, mult);
    setServo3(7, 6, 6, mult);
    setServo3(8, 7, 7, mult);
  }
  if (num == 8) {
    setServo3(1, 1, 1, mult);
    setServo3(2, 2, 2, mult);
    setServo3(3, 3, 3, mult);
    setServo3(4, 4, 4, mult);
    setServo3(5, 5, 5, mult);
    setServo3(6, 6, 6, mult);
    setServo3(7, 7, 7, mult);
    setServo3(8, 8, 8, mult);
  }
}

// this routine reads five distance sensors and points all servos at the position where hand is sensed
// it senses positions over each of the 5 sensors and 4 postitions between them for 9 possible postitions
void trackRoutine() {
  int mydelay = 5;
  int mycount = 5;
  int mytime = 2000;

  for (int myservo = 0; myservo < 36; myservo++) {
    tarPos[myservo] = positions[2][myservo];
  }
  goToTargets(36, 0);
  while (counter < 25) {
    distance1 = 0; distance2 = 0; distance3 = 0; distance4 = 0; distance5 = 0;
    for (int count = 0; count < 10; count++) {
      digitalWrite(trigPin1, LOW);
      delayMicroseconds(2);
      // Sets the trigPin3 on HIGH state for 10 micro seconds
      digitalWrite(trigPin1, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin1, LOW);
      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration = pulseIn(echoPin1, HIGH, mytime);
      if (duration == 0)duration = 2941;
      // Calculating the distance
      distance1 = distance1 + duration * 0.034 / 2;
      digitalWrite(trigPin2, LOW);
      delayMicroseconds(2);
      // Sets the trigPin3 on HIGH state for 10 micro seconds
      digitalWrite(trigPin2, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin2, LOW);
      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration = pulseIn(echoPin2, HIGH, mytime);
      if (duration == 0)duration = 2941;
      // Calculating the distance
      distance2 = distance2 + duration * 0.034 / 2;

      digitalWrite(trigPin3, LOW);
      delayMicroseconds(2);
      // Sets the trigPin3 on HIGH state for 10 micro seconds
      digitalWrite(trigPin3, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin3, LOW);
      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration = pulseIn(echoPin3, HIGH, mytime);
      if (duration == 0)duration = 2941;
      // Calculating the distance
      distance3 = distance3 + duration * 0.034 / 2;

      digitalWrite(trigPin4, LOW);
      delayMicroseconds(2);
      // Sets the trigPin1 on HIGH state for 10 micro seconds
      digitalWrite(trigPin4, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin4, LOW);
      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration = pulseIn(echoPin4, HIGH, mytime);
      // Calculating the distance
      if (duration == 0)duration = 2941;
      distance4 = distance4 + duration * 0.034 / 2;

      digitalWrite(trigPin5, LOW);
      delayMicroseconds(2);
      // Sets the trigPin1 on HIGH state for 10 micro seconds
      digitalWrite(trigPin5, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin5, LOW);
      // Reads the echoPin, returns the sound wave travel time in microseconds
      duration = pulseIn(echoPin5, HIGH, mytime);
      if (duration == 0)duration = 2941;
      // Calculating the distance
      distance5 = distance5 + duration * 0.034 / 2;
    }
    distance1 = 49 - distance1 / 10;
    distance2 = 49 - distance2 / 10;
    distance3 = 49 - distance3 / 10;
    distance4 = 49 - distance4 / 10;
    distance5 = 49 - distance5 / 10;
    int score = 0;
    if (distance1 > 0 and distance2 == 0) score = 1;
    else if (distance1 > 0 and distance2 > 0) score = 2;
    else if (distance2 > 0 and distance3 == 0) score = 3;
    else if (distance2 > 0 and distance3 > 0) score = 4;
    else if (distance3 > 0 and distance4 == 0) score = 5;
    else if (distance3 > 0 and distance4 > 0) score = 6;
    else if (distance4 > 0 and distance5 == 0) score = 7;
    else if (distance4 > 0 and distance5 > 0) score = 8;
    else if (distance5 > 0) score = 9;
    if (score > 0) {
      counter = 0;
      for (int myservo = 0; myservo < 36; myservo++) {
        if (score % 2 == 1) tarPos[myservo] = positions[score / 2][myservo];
        else tarPos[myservo] = (positions[score / 2][myservo] + positions[ score / 2 - 1][myservo]) / 2;
      }
      for (int count = 0; count < 5 + 5 * mycount; count++) {
        for (int myservo  = 0; myservo < 36; myservo++) {
          if (curPos[myservo] > tarPos[myservo]) curPos[myservo]--;
          if (curPos[myservo] < tarPos[myservo]) curPos[myservo]++;
          if (curPos[myservo] != tarPos[myservo]) {
            if (myservo < 15)  pwm.setPWM(myservo, 0, 3 * curPos[myservo]);
            if (count<25 && myservo > 14 && myservo < 28) pwm2.setPWM(myservo - 15, 0, 3 * curPos[myservo]);
            if (count <20 && myservo > 27) pwm3.setPWM(myservo - 28, 0, 3 * curPos[myservo]);
          }
        }
        delay(mydelay);
      }

    }
    counter++;
  }
}

Set Servos to Minimum

C/C++
Use this with one or more PWM controllers to set all servos to the minimum position in preparation for mounting on the board
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
 Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
 Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
 Adafruit_PWMServoDriver pwm3 = Adafruit_PWMServoDriver(0x42);




#define SERVOMIN  150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  588 // this is the 'maximum' pulse length count (out of 4096)

void setup() {
  pwm.begin();
  pwm2.begin();
  pwm3.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  pwm2.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  pwm3.setPWMFreq(60);
  delay(10);
  
}

void loop() {
for (int servonum=0; servonum<16; servonum++ ){
    pwm.setPWM(servonum, 0, 150);
    pwm2.setPWM(servonum, 0, 150);
    pwm3.setPWM(servonum, 0, 150);
  }
delay(10000);
}

Credits

Doug Domke

Doug Domke

0 projects • 5 followers

Comments