#define pinmag 0
#define pinpot 2
#define vccmotor 6
#define enablemotor 3
#define motor1 4
#define motor2 5
#define pincarrera 2
#define red 9
#define green 10
#define blue 11

int ledState = LOW;          
long previousMillis = 0;      
long interval = 1000;
int val = 0;
int val_old = 0;
int carrera = 0;
int potmin = 0;
int potmax = 1023;

int mag;
int mag_threshold = 100;
boolean flagmag = 0;

void setup() {

  Serial.begin(9600);

  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(blue, OUTPUT);

  pinMode(vccmotor, OUTPUT);
  pinMode(motor1, OUTPUT);
  pinMode(motor2, OUTPUT);
  pinMode(pincarrera, INPUT);
  
  digitalWrite(vccmotor, HIGH);
  digitalWrite(pincarrera, HIGH);
  
}


void loop() {

  //////// CALIBRACION //////// 
  if (Serial.available() > 0) {
    // get incoming byte:
    int inByte = Serial.read();
    if (inByte == '1')
      potmax = analogRead(pinpot);
    else if (inByte == '0')
      potmin = analogRead(pinpot);
    else if (inByte == '2') 
    {
      motor_left();
      delay(2000);
      motor_stop();
    } 
  }
  
  //////// POTENCIOMETRO //////// 
  val = analogRead(pinpot);
  val = map(val, potmin, potmax, 1, 17);
  if(val != val_old)
  {
    Serial.println(val, BYTE);
    val_old = val;
  }
  
  //////// CARRERA //////// 
  carrera = digitalRead(pincarrera);
  if (!carrera)
    motor_stop();

  
  //////// MAGNETICO Y BLOQUEO //////// 
  mag = analogRead(pinmag);
  if ( ( mag < mag_threshold ) && (flagmag == 0 ) )
  {
    digitalWrite(red, HIGH);
    motor_right();
    //delay(100);
    while(digitalRead(pincarrera)){}
    motor_stop();
    flagmag = 1;
    Serial.println(21, BYTE);
  }  
  else if ( ( mag >= mag_threshold ) && (flagmag == 1 ) )
  {
    digitalWrite(red, LOW);
    //motor_left();
    //delay(1500);
    //motor_stop();
    flagmag = 0;
    Serial.println(20, BYTE);
  }
  
  
  //////// DEBUG //////// 
  Serial.print("mag = ");
  Serial.print(mag);
  Serial.print(" pot = ");
  Serial.print(val);
  Serial.print(" carrera = ");
  Serial.print(carrera);
  Serial.print(" potmin = ");
  Serial.print(potmin);
  Serial.print(" potmax = ");
  Serial.println(potmax);
  
}

void motor_right()
{
  digitalWrite(motor1, HIGH);
  digitalWrite(motor2, LOW);
}

void motor_left()
{
  digitalWrite(motor1, LOW);
  digitalWrite(motor2, HIGH);
}

void motor_stop()
{
  digitalWrite(motor1, LOW);
  digitalWrite(motor2, LOW);
}