156 lines
4.1 KiB
C++
156 lines
4.1 KiB
C++
|
|
|
|
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 ) );
|
|
|
|
}
|