Proyecto robotica

/*PROYECTO DE ROBOTICA

GESTICULACION DE ROSTRO MEDIANTE MOTORES EMPLEADO EN OJOS, PARPADOS,

CEJAS, BOCA, SONRISA Y DIRECCION DE VISTA.

PARA ELLO SE UTILIZARON LOS MOTORES DYNAMIXEL AX-12.

UTILIZANDO CON ELLOS EL OPENCM9.04 COMO CEREBRO DEL PROYECTO.*/

#define DXL_BUS_SERIAL1 1  //Dynamixel on Serial1(USART1)  <-OpenCM9.04

#define DXL_BUS_SERIAL2 2  //Dynamixel on Serial2(USART2)  <-LN101,BT210

#define DXL_BUS_SERIAL3 3  //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP

/* Dynamixel ID defines */

#define OJO_IZQ 2

#define OJO_DER 3

#define BOCA 10

#define PARPADO_DER 16

#define PARPADO_IZQ 6

#define CEJA_DER 4

#define CEJA_IZQ 5

#define VISTA 1

#define SONRISA_IZQ 18

#define SONRISA_DER 14

/* Control table defines */

#define GOAL_POSITION 30

/* Libreria Dynamixel en el serial 1 */

Dynamixel Dxl(DXL_BUS_SERIAL1);

int Xarray[6];  //almacena los valores de x obtenidos del serial para el modo mirada

int Yarray[6];  //almacena los valores de y obtenidos del serial para el modo mirada

int x;        //almacena la posicion x para el modo mirada

int y;        //almacena la posicion y para el modo mirada

char XY;

char YX;

char n;

int miradax;    //valor que despliega el valor del motor en el modo mirada

int miraday;

int venojado=0;  //banderas para las variables de estado

int vtriste=0;

int vfeliz=0;

int valive=0;

int vhablar=0;

int vmapa=0;

int vquit=1;

int vparder;

int vparizq;

unsigned long previousMillis = -1000;    //variable para el millis en un segundo

const long tiempomovimiento=70;

const long interval = 1000;

int randomojod;    //variables para el modo alive que guarda el valor random de cada uno de los motores

int randomojoi;

int randomcejad;

int randomcejai;

int randomsonrisad;

int randomsonrisai;

int randomvista;

int randomboca;

int randomparpadod;

int randomparpadoi;

int tiempoactual=0;

int tiempoespera;

int bocacerrada=0;

int voz=0;

unsigned long currentMillis=millis();

void setup() {

  Serial2.begin(9600);    //se inicializa el serial en este caso para la comunicacion bluetooth

  Dxl.begin(3);          //se inicializa la comunicacion con los motores a 1Mb

  Dxl.jointMode(OJO_DER); //jointMode() is to use position mode

  Dxl.jointMode(OJO_IZQ); //jointMode() is to use position mode

  Dxl.jointMode(BOCA); //jointMode() is to use position mode

  Dxl.jointMode(PARPADO_DER); //jointMode() is to use position mode

  Dxl.jointMode(PARPADO_IZQ); //jointMode() is to use position mode

  Dxl.jointMode(VISTA); //jointMode() is to use position mode

  Dxl.jointMode(CEJA_IZQ); //jointMode() is to use position mode

  Dxl.jointMode(CEJA_DER); //jointMode() is to use position mode

  Dxl.jointMode(SONRISA_IZQ); //jointMode() is to use position mode

  Dxl.jointMode(SONRISA_DER); //jointMode() is to use position mode

}

void mapa() {    // esta funcion lee las coordenadas proporcionadas de 100 a 400, y las coloca en coordenadas para los motores

    for (int a = 0; a < 5; a++) {        //lee los caracteres enviados por el serial y los guarda en un array

      Xarray[a] = (SerialUSB.read() - 48);

      SerialUSB.print(Xarray[a]);              

    }

    SerialUSB.println("");

    x=((n-48)*100)+(Xarray[0] * 10)+(Xarray[1]);    //toma los primeros 3 caracteres del array y los convierte en coordenada x

    SerialUSB.println(int(x));

    y=(Xarray[2] * 100)+(Xarray[3] * 10)+(Xarray[4]);    //toma los primeros 3 caracteres del array y los convierte en coordenada y  

    SerialUSB.println(int(y));

    miradax=map(x,420,100,420,600);//convierte el rango de las coordenadas en x proporcionadas y las convierte en las coordenadas en x para el motor

    SerialUSB.println(int(miradax));

    miraday=map(y,100,340,455,545);//convierte el rango de las coordenadas en x proporcionadas y las convierte en las coordenadas en x para el motor

    SerialUSB.println(int(miraday));

    Dxl.writeWord(OJO_IZQ, GOAL_POSITION, miradax);    //escribe las coordenadas de x para el ojo izquierdo

    Dxl.writeWord(OJO_DER, GOAL_POSITION, miradax);  //escribe las coordenadas de x para el ojo derecho

    Dxl.writeWord(VISTA, GOAL_POSITION, miraday); //escribe las coordenadas de y para levantar o agachar la vista

    currentMillis = millis();//guarda los millis

    //vmapa=0;    

}

void enojado() {  //esta funcion gesticula los motores para obtener una simulacion de rostro enojado

  switch(venojado){

    case 1:    //en este caso, simula que parpaea

      SerialUSB.println("Enojado");

      Dxl.writeWord(PARPADO_IZQ, GOAL_POSITION, 630); //Compatible with all dynamixel serise

      Dxl.writeWord(PARPADO_DER, GOAL_POSITION, 825); //Compatible with all dynamixel serise

      currentMillis = millis();

      venojado=2;  //cambia el estado al siguiente

      break;

    case 2:    //en este caso espera 0.2 segundos para colocar los motores en las direcciones proporcionadas

      //tiempoactual=millis()+ previousMillis- currentMillis;

      //SerialUSB.println(tiempoactual);

      if (millis()- 300 >= currentMillis)

      {    

        Dxl.writeWord(PARPADO_DER, GOAL_POSITION, 650); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(PARPADO_IZQ, GOAL_POSITION, 850); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(CEJA_DER, GOAL_POSITION, 300); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(CEJA_IZQ, GOAL_POSITION, 650); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(SONRISA_DER, GOAL_POSITION, 820); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(SONRISA_IZQ, GOAL_POSITION, 520); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(VISTA, GOAL_POSITION, 512); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(OJO_IZQ, GOAL_POSITION, 600); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(OJO_DER, GOAL_POSITION, 600); //Coloca el motor correspondiente en la direccion indicada

        Dxl.writeWord(BOCA, GOAL_POSITION, 510); //Coloca el motor correspondiente en la direccion indicada

        venojado=3;//cambia el estado al siguiente

        currentMillis = millis();

      }

      break;

    case 3:  //en este estado, mueve los ojos de lado a lado, esperando 1 seguno de uno a otro

      //tiempoactual=millis()+ previousMillis- currentMillis;

      //SerialUSB.println(tiempoactual);

      if (millis()+ previousMillis >= currentMillis)

      {    

        Dxl.writeWord(OJO_IZQ, GOAL_POSITION, 420);

        Dxl.writeWord(OJO_DER, GOAL_POSITION, 420);

        venojado=4;//cambia el estado al siguiente

        currentMillis = millis();

      }

      break;

    case 4:  //en este estado regresa los ojos al estado normal

      //tiempoactual=millis()+ previousMillis- currentMillis;

      //SerialUSB.println(tiempoactual);

      if (millis()+ previousMillis >= currentMillis)

      {    

...