// Servo Sweep example for the ESP32
// https://wokwi.com/arduino/projects/323706614646309460

#include <ESP32Servo.h>#include <ESP32Servo.h>

Servo servo1,servo2,servo3; 
int pos1=0;
int pos2=0;
int pos3=0;   
int servoPin1 = 5;
int servoPin2= 25;
int servoPin3= 26;
const int switches[8] = {23, 22, 18, 19, 4, 21, 2, 15};

int pow(int a, int b){
  int aux = 1;
  for(int i=0; i<b; i++){
    aux=aux*a;
  }
  return aux;
}

int cont=0;

void setup() {
  Serial.begin(9600);
  
  for(int i=0; i<8; i++){
    pinMode(switches[i], INPUT);
  }

  
	// Allow allocation of all timers
	ESP32PWM::allocateTimer(0);
	ESP32PWM::allocateTimer(1);
	ESP32PWM::allocateTimer(2);
	ESP32PWM::allocateTimer(3);
	servo1.setPeriodHertz(50);    
	servo1.attach(servoPin1, 500, 2400); 
  servo2.setPeriodHertz(50);    
	servo2.attach(servoPin2, 500, 2400); 
  servo3.setPeriodHertz(50);    
	servo3.attach(servoPin3, 500, 2400); 
  
	
}

void loop() {

  
  aux();
}
 
  
void aux(){
    //Servo1:
  if (cont==0){
  int binarioServo1[8];
  int dec1 = 0;
  Serial.println("Digite o binario do primeiro servo: ");
  delay(10000);

  for(int i=0; i<8; i++){
    if(digitalRead(switches[i])==HIGH){
      binarioServo1[i]=1;
    } else {
      binarioServo1[i]=0;
    }
  }

   
    for (int j=0;j<8;j++){
      Serial.println(binarioServo1[j]);
    }

    for (int i=0;i<8;i++){
      int exp1=7-i;
      dec1 = dec1+binarioServo1[i]*(pow(2, exp1));
    }

    if (dec1>180) dec1=180;

    Serial.printf("Decimal: %d", dec1);
    Serial.printf("\n");
    
    Serial.println(pos1);
     for (pos1 = 0; pos1 < dec1; pos1 += 1) { // goes from 0 degrees to 180 degrees
          // in steps of 1 degree
       servo1.write(pos1);    // tell servo to go to position in variable 'pos'
        delay(30);             // waits 15ms for the servo to reach the position
     }

    
    Serial.printf("Posicao: %d", pos1);
    Serial.printf("\n");

    cont=cont+1;
  }

//Servo 2:
  if (cont==1){
  int binarioServo2[8];
  int dec2 = 0;
  Serial.println("Digite o binario do segundo servo: ");
  delay(10000);

  for(int i=0; i<8; i++){
    if(digitalRead(switches[i])==HIGH){
      binarioServo2[i]=1;
    } else {
      binarioServo2[i]=0;
    }
  }

   
    for (int j=0;j<8;j++){
      Serial.println(binarioServo2[j]);
    }

    for (int i=0;i<8;i++){
      int exp2=7-i;
      dec2 = dec2+binarioServo2[i]*(pow(2, exp2));
    }

    if (dec2>180) dec2=180;

    Serial.printf("Decimal: %d", dec2);
    Serial.printf("\n");
    
    Serial.println(pos2);
     for (pos2 = 0; pos2 < dec2; pos2 += 1) { // goes from 0 degrees to 180 degrees
          // in steps of 1 degree
       servo2.write(pos2);    // tell servo to go to position in variable 'pos'
        delay(30);             // waits 15ms for the servo to reach the position
     }

    
    Serial.printf("Posicao: %d", pos2);
    Serial.printf("\n");
  cont=cont+1;
  }
//Servo 3:
  if (cont==2){
  int binarioServo3[8];
  int dec3 = 0;
  Serial.println("Digite o binario do terceiro servo: ");
  delay(10000);

  for(int i=0; i<8; i++){
    if(digitalRead(switches[i])==HIGH){
      binarioServo3[i]=1;
    } else {
      binarioServo3[i]=0;
    }
  }

   
    for (int j=0;j<8;j++){
      Serial.println(binarioServo3[j]);
    }

    for (int i=0;i<8;i++){
      int exp3=7-i;
      dec3 = dec3+binarioServo3[i]*(pow(2, exp3));
    }

    if (dec3>180) dec3=180;

    Serial.printf("Decimal: %d", dec3);
    Serial.printf("\n");
    
    Serial.println(pos3);
     for (pos3 = 0; pos3 < dec3; pos3 += 1) { // goes from 0 degrees to 180 degrees
          // in steps of 1 degree
       servo3.write(pos3);    // tell servo to go to position in variable 'pos'
        delay(30);             // waits 15ms for the servo to reach the position
     }

    
    Serial.printf("Posicao: %d", pos3);
    Serial.printf("\n");
  cont=cont+1;
  }
  if(cont==3){
    Serial.println("fim");
    delay(10000);
  }
}