//CHOCO 1 - NARANJA
//CHOCO 2 - AZUL

#include "DynamixelSerial.h"
#include <SPI.h>
#include "wiring_private.h" 

#include <WiiChuck.h>
Accessory nunchuck1;

uint8_t rele1 = A1;
uint8_t rele2 = A0;

uint8_t Zvalue;
uint8_t Cvalue;
uint8_t Zvalue_old = 0;
uint8_t Cvalue_old = 0;
uint8_t Ztag, Ctag;
uint8_t timerTag;

unsigned long previousMillis = 0;    
const long interval = 1000;          

unsigned long timerMillis = 0;  
long randomInterval = 1000;          


void resetBoard()
{
  //Serial.println("reset");
  NVIC_SystemReset();       // esta funcion en teoria si funciona en SAMD
}



void setup() {


  Dynamixel.begin(1000000,5);  // Inicialize the servo at 1Mbps and Pin Control 5
  //Dynamixel.setCSlope(2, 0, 0);
  
  //Dynamixel.setCMargin(2, 1, 1);

  Dynamixel.setMaxTorque(1, 1023);
  Dynamixel.setMaxTorque(2, 1023);

  //Dynamixel.setID(1,2); // Solo para programar las IDs
  //Dynamixel.setID(1,2); // Solo para programar las IDs
  //Dynamixel.setID(1,2); // Solo para programar las IDs

  Serial.begin(512000);
  //delay(4000);
  //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;
  }
  
}
//
//
//void abreBoca() // id 1
//{
//   Dynamixel.torqueStatus(1,1);
//   Dynamixel.moveSpeed(1, 100, 1000); // BOCA OLD
//   delay(1000);
//   Dynamixel.moveSpeed(1, 420, 1000); // BOCA 
//   Dynamixel.torqueStatus(1,0); 
//   delay(1000);  
//}

//void abreOjo() // id 2
//{
//   Dynamixel.torqueStatus(2,1);
//   Dynamixel.moveSpeed(2, 650, 1000); // OJOS OLD
//   delay(1000);
//   Dynamixel.moveSpeed(2, 300, 1000); // OJOS
//   Dynamixel.torqueStatus(2,0);
//   delay(1000);
// 
//}

//
//// CHICO
//    Dynamixel.moveSpeed(2, 760, 1000); // OJOS OLD
//    Dynamixel.moveSpeed(1, 420, 1000); // BOCA OLD
//    delay(1000);
//    //Dynamixel.moveSpeed(1, 720, 1000); // BOCA OLD
//    Dynamixel.moveSpeed(2, 300, 1000); // OJOS
//    Dynamixel.moveSpeed(1, 100, 1000); // BOCA 

void abreBoca() // id 1
{
   Dynamixel.torqueStatus(2,1); // ids diferentes a orgos porque los motores estan al reves
   Dynamixel.moveSpeed(2, 100, 1000); // BOCA OLD
   delay(500);
  
}

void cierraBoca() // id 1
{
   Dynamixel.torqueStatus(2,1);
   Dynamixel.moveSpeed(2, 420, 1000); // BOCA 
   delay(500);  
}

void abreOjo() // id 2
{
   Dynamixel.torqueStatus(1,1);
   Dynamixel.moveSpeed(1, 350, 1023); // OJOS
   delay(200);
   Dynamixel.torqueStatus(1,0);

 
}

void cierraOjo() // id 2
{
   Dynamixel.torqueStatus(1,1);
   Dynamixel.moveSpeed(1, 700, 1023); // OJOS OLD
   delay(200);
   Dynamixel.torqueStatus(1,0);

   
}


void loop() 
{

abreOjo();
delay(100);
cierraOjo();
delay(1000);
abreBoca();
delay(100);
cierraBoca();
delay(2000);

abreOjo();
abreBoca();
delay(200);
cierraOjo();
cierraBoca();
delay(1000);

abreOjo();
delay(10);
cierraOjo();
delay(100);
abreOjo();
delay(10);
cierraOjo();
delay(4000);

abreBoca();
delay(100);
cierraBoca();
delay(2000);

abreOjo();
delay(100);
cierraOjo();
delay(10);
abreOjo();
delay(10);
cierraOjo();
delay(10);
abreOjo();
delay(10);
cierraOjo();
delay(10);
abreOjo();
delay(10);
cierraOjo();
delay(3000);

abreOjo();
delay(100);
cierraOjo();
delay(2000);

abreOjo();
abreBoca();
delay(200);
cierraOjo();
cierraBoca();
delay(4000);

abreBoca();
delay(200);
abreOjo();
delay(100);
cierraOjo();
delay(10);
abreOjo();
delay(10);
cierraOjo();
delay(10);
cierraBoca();
delay(5000);


}