Fase de desarrollo de codigo y trabajo con materiales

Control de Servos:
Dentro de el control de servos tenemos 4 partes de el prototipo que necesitaran el uso de ellos para ello se definira las prates del prototipo segun el desarrolo.

Servo 1:
El primer servo trabajara en el movimiento de la cabeza este girara dependiendo la distancia en la que se encuentre el robot  para ello se utiliza el siguient codigo.




void updateHeadAngle()
{
  headServo.write(servoAngle); 
  servoAngle += angleStep;
  if(servoAngle >= 150)
  {
    servoAngle = 150;
    angleStep *= -1;
  }
 if(servoAngle <= 30)
  {
    servoAngle = 30; 
    angleStep *= -1;
  }
}
Servos 2 y3 : 
Trabajaran en el movimiento de los brazos de el robot para ello utilizamos el iguiente codigo.
void brazos() {
  for (pos = 0; pos <= 90; pos += 6) {
      myservo1.write(pos);           
    delay(15);     
    myservo2.write(pos);            
    delay(15);
  }
  for (pos = 90
  ; pos >= 0; pos -= 1) {
    myservo1.write(pos);            
    delay(15);   
    myservo2.write(pos);            
    delay(15);
  }
}







Control de sensor ultrasonico :
El sensor ultrasonico nos permitira determinar las distancias para que asi el roobot pueda obstaculizar objetos ke se encuentren frente a el.
Sensor Ultrasonico:
En este caso se utilizara el sensor ultrasonico para en la cabeza de el robot el mismo que trabajara con el siguiente codigo.
void checkDistance()
{
  distance = ultrasonic.Ranging(CM);
  if(LOG) Serial.println(distance);
}

void moove()
{
  if( distance > OBSTRACLE_DISTANCE )
  {
    if(LOG) Serial.println("FORWARD");
     caminar();
    delay(TURN_DELAY);
  }
  else
  {
    parar();
    
    checkobstaculo();
  }
}