Lab_interaccio/2013/Marceli/ventricol_rev/Funcions.ino

void refresh_values(uint32_t OSC_data)
{
  intValue1 = OSC_data&0x00000000FF;
  intValue2 = (OSC_data>>8)&0x00000000FF;
  intValue3 = (OSC_data>>16)&0x00000000FF;
  intValue4 = (OSC_data>>24)&0x00000000FF;
}

void servoReset()
{
  
  moveServo(SERVO1, center1); //Ojo derecho (Izquierda-->HIGH, Derecha-->LOW)
  moveServo(SERVO2, center2); //Ojo derecho (Arriba-->HIGH, abajo-->LOW)  
  moveServo(SERVO3, center3); //Ojo izquierdo (Izquierda-->HIGH, Derecha-->LOW)
  moveServo(SERVO4, center4); //Ojo izquierdo (Arriba-->HIGH, abajo-->LOW)
  moveServo(SERVO5, center5); //Parpado izquierdo
  moveServo(SERVO6, center6); //Parpado derecho
  moveServo(SERVO7, limit7Low); //Oreja derecha
  moveServo(SERVO8, limit8Low); //Oreja izquierda
  
  moveServo(SERVO10, limit10High); //Lengua
  moveServo(SERVO9,  limit9High);  //Boca
}

void test(OSCMessage *_mes)
{
  for(int i=1; i<101; i++)
  {
     moveServo(SERVO1, i); //Ojo izquierdo (Arriba-->LOW, abajo-->High) //Corregido?
     moveServo(SERVO3, i); //Ojo izquierdo (Arriba-->LOW, abajo-->High) //Corregido?
     delay(10);
  }
  for(int i=101; i>1; i--) 
  {
    moveServo(SERVO1, i); //Ojo izquierdo (Arriba-->LOW, abajo-->High) //Corregido?
    moveServo(SERVO3, i); //Ojo izquierdo (Arriba-->LOW, abajo-->High) //Corregido?
    delay(10);
  }
}

void sReset(OSCMessage *_mes){
  servoReset();
}

void setMotor1(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  moveServo(SERVO1, intValue1);
  moveServo(SERVO2, intValue2);
  moveServo(SERVO3, intValue3);
  moveServo(SERVO4, intValue4);
//  Serial.println(intValue1);
//  Serial.println(intValue2);
//  Serial.println(intValue3);
//  Serial.println(intValue4);
}

void setMotor2(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  moveServo(SERVO5, intValue1);
  moveServo(SERVO6, intValue2);
  moveServo(SERVO7, intValue3);
  moveServo(SERVO8, intValue4);
//  Serial.println(intValue1);
//  Serial.println(intValue2);
//  Serial.println(intValue3);
//  Serial.println(intValue4);
}

void setMotor3(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  moveServo(SERVO9, intValue1);
  moveServo(SERVO10, intValue2);
//  Serial.println(intValue1);
//  Serial.println(intValue2);
}

void setVel1(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  velServo(VELSERVO1, intValue1);
  velServo(VELSERVO2, intValue2);
  velServo(VELSERVO3, intValue3);
  velServo(VELSERVO4, intValue4);
//  Serial.println(intValue1);
//  Serial.println(intValue2);
//  Serial.println(intValue3);
//  Serial.println(intValue4);
}

void setVel2(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  velServo(VELSERVO5, intValue1);
  velServo(VELSERVO6, intValue2);
  velServo(VELSERVO7, intValue3);
  velServo(VELSERVO8, intValue4);
//  Serial.println(intValue1);
//  Serial.println(intValue2);
//  Serial.println(intValue3);
//  Serial.println(intValue4);
}

void setVel3(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  velServo(VELSERVO9, intValue1);
  velServo(VELSERVO10, intValue2);
//  Serial.println(intValue1);
//  Serial.println(intValue2);
}

void setStop(OSCMessage *_mes){
  uint32_t Value=_mes->getArgInt32(0);
  refresh_values(Value);
  if (intValue1) digitalWrite(releServos, HIGH);
  else digitalWrite(releServos, LOW);
}

void SD21( byte CMD, byte inByte )
{
  digitalWrite(ledDatos, LOW);
  Wire.beginTransmission(ADDRESS);
  Wire.write(CMD);
  Wire.write(inByte);                   // Send a value of 0-255 to servo 1
  Wire.endTransmission();
  digitalWrite(ledDatos, HIGH);  

}

void moveServo(long servo, byte pos)
{
  byte limitDown = limitsLow[servo - 0x3F]; 
  byte limitHigh = limitsHigh[servo - 0x3F]; 
  if(pos > 100) pos = 100;
  else if(pos < 0)  pos = 0;
  if(pos != 0) { // si es igual a 0 no se actualiza el motor!!!
    pos = map(pos, 1, 100, limitDown, limitHigh);
    SD21( servo, pos );
  }
}

void velServo(long servoVel, long vel)
{

  if(vel>100) vel = 100;
  else if(vel<0)  vel = 0;
  if(vel > 0) // si es igual a 0 no se actualiza el motor!!!
    SD21( servoVel , map(vel, 1, 100, 1, 128 ) ); 

}