Lab_interaccio/2010/Ricardo/robot_cabeza/applet/robot_cabeza.pde

#include <Servo.h>
#undef int
#undef abs
#undef double
#undef float
#undef round

Servo servoA, servoB, servoC;

int IR = 6; 
int zumbador =11;
int camara = 8;
int girocam = 3;
int bit1 = 0;
int bit2 = 1;
int bit3 = 2;
int bit4 = 3;
int val1 = 0;
int val2 = 0;
int val3 = 0;
int val4 = 0;
byte val = 0;
int speed1 = 9;
int speed2 = 10;
int id = 0;
unsigned long time=0;
unsigned long time_ant=0;
int frec = 0;
int activa=0;

void stop()
{
  servoA.write(91); //Parado
  servoB.write(90); //Parado
}

void izquierda(int iz) 
{ 
  servoA.write(iz);
}

void derecha(int der) 
{ 
  servoB.write(der);
}
 
void retrocede(int re) 
{ 
  stop();
  servoA.write(re);
  servoB.write(180-re);
} 

void avanza(int av) 
{ 
  stop();
  servoA.write(180-av);
  servoB.write(av);

} 

void setup() 
{ 
  Serial.begin(9600); 
  delay(100); 
  
  analogWrite(zumbador,0);
  
  servoA.attach(speed1);
  //servo1.setMaximumPulse(2200);
  servoB.attach(speed2);
  servoC.attach(girocam);
  
  stop();
  
  servoC.write(93); //Parado
  
  pinMode(IR, OUTPUT);
  pinMode(zumbador, OUTPUT);
  pinMode(camara, OUTPUT);
  pinMode(14, INPUT);
  pinMode(15, INPUT);
  pinMode(16, INPUT);
  pinMode(17, INPUT);
  digitalWrite(14, HIGH);
  digitalWrite(15, HIGH);
  digitalWrite(16, HIGH);
  digitalWrite(17, HIGH);
  if (analogRead(bit1)>512) val1=16;
  else val1=0;
  if (analogRead(bit2)>512) val2=32;
  else val2=0;
  if (analogRead(bit3)>512) val3=64;
  else val3=0;
  if (analogRead(bit4)>512) val4=128;
  else val4=0;
  id=val1+val2+val3+val4;
  Serial.print("Hola, soy el robot numero ");
  Serial.println(id);
  
} 

void loop() 
{ 
   Servo::refresh();  
   if (Serial.available()) {
    val = Serial.read();
    if ((val&0xF0)==id)
    {
      //Serial.print(val, BYTE);
      
      switch (val&0x0F) {
        case 0x00: //Avanza
          servoC.write(93); //Parado
          activa=0;
          analogWrite(zumbador,0);
          avanza(180);
        break;
        case 0x01: //Retrocede
          retrocede(180);
          frec= 100;
          activa=1;
          time_ant=millis();
          //analogWrite(zumbador,200);
        break;
        case 0x02: //Derecha
        {
          izquierda(0);
          derecha(0);
          servoC.write(180);
          activa=0;
          analogWrite(zumbador,0);
        }
        break;
        case 0x03: //Izquierda
        {
          derecha(180);
          izquierda(180);
          servoC.write(0);
          activa=0;
          analogWrite(zumbador,0);
        }
        break;
        case 0x04:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x05:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x06:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x07:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x08:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x09:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x0A:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x0B:
        {
          //COMANDO LIBRE
        }
        break;
        case 0x0C:
          stop();
          activa=0;
          analogWrite(zumbador,0);
          //analogWrite(zumbador,0);
        break;
        case 0x0D:
          digitalWrite(camara,  HIGH);
        break;
        case 0x0E:
          digitalWrite(camara,  LOW);
        break;
      }
   }
  }
  time = millis();
  if (activa==1)
  {
    if (time-time_ant >= frec)
      {
        analogWrite(zumbador,255);
        if (time-time_ant >= 2*frec) time_ant=time;
       }
    else analogWrite(zumbador,0);
  }
}