// https://wokwi.com/projects/411109185758524417
// DC motor sim with encoders (taps on stepper quadrature signals)
// inertia of motor set by time constant Tau * 10^TauExponent
// Top speed of motor set by MaxSpeed.
//
// Using thse custom chips in diagram.json:
// https://github.com/drf5n/Wokwi-Chip-L298N
// https://github.com/drf5n/Wokwi-Chip-stepper-esc
// https://github.com/Dlloydev/Wokwi-Chip-Scope
//
// Added github repositories and .json dependencies to
// https://wokwi.com/projects/410601389043609601
// built from:
// https://wokwi.com/projects/410302035690579969
// ESC steppers simulating DC motor with L298N
//
// This project has two custom chips:
//
// L298N motor driver translates digital pins+PWM into
//  PWM +/- signals for a DC motor./
//
// A custom chip to tranlate the PWM signals into quadrature
// signals for a stepper motor.  Essentially a Stepper ESC.
//
// L298N Wokwi custom chip with LEDs-simulating motors
// and custom ESCs for PWM->Stepper control
// With potentiometers to control directions
// Also with @Dlloyd's Chip Scopes from https://github.com/Dlloydev/Wokwi-Chip-Scope
// to see the inputs abd outputs of the driver board
//
// Modified from
// https://wokwi.com/projects/351764383409373773 by Carlos Arino

#include <Encoder.h>
// Motor A
const byte  PWMA = 11;
const byte  BIN1 = 10;
const byte  BIN2 = 9;

// Motor B
const byte  PWMB = 3;
const byte  AIN2 = 5;
const byte  AIN1 = 6;

int xx = 0, yy = 0; // x&y potentiometers

Encoder left(19, 16);
Encoder right(18, 17);

void setup ()
{
  // Declaramos todos los pines como salidas
  pinMode (PWMA, OUTPUT);
  pinMode (PWMB, OUTPUT);
  pinMode (BIN1, OUTPUT);
  pinMode (BIN2, OUTPUT);
  pinMode (AIN2, OUTPUT);
  pinMode (AIN1, OUTPUT);
  pinMode (A0, INPUT);
  pinMode (A1, INPUT);
  Serial.begin(115200);
  Serial.print("Encoder Positions\nleft, right, ave, diff, d_avg, d_diff\n");
}
//Mover los motores a pleno rendimiento (255), si quieres bajar la velocidad puedes reducir el valor hasta la mínima que son 0 (parados)</pre>
//Para mover los motores en sentido de giro contrario, cambia BIN1 a LOW e BIN2 a HIGH


void writeL(int a1, int a2) {
  digitalWrite(BIN1, a1);
  digitalWrite(BIN2, a2);
}
void writeR(int a1, int a2) {
  digitalWrite(AIN2, a1);
  digitalWrite(AIN1, a2);
}

void loop ()
{
  static int lastXX, lastYY;
  xx = (analogRead(A0) - 512) / 8;
  yy = (analogRead(A1) - 512) / 2;

  int speedL = constrain(yy - xx, -255, 255);
  int speedR = constrain(yy + xx, -255, 255);

  //Direccion motor L
  if (speedL >= 0 ) {
    writeL(0, 1);
    analogWrite(PWMB, speedL); //Velocidad motor L
  }
  else
  {
    writeL(1, 0);
    analogWrite(PWMB, -speedL); //Velocidad motor L
  }
  //Direccion motor R
  if (speedR >= 0 ) {
    writeR(0, 1);
    analogWrite(PWMA, speedR); //Velocidad motor R
  }
  else
  {
    writeR(1, 0);
    analogWrite(PWMA, -speedR); //Velocidad motor R
  }

  if (false && (xx != lastXX || yy != lastYY)) {
    lastXX = xx;
    lastYY = yy;
    Serial.print(xx);
    Serial.print(",");
    Serial.print(yy);
    Serial.print(" ");
  }
  left.read();
  right.read();
  report();
}


void report() {
  auto now = millis();
  const typeof(now) interval = 1000;
  static typeof(now) last = -interval;
  static long last_avg, last_diff;
  if (now - last >= interval) {
    last += interval;
    long lval = -left.read();
    long rval = right.read();
    long avg = (lval + rval) / 2;
    long diff = (lval - rval) / 2;
    long delta_avg = avg - last_avg;
    long delta_diff = diff - last_diff;
    last_avg = avg;
    last_diff = diff;
    Serial.print(lval);
    Serial.print(" ");
    Serial.print(rval);
    Serial.print(" ");
    Serial.print(avg);
    Serial.print(" ");
    Serial.print(diff);
    Serial.print(" ");
    Serial.print(delta_avg);
    Serial.print(" ");
    Serial.print(delta_diff);
    Serial.println();
  }
}