Picasso Core

main.cpp

//Portas correspondentes aos servos
#define MOTOR_1 4 //MIN:0 - MAX:180
#define MOTOR_2 5 //MIN:0 - MAX:180
#define MOTOR_3 9 //MIN:10 - MAX:70
#define MOTOR_4 13 //MIN:80 - MAX:180

#define MOTOR_VAR 2 //constante somada a cada ciclo para mover cada motor

#include <Servo.h>

//Servos
Servo motor[6];


int joystick1[3];
int joystick2[3];

//hand flags
boolean handClosed = false;

//motor rotation
int motor_rotation[5] = {0, 90, 0, 25, 80}; //inicializa com o minimo de cada motor

/* Configurar todos os servos com suas portas correspondentes */
void setupMotors(){
  motor[1].attach(MOTOR_1);
  motor[2].attach(MOTOR_2);
  motor[3].attach(MOTOR_3);
  motor[4].attach(MOTOR_4);
}

void setupJoysticks(){
  //joystick 1
  pinMode(A1, INPUT); //X
  pinMode(A0, INPUT); //Y
  pinMode(11, INPUT_PULLUP); //Z
  
  
  //joystick 2
  pinMode(A3, INPUT); //X
  pinMode(A2, INPUT); //Y
  pinMode(12, INPUT_PULLUP); //Z


  //INICIAR MOTORES EM SUAS POSICOES DEFAUL
  motor[1].write(motor_rotation[1]);
  motor[2].write(motor_rotation[2]);
  motor[3].write(motor_rotation[3]);
  motor[4].write(motor_rotation[4]);
  
}

void setup() {
  setupMotors();
  setupJoysticks();
  Serial.begin(9600);
}


/* Movimentar mao */
void moveHand(int a){
  if(a == 0){
    handClosed = !handClosed;
    motor[2].write(handClosed ? 178 : 10);
    delay(200);
  }else if(handClosed){
    motor[2].write(handClosed ? 178 : 10);
  }  
}

void moveMotor1(int x){ 
  
  int MOTOR_NUM = 1;
  
  if(x < 350){ //left
    motor_rotation[MOTOR_NUM] += MOTOR_VAR;
  }else if(x > 700){
    motor_rotation[MOTOR_NUM] -= MOTOR_VAR;  
  }else{
    return; 
  }
  
  motor_rotation[MOTOR_NUM] = (motor_rotation[MOTOR_NUM] > 150) ? 150 : motor_rotation[MOTOR_NUM];
  motor_rotation[MOTOR_NUM] = (motor_rotation[MOTOR_NUM] < 20) ? 20 : motor_rotation[MOTOR_NUM];
  motor[MOTOR_NUM].write(motor_rotation[MOTOR_NUM]);
}

void moveMotor4(int x){
  
  int MOTOR_NUM = 4;
  
  if(x < 350){ //left
    motor_rotation[MOTOR_NUM] += MOTOR_VAR;
  }else if(x > 700){
    motor_rotation[MOTOR_NUM] -= MOTOR_VAR;  
  }else{
    return; 
  }
  
  motor_rotation[MOTOR_NUM] = (motor_rotation[MOTOR_NUM] > 180) ? 180 : motor_rotation[MOTOR_NUM];
  motor_rotation[MOTOR_NUM] = (motor_rotation[MOTOR_NUM] < 80) ? 80 : motor_rotation[MOTOR_NUM];
  motor[MOTOR_NUM].write(motor_rotation[MOTOR_NUM]);
 
}

void moveMotor3(int x){
  
  
  int MOTOR_NUM = 3;
  
  if(x < 350){ //left
    motor_rotation[MOTOR_NUM] -= MOTOR_VAR;
  }else if(x > 700){
    motor_rotation[MOTOR_NUM] += MOTOR_VAR;  
  }else{
    return; 
  }
  
  motor_rotation[MOTOR_NUM] = (motor_rotation[MOTOR_NUM] > 70) ? 70 : motor_rotation[MOTOR_NUM];
  motor_rotation[MOTOR_NUM] = (motor_rotation[MOTOR_NUM] < 10) ? 10 : motor_rotation[MOTOR_NUM];
  motor[MOTOR_NUM].write(motor_rotation[MOTOR_NUM]);

}


