#include <RobojaxBTS7960.h>

#define R_EN1 7
#define LPWM1 8
#define L_EN1 5
#define L_IS1 6

#define R_EN2 19
#define R_PWM2 9
#define R_IS2 18
#define LPWM2 10
#define L_EN2 11
#define L_IS2 12

int ENCODER1_OUT = 2;
int FREN_OUT = 3;

int OPTIK = 4;
int ROLE = 13;

unsigned int tur_ = 0;

RobojaxBTS7960 fren(R_EN1, 0, 0, L_EN1, LPWM1, L_IS1, 0);
RobojaxBTS7960 motor(R_EN2, R_PWM2, R_IS2, L_EN2, LPWM2, L_IS2, 0);

void setup() {
  Serial.begin(9600);
  motor.begin();
  fren.begin();

  pinMode(OPTIK, INPUT_PULLUP);
  pinMode(ROLE, INPUT_PULLUP);
  pinMode(17, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  pinMode(R_EN2, OUTPUT);
  pinMode(L_EN2, OUTPUT);

  digitalWrite(R_EN2, 1);
  digitalWrite(L_EN2, 1);

  fren.stop();
  motor.rotate(50, 1);
  delay(300);
  tur_ = 0;
  while (1) {
    if (digitalRead(17) == 1) {
      tur_++;
      Serial.println(tur_);
    }
    fren.stop();
    motor.rotate(50, 1);
    if (digitalRead(3) == 1) {
      break;
    }
  }
  motor.stop();
  fren.rotate(100, 0);
  Serial.println(tur_);
}

void loop() {
  while (digitalRead(ROLE) == 0) {
    if (digitalRead(OPTIK) == 1) {
      fren.stop();
      motor.rotate(50, 1);
      delay(300);
      tur_ = 0;
      while (1) {
        if (digitalRead(17) == 1) {
          tur_++;
          Serial.println(tur_);
        }
        fren.stop();
        motor.rotate(50, 1);
        if (digitalRead(3) == 1) {
          break;
        }
      }
      motor.stop();
      fren.rotate(100, 0);
      Serial.println(tur_);
    }
    delay(1000);
  }
}