// ServoOverdone.ino
//
// Example for multiple Servo objects in a array.
//
// Version 1, 28 July 2021, by Koepel.
// Version 2, 15 August 2021, by Koepel.
//   changed timing, a little slower
//   diagram.json has servos in reverse order (I think it is visually better)
//   Added fourth sequence: "compass"
//
// Public Domain
//

#include <Servo.h>

#define NUM_SERVOS 32
Servo myServo[NUM_SERVOS];

void setup() 
{
  // Attach pins from the Arduino Mega board to the Servo objects.
  // Starting from pin 22, there happen to be exactly 32 pins on the double row pins.
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].attach( i + 22);      // pin 22 up to 53 is 32 pins
  }
}

void loop() 
{
  // Sequence one.
  // All servo motor are set to a random angle.
  for( int a=0; a<15; a++)
  {
    for( int i=0; i<NUM_SERVOS; i++)
    {
      myServo[i].write( random( 0, 181));
      delay( 2);
    }
    delay( 150);
  }

  // Sequence two.
  // All servo motors move with the same angle.
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].write( 0);            // set to begin position (horn is rotated left)
  }
  delay( 1000);                      // wait to let the viewer get used to it

  for( int a=0; a<3; a++)
  {
    for( int r=0; r<=180; r++)       // move horns to the right
    {
      for( int i=0; i<NUM_SERVOS; i++)
      {
        myServo[i].write( r);
      }
      delay( 6);
    }
    for( int r=180; r>=0; r--)
    {
      for( int i=0; i<NUM_SERVOS; i++)  // move horns to the left
      {
        myServo[i].write( r);
      }
      delay( 6);
    }
  }

  // Sequence three.
  // A rotating wave.
  for( int a=0; a<6; a++)
  {
    for( int i=0; i<NUM_SERVOS; i++)
    {
      for( int j=0; j<NUM_SERVOS; j++)
      {
        // Calculate distance to active servo
        int d = j - i;
        if( d < 0)
          d = -d;
        if( d > (NUM_SERVOS / 2))
          d = NUM_SERVOS - d;

        int angle = 90 - (10 * d);
        if( angle < 0)
          angle = 0;
        myServo[j].write( angle);
      }
      delay(40);
    }
  }

  // Sequence four.
  // A "compass"
  // Start by pointing upwards
  int pointer = NUM_SERVOS * 3 / 4;
  showPointer( pointer);
  delay( 1000);                       // let the viewer get used to new pattern

  for( int i=0; i<5; i++)
  {
    showPointer( --pointer);
    delay( 150);
  }
  delay( 200);
  for( int i=0; i<9; i++)
  {
    showPointer( ++pointer);
    delay( 150);
  }
  delay( 200);
  for( int i=0; i<5; i++)
  {
    showPointer( --pointer);
    delay( 150);
  }
  delay( 200);
  for( int i=0; i<4; i++)
  {
    showPointer( ++pointer);
    delay( 150);
  }
  delay( 160);
  for( int i=0; i<2; i++)
  {
    showPointer( --pointer);
    delay( 150);
  }
  delay( 80);
  for( int i=0; i<1; i++)
  {
    showPointer( ++pointer);
    delay( 150);
  }

  delay( 2000);
}

// This function makes a "pointer" with the servos.
// It is used to create the "compass".
// The parameter 's' is the servo motor that has the pointer.
// It is allowed that 's' is below zero or larger than the numbers of servo motors.
void showPointer( int s)
{
  int pointerA = s % NUM_SERVOS;        // Using the '%' (remainder) for valid number
  int pointerB = (s + 1) % NUM_SERVOS;  // pointer is made with the next servo motor
  int tailA = (s + 16) % NUM_SERVOS;
  int tailB = (s + 17) % NUM_SERVOS;

  // make pointer with servo motor s and s+1.
  myServo[pointerA].write(180-56);
  myServo[pointerB].write(56);

  // make tail with servo motor s+16 and s+17.
  myServo[tailA].write(95);
  myServo[tailB].write(85);

  // Set servos right of pointer
  int n = (NUM_SERVOS / 2) - 2;
  int start = pointerB + 1;
  for( int i=0; i<n; i++)
  {
    int j = (start + i) % NUM_SERVOS;
    myServo[j].write( 2);
  }

  // Set servos left of pointer
  start = tailB + 1;
  for( int i=0; i<n; i++)
  {
    int j = (start + i) % NUM_SERVOS;
    myServo[j].write( 178);
  }
}

