// Top-Rigth, Top Left, Bottom Right, Bottom Left
// motors top... as seem from "driver perspective"

// readings from potentiometers
int Pot_Front=0;
int Pot_Lef_Rig=0;
int Pot_Vert=0;

// used for "inverse rotation" depend on pot values
int Pot_Front_inv=0;
int Pot_Lef_Rig_inv=0;
int Pot_Vert_inv=0;

// pinout for PWM - motor speeds
int pin_M_Top=10;
int pin_M_Bot=9;
int pin_M_Lef=6;
int pin_M_Rig=5;

// outputs for relays to reverse motors
int pin_M_Top_rev=8;
int pin_M_Bot_rev=7;
int pin_M_Lef_rev=4;
int pin_M_Rig_rev=2;

// speeds per each motor
int M_Top_Value=0;
int M_Bot_Value=0;
int M_Lef_Value=0;
int M_Rig_Value=0;

// forward ( =0 ) or reverse ( = 1) per each motor
int M_Top_rev=0;
int M_Bot_rev=0;
int M_Lef_rev=0;
int M_Rig_rev=0;



void setup() {
  
// configuration of outputs 

pinMode(pin_M_Top, OUTPUT);
pinMode(pin_M_Bot, OUTPUT);
pinMode(pin_M_Lef, OUTPUT);
pinMode(pin_M_Rig, OUTPUT);

// initial rev outputs
digitalWrite(pin_M_Top_rev, LOW);
digitalWrite(pin_M_Bot_rev, LOW);
digitalWrite(pin_M_Lef_rev, LOW);
digitalWrite(pin_M_Rig_rev, LOW);

// initial motor speeds
analogWrite(pin_M_Top,0);
analogWrite(pin_M_Bot,0);
analogWrite(pin_M_Lef,0);
analogWrite(pin_M_Rig,0);

}

void loop() {

// read analogical info from potentiometers - 0 to 1023 converted to 0 to 255
Pot_Front= round(analogRead(A1)/4);
Pot_Lef_Rig= round(analogRead(A2)/4);
Pot_Vert= round(analogRead(A3)/4);

// values between 118 and 132 are considered as center , that is equal to ZERO
// all values < 188 or > 132 are transformed to 1 to 255
// if values are < 118, it's considered as REVERSE

// Forward-Reward (front)
if (Pot_Front < 118) 
 { Pot_Front=2.16*(118-Pot_Front);
   Pot_Front_inv=1;
 }
if ( Pot_Front > 132 )
 { 
  Pot_Front=2.07*(Pot_Front-132);
  Pot_Front_inv=0;
 }
if ( Pot_Front >= 118 && Pot_Front <= 132)
 {
   Pot_Front=0;
   Pot_Front_inv=0;
 }

// ajuste cero e inv - derecha - izq
// Frente
if (Pot_Lef_Rig < 118) 
  {
    Pot_Lef_Rig=2.16*(118-Pot_Lef_Rig);
    Pot_Lef_Rig_inv=1;
  }
else
  {
    if ( Pot_Lef_Rig > 132 )
    { 
    Pot_Lef_Rig=2.07*(Pot_Lef_Rig-132);
    Pot_Lef_Rig_inv=0;
    }
  else
   {
    Pot_Lef_Rig=0;
    Pot_Lef_Rig_inv=0;
   }
  }

// Up-Down (Vertical) potentiometer conversion
// 
if (Pot_Vert < 118) 
  {
    Pot_Vert=2.16*(118-Pot_Vert);
    Pot_Vert_inv=1;
  }
  else
  {
    if ( Pot_Vert > 132 )
    { 
    Pot_Vert=2.07*(Pot_Vert-132);
    Pot_Vert_inv=0;
    }
  else
    {
    Pot_Vert=0;
    Pot_Vert_inv=0;
 }
 }

// calculation of speeds (values) and rev per motor

// Motor TOP
// Value
if ((Pot_Front + Pot_Vert) > 255 )
   { M_Top_Value= 255; }
  else 
   { M_Top_Value= Pot_Front + Pot_Vert; }

// Motor TOP
// Reverse
if ( (Pot_Front_inv )>0 )
   { M_Top_rev=1; }
  else
   { M_Top_rev=0; }

// Motor Bottom
// Values
if ((Pot_Front + Pot_Vert) > 255 )
   { M_Bot_Value= 255; }
  else 
   { M_Bot_Value= Pot_Front + Pot_Vert; }

// Motor Bot
// Reverse
if ( (Pot_Front_inv + Pot_Vert_inv)>0 )
   { M_Bot_rev=1; }
 else
   { M_Bot_rev=0; }

// Motor LEFT 
// Values
if ((Pot_Front + Pot_Lef_Rig) > 255 )
   { M_Lef_Value= 255; }
  else 
   { M_Lef_Value= Pot_Front + Pot_Lef_Rig; }
// Reverse
if ( (Pot_Front_inv + Pot_Lef_Rig_inv)>0 )
   { M_Lef_rev=1; }
  else
   { M_Lef_rev=0; }

// Motor Right
// Values
if ((Pot_Front + Pot_Lef_Rig) > 255 )
   { M_Rig_Value= 255; }
  else 
   { M_Rig_Value= Pot_Front + Pot_Lef_Rig; }
// Reverse
if ( (Pot_Front_inv + Pot_Lef_Rig_inv)>0 )
   { M_Rig_rev=1; }
  else
   { M_Rig_rev=0; }


analogWrite(pin_M_Top,M_Top_Value);
analogWrite(pin_M_Bot,M_Bot_Value);
analogWrite(pin_M_Lef,M_Lef_Value);
analogWrite(pin_M_Rig,M_Rig_Value);

 digitalWrite(pin_M_Lef_rev, HIGH);
if ( 1 == 2 )
{

if (M_Top_rev=1) 
   { digitalWrite(pin_M_Top_rev, HIGH); }
  else
   { digitalWrite(pin_M_Top_rev, LOW); }

if (M_Bot_rev=1) 
  { 
   digitalWrite(pin_M_Bot_rev, HIGH);
  }
  else
  {
   digitalWrite(pin_M_Bot_rev, LOW);
  }

if (M_Lef_rev=1) 
  {
   digitalWrite(pin_M_Lef_rev, HIGH);
  }
  else
  {
   digitalWrite(pin_M_Lef_rev, LOW);
  }

if (M_Rig_rev=1) 
  {
   digitalWrite(pin_M_Rig_rev, HIGH);
  }
  else
  {
   digitalWrite(pin_M_Rig_rev, LOW);
  }
}


}