Navigation

Arduino - servos example

#include <Servo.h> 
#define number_of_servos 12
#define servo_max_possition 100
#define min_possition 20
#define max_working_servos 6
Servo servos[number_of_servos];
int servoPins[] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13};
int servoStatus[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int servoPosition[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int servoSpeed[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int active_servos = 0;
int i;

void setup() 
{ 
  for (i=0; i<number_of_servos; i++) 
  {
    servos[i].attach(servoPins[i]);
    servos[i].write(min_possition);
    //delay(300);
  }
  //delay(5000);
} 

void losovat() {
  int rnd;
  bool vylosovano = false;
  do
  {
    rnd = random(0, number_of_servos);
    if (servoStatus[rnd] == 0) {
      servoStatus[rnd] = 1;
      servoSpeed[rnd] = random(1, 3);
      vylosovano = true;
      active_servos++;
    }
  } while (vylosovano == false);
}

void pohyb() {
  for (i=0; i<number_of_servos; i++) 
  {
    switch (servoStatus[i]) {
      case 1:
        if (servoPosition[i] > servo_max_possition) {
          servoStatus[i] = 2;
        } else {
          servoPosition[i] = servoPosition[i]+servoSpeed[i];
          servos[i].write(servoPosition[i]);
        }
        break;
      case 2:
        if (servoPosition[i] < min_possition) {
          servoStatus[i] = 0;
          active_servos--;
          servos[i].write(min_possition);
        } else {
          servoPosition[i] = servoPosition[i]-servoSpeed[i];
          servos[i].write(servoPosition[i]);
        }
        break;
    }
  }  
}


void loop()
{
  if (active_servos < max_working_servos && random(0, 15) == 0) {
    losovat();
  }
  pohyb();
  delay(15);
}