// ServoOverdone.ino
// Beispiel für mehrere Servo-Objekte in einem Array / 多伺服电机数组示例
//
// Version 1, 28. Juli 2021, von Koepel
// Version 2, 15. August 2021, von Koepel
//   Zeitverhalten geändert, etwas langsamer
//   diagram.json hat Servos in umgekehrter Reihenfolge (visuell besser)
//   Vierte Sequenz "Kompass" hinzugefügt
//
// Gemeinfrei / 公共领域
//

#include <Servo.h>

#define NUM_SERVOS 32  // Anzahl der Servos / 伺服电机数量
Servo myServo[NUM_SERVOS];  // Servo-Objekt-Array / 伺服电机对象数组

void setup() 
{
  // Servos an Arduino Mega Pins anhängen / 将伺服电机连接到Arduino Mega引脚
  // Ab Pin 22 gibt es auf dem Doppelreihenpinheader genau 32 Pins / 从引脚22开始，双排针脚上正好有32个引脚
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].attach( i + 22);  // Pin 22 bis 53 / 引脚22到53
  }
}

void loop() 
{
  // Sequenz 1: Zufällige Positionen / 序列1：随机位置
  for( int a=0; a<15; a++)
  {
    for( int i=0; i<NUM_SERVOS; i++)
    {
      myServo[i].write( random( 0, 181));  // Zufälliger Winkel zwischen 0 und 180° / 0到180度之间的随机角度
      delay( 2);  // Kurze Pause für Servo-Bewegung / 短暂延迟以便伺服电机移动
    }
    delay( 150);  // Pause zwischen Winkeländerungen / 角度变化之间的暂停
  }

  // Sequenz 2: Synchronbewegung / 序列2：同步移动
  // Alle Servos auf 0° setzen / 将所有伺服电机设置为0度
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].write( 0);  // Anfangsposition (Horn nach links gedreht) / 起始位置（舵机向左旋转）
  }
  delay( 1000);  // Wartezeit für Betrachter / 给观察者的等待时间

  // Hin- und Herbewegung / 来回移动
  for( int a=0; a<3; a++)
  {
    // Nach rechts drehen / 向右旋转
    for( int r=0; r<=180; r++)
    {
      for( int i=0; i<NUM_SERVOS; i++)
      {
        myServo[i].write( r);  // Alle Servos auf aktuellen Winkel setzen / 设置所有伺服电机的当前角度
      }
      delay( 6);  // Geschwindigkeit der Bewegung / 移动速度
    }
    // Nach links drehen / 向左旋转
    for( int r=180; r>=0; r--)
    {
      for( int i=0; i<NUM_SERVOS; i++)
      {
        myServo[i].write( r);
      }
      delay( 6);
    }
  }

  // Sequenz 3: Wellenförmige Bewegung / 序列3：波浪式运动
  for( int a=0; a<6; a++)
  {
    for( int i=0; i<NUM_SERVOS; i++)
    {
      for( int j=0; j<NUM_SERVOS; j++)
      {
        // Abstand zum aktiven Servo berechnen / 计算到活动伺服电机的距离
        int d = j - i;
        if( d < 0) d = -d;  // Absolutwert / 绝对值
        if( d > (NUM_SERVOS / 2)) d = NUM_SERVOS - d;  // Kürzester Abstand / 最短距离

        int angle = 90 - (10 * d);  // Winkel berechnen / 计算角度
        if( angle < 0) angle = 0;  // Begrenzung / 限制
        myServo[j].write( angle);  // Servo auf berechneten Winkel setzen / 设置伺服电机的计算角度
      }
      delay(40);  // Geschwindigkeit der Welle / 波浪速度
    }
  }

  // Sequenz 4: Kompass-Animation / 序列4：指南针动画
  int pointer = NUM_SERVOS * 3 / 4;  // Startposition des Zeigers / 指针起始位置
  showPointer( pointer);  // Zeiger anzeigen / 显示指针
  delay( 1000);  // Betrachterzeit / 观察时间

  // Kompassnadel bewegen / 移动指南针指针
  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);  // Pause vor Wiederholung / 重复前的暂停
}

// Funktion zur Anzeige eines "Zeigers" mit den Servos / 用伺服电机显示"指针"的函数
// s: Index des Servos, der als Zeiger dient / 作为指针的伺服电机索引
void showPointer( int s)
{
  // Gültigen Index berechnen (Modulo-Operation) / 计算有效索引（模运算）
  int pointerA = s % NUM_SERVOS;        
  int pointerB = (s + 1) % NUM_SERVOS;  // Zeiger besteht aus zwei Servos / 指针由两个伺服电机组成
  int tailA = (s + 16) % NUM_SERVOS;    // Schwanzpositionen / 尾部位置
  int tailB = (s + 17) % NUM_SERVOS;

  // Zeiger erstellen / 创建指针
  myServo[pointerA].write(180-56);  // Erster Servo des Zeigers / 指针的第一个伺服电机
  myServo[pointerB].write(56);      // Zweiter Servo des Zeigers / 指针的第二个伺服电机

  // Schwanz erstellen / 创建尾部
  myServo[tailA].write(95);
  myServo[tailB].write(85);

  // Servos rechts vom Zeiger setzen / 设置指针右侧的伺服电机
  int n = (NUM_SERVOS / 2) - 2;  // Anzahl der Servos / 伺服电机数量
  int start = pointerB + 1;
  for( int i=0; i<n; i++)
  {
    int j = (start + i) % NUM_SERVOS;
    myServo[j].write( 2);  // Nach rechts gedreht / 向右旋转
  }

  // Servos links vom Zeiger setzen / 设置指针左侧的伺服电机
  start = tailB + 1;
  for( int i=0; i<n; i++)
  {
    int j = (start + i) % NUM_SERVOS;
    myServo[j].write( 178);  // Nach links gedreht / 向左旋转
  }
}

// Funktion zum Generieren des Wokwi-Diagramms / 生成Wokwi图表的函数
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");

  // Arduino Mega Board / Arduino Mega开发板
  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");

  // Servos in umgekehrter Reihenfolge für bessere Visualisierung / 反向排列伺服电机以获得更好的可视化效果
  for( int i=NUM_SERVOS-1; i>=0; i--)
  {
    float rotate = float(i) * (360.0 / float(NUM_SERVOS));  // Winkel für Kreisanordnung / 圆形排列的角度
    float rad = rotate / 360.0 * 2.0 * M_PI;  // In Bogenmaß umrechnen / 转换为弧度
    float top = (300.0 * sin(rad)) + 300.0;   // Y-Position / Y位置
    float left = (300.0 * cos(rad)) + 300.0;  // X-Position / X位置
    
    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");

  // Verbindungen definieren / 定义连接
  for( int i=0; i<NUM_SERVOS; i++)
  {
    int j = i + 1;
    if(j == NUM_SERVOS) j = 0;
    
    // Stromversorgung und Masse verbinden / 连接电源和地线
    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");

    // PWM-Signale verbinden / 连接PWM信号
    Serial.print( "    [ \"mega:");
    Serial.print(i + 22);
    Serial.print( "\", \"servo");
    Serial.print(i);
    Serial.print( ":PWM\", \"Green\", [] ],\n");
  }
  
  // Zusätzliche Verbindungen für Stromversorgung / 额外的电源连接
  Serial.print( "    [ \"mega:GND.2\", \"servo9:GND\", \"Black\", [] ],\n");
  Serial.print( "    [ \"mega:5V\", \"servo9:V+\", \"Red\", [] ]\n");

  Serial.print( "  ]\n");
  Serial.print( "}\n");
}