127 lines
2.4 KiB
C++
127 lines
2.4 KiB
C++
|
|
|
|
#include "DynamixelSerial.h"
|
|
#include <SPI.h>
|
|
#include "wiring_private.h"
|
|
|
|
uint8_t rele1 = A1;
|
|
uint8_t rele2 = A0;
|
|
uint8_t interruptPin = A2;
|
|
|
|
unsigned long previousMillis = 0;
|
|
long interval = 1000;
|
|
unsigned long timerMillis = 0;
|
|
long randomInterval = 1000;
|
|
|
|
volatile bool bocaOpenFlag, bocaCloseFlag;
|
|
|
|
void resetBoard()
|
|
{
|
|
//Serial.println("reset");
|
|
NVIC_SystemReset(); // esta funcion en teoria si funciona en SAMD
|
|
}
|
|
|
|
|
|
|
|
void boca()
|
|
{ // toas las variables adentro han de ser volatil
|
|
|
|
|
|
if((digitalRead(interruptPin) == 1) && (bocaCloseFlag == 1) )
|
|
{
|
|
Serial.println("Cierra Boca");
|
|
cierraBoca();
|
|
delay(20);
|
|
bocaCloseFlag = 0;
|
|
}
|
|
else if((digitalRead(interruptPin) == 0) && (bocaCloseFlag == 0) )
|
|
{
|
|
Serial.println("Abre Boca");
|
|
abreBoca();
|
|
delay(20);
|
|
bocaCloseFlag = 1;
|
|
}
|
|
|
|
}
|
|
|
|
void setup() {
|
|
|
|
//Dynamixel.setID(1,2); // Solo para programar las IDs
|
|
Dynamixel.begin(1000000, 5); // Inicialize the servo at 1Mbps and Pin Control 5
|
|
|
|
Serial.begin(512000);
|
|
//while (!Serial) {
|
|
//; // wait for serial port to connect. Needed for Leonardo only
|
|
//}
|
|
|
|
//pinMode(rele1, OUTPUT);
|
|
//pinMode(rele2, OUTPUT);
|
|
|
|
// nunchuck1.begin();
|
|
// if (nunchuck1.type == Unknown_) { // Modificado en la libreria para que no de error con la Ethernet.h
|
|
// nunchuck1.type = NUNCHUCK;
|
|
// }
|
|
|
|
pinMode(interruptPin, INPUT_PULLUP);
|
|
//attachInterrupt(digitalPinToInterrupt(interruptPin), ISR_boca, CHANGE);
|
|
|
|
randomSeed(42);
|
|
|
|
}
|
|
|
|
|
|
void abreBoca() // id 1
|
|
{
|
|
//Dynamixel.torqueStatus(2,1);
|
|
Dynamixel.moveSpeed(2, 100, 1000); // BOCA
|
|
delay(100);
|
|
//Dynamixel.torqueStatus(2,0);
|
|
|
|
}
|
|
|
|
void cierraBoca() // id 1
|
|
{
|
|
//Dynamixel.torqueStatus(2,1);
|
|
Dynamixel.moveSpeed(2, 420, 1000); // BOCA
|
|
//Dynamixel.torqueStatus(2,0);
|
|
//delay(100);
|
|
}
|
|
|
|
void abreOjo() // id 2 INVERTIDO MOTOR
|
|
{
|
|
Dynamixel.torqueStatus(1, 1);
|
|
Dynamixel.moveSpeed(1, 350, 1023); // OJOS
|
|
delay(250);
|
|
Dynamixel.torqueStatus(1, 0);
|
|
}
|
|
|
|
void cierraOjo() // id 2 INVERTIDO MOTOR
|
|
{
|
|
Dynamixel.torqueStatus(1, 1);
|
|
Dynamixel.moveSpeed(1, 700, 1023); // OJOS OLD
|
|
delay(250);
|
|
Dynamixel.torqueStatus(1, 0);
|
|
}
|
|
|
|
|
|
void loop()
|
|
{
|
|
|
|
boca();
|
|
|
|
unsigned long currentMillis = millis();
|
|
|
|
if (currentMillis - previousMillis >= interval)
|
|
{
|
|
// save the last time you blinked the LED
|
|
previousMillis = currentMillis;
|
|
interval = random(500, 5000);
|
|
|
|
abreOjo();
|
|
delay(10);
|
|
cierraOjo();
|
|
}
|
|
|
|
|
|
}
|