Lab_interaccio/2015/Ballena/Ballena_control/Ballena_control.ino

#define UP 0
#define DOWN 1
#define STOP 2

#define DEBUG false

int G[5] = { 33, 31, 29, 27, 25};
//int fc[5] = {A2, A1, A0, 13, 12 };
int button[3] = { A5, A4, A3};
int rele_up[3] = { 11, 9, 7 };
int rele_down[3] = { 10, 8, 6 };


void setup() {
  Serial.begin(9600);
  Serial1.begin(9600);
//  for (int i = 0; i < 5; i++)
//  {
//    pinMode(fc[i], INPUT);
//    digitalWrite(fc[i], HIGH);
//  }
  for (int i = 0; i < 3; i++)
  {
    pinMode(button[i], INPUT);
    digitalWrite(button[i], HIGH);
  }
 for (int i = 0; i < 3; i++)
  {
    pinMode(rele_up[i], OUTPUT);
    digitalWrite(rele_up[i], HIGH);
  }
 for (int i = 0; i < 3; i++)
  {
    pinMode(rele_down[i], OUTPUT);
    digitalWrite(rele_down[i], HIGH);
  }
}

int mode_ant[3] = { STOP, STOP, STOP};
unsigned long time_motor[3] = { 0, 0, 0 };
unsigned long time_final[3] = { 0, 0, 0 };
boolean flag_motor[3] = { 0, 0, 0 };

void motor(int num, int mode, unsigned long time)
  {
    if ((mode_ant[num]!=STOP)&&(mode!=STOP)&&(mode!=mode_ant[num]))
      {
        digitalWrite(rele_up[num], HIGH);    
        digitalWrite(rele_down[num], HIGH);  
        delay(1000);
      }
    if (mode==UP)
      {
        digitalWrite(rele_up[num], LOW);    
        digitalWrite(rele_down[num], HIGH);
      }
    else if (mode==DOWN)
      {
        digitalWrite(rele_up[num], HIGH);    
        digitalWrite(rele_down[num], LOW);
      } 
    else
      {
        digitalWrite(rele_up[num], HIGH);    
        digitalWrite(rele_down[num], HIGH);  
      }
    if (time!=0)
      {
        time_final[num] = time;
        time_motor[num] = millis();
        flag_motor[num] = true;
        #if DEBUG==true
          Serial.print("Motor ");
          Serial.print(num);
          if (mode==UP) Serial.println(" UP");
          else if (mode==DOWN) Serial.println(" DOWN");
          else if (mode==STOP) Serial.println(" STOP");
        #endif
      }
    else flag_motor[num] = false;
    mode_ant[num] = mode;  
  }

int all_mode_ant = STOP;

void allmotors(int mode, unsigned long time)
  {
    if ((all_mode_ant!=STOP)&&(mode!=STOP)&&(mode!=all_mode_ant))
      {
        digitalWrite(rele_up[0], HIGH);    
        digitalWrite(rele_down[0], HIGH);  
        digitalWrite(rele_up[1], HIGH);    
        digitalWrite(rele_down[1], HIGH);  
        digitalWrite(rele_up[2], HIGH);    
        digitalWrite(rele_down[2], HIGH);  
        delay(1000);
      }
    if (mode==UP)
      {
        digitalWrite(rele_up[0], LOW);    
        digitalWrite(rele_down[0], HIGH);
        digitalWrite(rele_up[1], LOW);    
        digitalWrite(rele_down[1], HIGH);
        digitalWrite(rele_up[2], LOW);    
        digitalWrite(rele_down[2], HIGH);
      }
    else if (mode==DOWN)
      {
        digitalWrite(rele_up[0], HIGH);    
        digitalWrite(rele_down[0], LOW);
        digitalWrite(rele_up[1], HIGH);    
        digitalWrite(rele_down[1], LOW);
        digitalWrite(rele_up[2], HIGH);    
        digitalWrite(rele_down[2], LOW);
      } 
    else
      {
        digitalWrite(rele_up[0], HIGH);    
        digitalWrite(rele_down[0], HIGH);  
        digitalWrite(rele_up[1], HIGH);    
        digitalWrite(rele_down[1], HIGH); 
        digitalWrite(rele_up[2], HIGH);    
        digitalWrite(rele_down[2], HIGH); 
      }
    all_mode_ant = mode;  
  }
  
void control()
  {
    while (flag_motor[0]||flag_motor[1]||flag_motor[2])
      {
        for (int i=0; i<3; i++)
          {
            if (flag_motor[i])
              {
//                Serial.print("Motor ");
//                Serial.print(i);
//                if (mode_ant[i]==UP) Serial.println(" UP");
//                else if (mode_ant[i]==DOWN) Serial.println(" DOWN");
//                else if (mode_ant[i]==STOP) Serial.println(" STOP");
                
                if (((millis()-time_motor[i]) >= time_final[i])||((!digitalRead(button[i]))&&(mode_ant[i]==UP)))
                  {
                    motor(i,STOP,0);
                    #if DEBUG==true
                      Serial.print("Motor ");
                      Serial.print(i);
                      Serial.print(" STOP ");
                      Serial.println(millis()-time_motor[i]);
                    #endif
                  }
              }
          }
      }
  }
  
void loop() {

//if (!digitalRead(fc[2]))  motor(0, UP, 100);
//else if (!digitalRead(fc[1])) motor(0, DOWN, 100);
//else if (!digitalRead(fc[0])) motor(0, STOP, 100);

//if (!digitalRead(fc[2]))  motor(1, UP, 100);
//else if (!digitalRead(fc[1])) motor(1, DOWN, 100);
//else if (!digitalRead(fc[0])) motor(1, STOP, 100);

//if (!digitalRead(fc[2]))  motor(2, UP, 100);
//else if (!digitalRead(fc[1])) motor(2, DOWN, 100);
//else if (!digitalRead(fc[0])) motor(2, STOP, 100);

if (!digitalRead(button[2]))  allmotors(UP, 100);
else if (!digitalRead(button[1])) allmotors(DOWN, 100);
else if (!digitalRead(button[0])) allmotors(STOP, 100);


// motor(0, UP, 100);
// motor(1, UP, 100);
// motor(2, UP, 100);
// delay(4000);
// motor(0, DOWN, 100);
// motor(1, DOWN, 100);
// motor(2, DOWN, 100);
// delay(4000);
// motor(0, STOP, 0);
// delay(1000);
// motor(0, DOWN, 0);
// delay(1000);
 
// control();
// delay(1000);

//  if (Serial.available())
//    Serial1.write(Serial.read());
//  if (Serial1.available())
//    Serial.write(Serial1.read());
    
}