Lab_interaccio/2011/Hormigas/Hormiga/Hormiga.pde

#define m1 9
#define g1 2
#define m2 10
#define g2 3
#define m3 11
#define fc1 18
#define fc2 19

boolean st1 = 0;
boolean st2 = 1;

int ejex_med = 0;
int ejey_med = 0;

#define FREQ 1 // How many interrupts occur before the serial commands are read
#if FREQ > 1
byte int_counter = 0;
#endif

ISR(TIMER2_OVF_vect) {
  sei(); //Reenable global interrupts, otherwise serial commands will get dropped
#if FREQ > 1
  if(++int_counter == FREQ){ // Only do this once every FREQ-th interrupt
    int_counter = 0;
#endif //FREQ
  
   st1=!st1;
//  st2=!st2;  
//  digitalWrite(7, st1); 
//  digitalWrite(5, st2); 
 if (st1==HIGH)
   {
      PORTD |= B00110000;
      PORTD &= B00111111;
   }
  else
    {
      PORTD |= B11000000;
      PORTD &= B11001111;
    }
    
#if FREQ > 1
  }
#endif //FREQ
}

void setup() { 
  pinMode(g1, OUTPUT); 
  pinMode(g2, OUTPUT);  
  pinMode(m1, OUTPUT); 
  pinMode(m2, OUTPUT); 
  pinMode(m3, OUTPUT); 
  pinMode(fc1, INPUT); 
  pinMode(fc2, INPUT); 
  /*pinMode(7, OUTPUT); 
  pinMode(5, OUTPUT);*/
  
  DDRD |= B11110000;
  PORTD |= B00110000;
  PORTD &= B00111111;
  
  digitalWrite(g1, LOW); 
  digitalWrite(g2, LOW); 
  digitalWrite(fc1, HIGH); 
  digitalWrite(fc2, HIGH); 
  digitalWrite(m1, 0); 
  digitalWrite(m2, 0); 
  digitalWrite(m3, 0); 
  
  ejey_med=analogRead(1); 
  ejex_med=analogRead(0); 
  
  /*010 (2MHz) with FREQ set to 10 or less seem to work reasonably well
   Trying to use 011 (500kHz), to eliminate the if statement needed for FREQ
   Might even work as slow as 100 (250kHz).
   */
  TCCR2A = 0;
  TCCR2B = 0<<CS22 | 1<<CS21 | 0<<CS20;

  //Timer2 Overflow Interrupt Enable
  TIMSK2 = 1<<TOIE2;
}

void loop() {
    
  if (analogRead(1)>=(ejey_med + 200)) 
    {
      if (digitalRead(fc2)&& digitalRead(fc1))
            {
              motor2(0,1);
              motor1(1,1);
            }
           else if (!digitalRead(fc2)&& !digitalRead(fc1))
                 {
                   motor2(0,1);
                   motor1(1,1);
                   delay(1000);
                 }
           else if (!digitalRead(fc2)&& digitalRead(fc1))
                 {
                   motor2(0,0);
                   motor1(1,1);
                 }
           else if (digitalRead(fc2)&& !digitalRead(fc1))
                 {
                   motor2(0,1);
                   motor1(1,0);
                 }
      digitalWrite(m3, HIGH); 
    }
   else if (analogRead(1)<(ejey_med - 200))
         {
           if (digitalRead(fc2)&& digitalRead(fc1))
            {
              motor2(1,1);
              motor1(0,1);
            }
           else if (!digitalRead(fc2)&& !digitalRead(fc1))
                 {
                   motor2(1,1);
                   motor1(0,1);
                   delay(1000);
                 }
           else if (!digitalRead(fc2)&& digitalRead(fc1))
                 {
                   motor2(1,0);
                   motor1(0,1);
                 }
           else if (digitalRead(fc2)&& !digitalRead(fc1))
                 {
                   motor2(1,1);
                   motor1(0,0);
                 }
           digitalWrite(m3, HIGH); 
         }
   else if (analogRead(0)>=(ejex_med + 200))
         {
              motor2(1,1);
              motor1(1,1);
         }
   else if (analogRead(0)<(ejex_med - 200))
         {
              motor2(0,1);
              motor1(0,1);
         }
  else 
  {
    motor2(0,0);
    motor1(0,0);
    digitalWrite(m3, 0); 
  }
  
  /*if (analogRead(1)>=512) motor2(1,map(analogRead(1),512,1023,0,255));
  else motor2(0,map(analogRead(1),0,511,255,0));*/
  
  /*while (digitalRead(fc2)) motor2(0,255);
  motor2(0,0);*/
 
}

void motor1 (boolean sent, int vel) {
  digitalWrite(g1, sent); 
  digitalWrite(m1, vel);
}

void motor2 (boolean sent, int vel) {
  digitalWrite(g2, sent); 
  digitalWrite(m2, vel);
}