// The function GenerateDiagram() can be used to generate
// the diagram.json file for Wokwi.
// To use it, call it from the setup() function, and the
// serial output can be copied into the diagram.json file.
void GenerateDiagram()
{
  Serial.begin(115200);

  Serial.print( "{\n");
  Serial.print( "  \"version\": 1,\n");
  Serial.print( "  \"author\": \"Generated\",\n");
  Serial.print( "  \"editor\": \"wokwi\",\n");
  Serial.print( "  \"parts\": [\n");

  Serial.print( "    {\n");
  Serial.print( "      \"type\": \"wokwi-arduino-mega\",\n");
  Serial.print( "      \"id\": \"mega\",\n");
  Serial.print( "      \"top\": 270,\n");
  Serial.print( "      \"left\": 185,\n");
  Serial.print( "      \"attrs\": {}\n");
  Serial.print( "    },\n");

  // Put the servo motor in reverse order in the diagram.json
  // I think that is visually better.
  // The horn now overlaps the next servo when the horn moves to the right.
  for( int i=NUM_SERVOS-1; i>=0; i--)
  {
    float rotate = float( i) * (360.0 / float( NUM_SERVOS));
    float rad = rotate / 360.0 * 2.0 * M_PI;
    float top = (300.0 * sin( rad)) + 300.0;
    float left = (300.0 * cos( rad)) + 300.0;
    Serial.print( "    {\n");
    Serial.print( "      \"type\": \"wokwi-servo\",\n");
    Serial.print( "      \"id\": \"servo");
    Serial.print( i);
    Serial.print( "\",\n");
    Serial.print( "      \"top\": ");
    Serial.print( top);
    Serial.print( ",\n");
    Serial.print( "      \"left\": ");
    Serial.print( left);
    Serial.print( ",\n");
    Serial.print( "      \"rotate\": ");
    Serial.print( rotate);
    Serial.print( ",\n");
    Serial.print( "      \"attrs\": { \"hornColor\": \"Red\" }\n");
    Serial.print( "    }");
    if( i != 0)
      Serial.print( ",");
    Serial.print( "\n");
  } 

  Serial.print( "  ],\n");
  Serial.print( "  \"connections\": [\n");

  for( int i=0; i<NUM_SERVOS; i++)
  {
    int j = i + 1;
    if( j == NUM_SERVOS)
      j = 0;
    Serial.print( "    [ \"servo");
    Serial.print( i);
    Serial.print( ":V+\", \"servo");
    Serial.print( j);
    Serial.print( ":V+\", \"Red\", [] ],\n");
    Serial.print( "    [ \"servo");
    Serial.print( i);
    Serial.print( ":GND\", \"servo");
    Serial.print( j);
    Serial.print( ":GND\", \"Black\", [] ],\n");

    Serial.print( "    [ \"mega:");
    Serial.print( i + 22);
    Serial.print( "\", \"servo");
    Serial.print( i);
    Serial.print( ":PWM\", \"Green\", [ ] ],\n");
  }
  Serial.print( "    [ \"mega:GND.2\", \"servo9:GND\", \"Black\", [ ] ],\n");
  Serial.print( "    [ \"mega:5V\", \"servo9:V+\", \"Red\", [ ] ]\n");

  Serial.print( "  ]\n");
  Serial.print( "}\n");
}
mega:SCL
mega:SDA
mega:AREF
mega:GND.1
mega:13
mega:12
mega:11
mega:10
mega:9
mega:8
mega:7
mega:6
mega:5
mega:4
mega:3
mega:2
mega:1
mega:0
mega:14
mega:15
mega:16
mega:17
mega:18
mega:19
mega:20
mega:21
mega:5V.1
mega:5V.2
mega:22
mega:23
mega:24
mega:25
mega:26
mega:27
mega:28
mega:29
mega:30
mega:31
mega:32
mega:33
mega:34
mega:35
mega:36
mega:37
mega:38
mega:39
mega:40
mega:41
mega:42
mega:43
mega:44
mega:45
mega:46
mega:47
mega:48
mega:49
mega:50
mega:51
mega:52
mega:53
mega:GND.4
mega:GND.5
mega:IOREF
mega:RESET
mega:3.3V
mega:5V
mega:GND.2
mega:GND.3
mega:VIN
mega:A0
mega:A1
mega:A2
mega:A3
mega:A4
mega:A5
mega:A6
mega:A7
mega:A8
mega:A9
mega:A10
mega:A11
mega:A12
mega:A13
mega:A14
mega:A15
servo31:GND
servo31:V+
servo31:PWM
servo30:GND
servo30:V+
servo30:PWM
servo29:GND
servo29:V+
servo29:PWM
servo28:GND
servo28:V+
servo28:PWM
servo27:GND
servo27:V+
servo27:PWM
servo26:GND
servo26:V+
servo26:PWM
servo25:GND
servo25:V+
servo25:PWM
servo24:GND
servo24:V+
servo24:PWM
servo23:GND
servo23:V+
servo23:PWM
servo22:GND
servo22:V+
servo22:PWM
servo21:GND
servo21:V+
servo21:PWM
servo20:GND
servo20:V+
servo20:PWM
servo19:GND
servo19:V+
servo19:PWM
servo18:GND
servo18:V+
servo18:PWM
servo17:GND
servo17:V+
servo17:PWM
servo16:GND
servo16:V+
servo16:PWM
servo15:GND
servo15:V+
servo15:PWM
servo14:GND
servo14:V+
servo14:PWM
servo13:GND
servo13:V+
servo13:PWM
servo12:GND
servo12:V+
servo12:PWM
servo11:GND
servo11:V+
servo11:PWM
servo10:GND
servo10:V+
servo10:PWM
servo9:GND
servo9:V+
servo9:PWM
servo8:GND
servo8:V+
servo8:PWM
servo7:GND
servo7:V+
servo7:PWM
servo6:GND
servo6:V+
servo6:PWM
servo5:GND
servo5:V+
servo5:PWM
servo4:GND
servo4:V+
servo4:PWM
servo3:GND
servo3:V+
servo3:PWM
servo2:GND
servo2:V+
servo2:PWM
servo1:GND
servo1:V+
servo1:PWM
servo0:GND
servo0:V+
servo0:PWM