void shapeOne(){
  //linha1
  while(motor_rotation[3] > 10){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] > 90){ moveMotor4(1000); delay(20);}
  while(motor_rotation[1] < 150){ moveMotor1(0); delay(20); }
  while(motor_rotation[3] < 70){ moveMotor3(1000); delay(20); }
  while(motor_rotation[1] > 20){ moveMotor1(1000); delay(20); }
  while(motor_rotation[3] > 10){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] < 180){ moveMotor4(0); delay(20); }  
  
  //linha2
  while(motor_rotation[3] > 30){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] > 90){ moveMotor4(1000); delay(20);}
  while(motor_rotation[1] < 140){ moveMotor1(0); delay(20); }
  while(motor_rotation[3] < 60){ moveMotor3(1000); delay(20); }
  while(motor_rotation[1] > 30){ moveMotor1(1000); delay(20); }
  while(motor_rotation[3] > 30){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] < 180){ moveMotor4(0); delay(20); }

  //linha3
  while(motor_rotation[3] > 30){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] > 80){ moveMotor4(1000); delay(20);}
  while(motor_rotation[1] < 130){ moveMotor1(0); delay(20); }
  while(motor_rotation[3] < 60){ moveMotor3(1000); delay(20); }
  while(motor_rotation[1] > 40){ moveMotor1(1000); delay(20); }
  while(motor_rotation[3] > 30){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] < 180){ moveMotor4(0); delay(20); }
}


void shapeTwo(){
  //linha1
  while(motor_rotation[3] > 10){ moveMotor3(0); delay(20); }
  while(motor_rotation[4] > 90){ moveMotor4(1000); delay(20);}
  while(motor_rotation[1] < 150){ moveMotor1(0); delay(20); }
  
    while(motor_rotation[3] < 70){ moveMotor3(1000); delay(20); }
    
    while(motor_rotation[1] > 130){ moveMotor1(1000); delay(20); }
    while(motor_rotation[4] < 150){ moveMotor4(0); delay(20); }

    while(motor_rotation[1] > 110){ moveMotor1(1000); delay(20); }
    while(motor_rotation[4] > 80){ moveMotor4(1000); delay(20); }
    
    while(motor_rotation[1] > 90){ moveMotor1(1000); delay(20); }
    while(motor_rotation[4] < 150){ moveMotor4(0); delay(20); }
    
    while(motor_rotation[1] > 70){ moveMotor1(1000); delay(20); }
    while(motor_rotation[4] > 80){ moveMotor4(1000); delay(20); }
    
    while(motor_rotation[1] > 50){ moveMotor1(1000); delay(20); }
    while(motor_rotation[4] < 150){ moveMotor4(0); delay(20); }

    while(motor_rotation[1] > 30){ moveMotor1(1000); delay(20); }
    while(motor_rotation[4] > 80){ moveMotor4(1000); delay(20); }

    while(motor_rotation[3] > 10){ moveMotor3(0); delay(20); }
  

}

void automaticDraw(boolean a){
  if(!a) return;

  int val = random(2);
  
  if(val == 0){
    shapeOne();  
  }else{
    shapeTwo();
  }
  
  
}


void loop() {
  
  joystick1[0] = analogRead(A1); //X
  joystick1[1] = analogRead(A0); //Y
  joystick1[2] = digitalRead(11); //z
  
  joystick2[0] = analogRead(A3); //X
  joystick2[1] = analogRead(A2); //Y
  joystick2[2] = digitalRead(12); //Z
 
    
  moveMotor1(joystick2[0]);
  moveMotor1(joystick1[1]);
  moveMotor4(joystick1[0]);
  moveMotor3(joystick2[1]);  
  moveHand(joystick1[2]);
  automaticDraw(joystick2[2] == 0);
  
  delay(20);
}