/* Driving two DC motors 
for open columns based on CO2 levels. 9/2007
brian larson clark
*/
int debug = 0;            //change to 1 to exit debug mode
int manualControl = 1;    //set to 1 to exit mode

//#define ebutton1  2    // for motor2 defines pins for interrupts
//#define ebutton2  3    // for motor1

int irVal1;
int irVal2;
int irPin1 = 3;
int irPin2 = 4;

int encoder1State = 0;
int encoder2State = 0;

int fullTimeLengthUp = 48 * 3;  // 2min 41 sec
int fullTimeLengthDown = 47 * 3; // 2min 36 sec
int thirdTimeLengthUp = 48;   //53
int thirdTimeLengthDown = 47; //52
int thirdTimeLengthDif = 1;
int topTimeBuffer= 8; // 8 sec

long randNumber;

int ledPin0 = 13;      // LED connected to digital pin 13 

//pins for motor 1
int mode1 = 9;
int phase1 = 8;
int enable1 = 11;

int sensorThreshold = 10;

//pins for motor2
int mode2 = 7;
int phase2 = 6;
int enable2 = 10;

//input pins
int co2Pin1 = 5;     // input pin to test a/d values on motors 

int stopButton1 = 1;    // motor 1
int stopButton2 = 2;    // motor 2
int stopValue1 = 0;
int stopValue2 = 0;

int manualUpDown = 12;  // analog pin assignment for manually controlling motor
int manualMotor = 5;
int manualOn = 4;
int upDownState = 0;   // variable to capture manual control data
int motorState = 0;
int onState = 0;

int val1 = 0;            //co2 sensor values for debugging and motor function
int oldVal1 = 0;         // stores sensor data from previous loop

int co2max1 = 900; // 1023;        //for defining max and min values of co2 sensors
int co2min1 = 550; // 485;

int limit1;
int limit2;

int motor1Dir = 3;        //for determining the motor state during interrupt
int motor2Dir = 3;
//motorDir = O  Stable
//motorDir = 1  Forward (Up)
//motorDir = 2  Backwards (Down)
int motor1Limit = 4;   //1 lowest 4 highest
int motor2Limit = 4;
//motorLimit = 0 In-between
//motorLimit = 1 Top
//motorLimit = 2 Bottom

int motor1Position = 4;    // determines the 4 static positions of both motors
int motor2Position = 4;

int x = 1;
int calibrated1 = 0;
int calibrated2 = 0;
int done1 = 0;
int done2 = 0;


void setup() 
{ 
  pinMode (ledPin0, OUTPUT);     // sets the digital pin as output 
  
  pinMode (mode1, OUTPUT);     // sets the digital pin as output 
  pinMode (phase1, OUTPUT);    // sets the digital pin as output
  pinMode (enable1, OUTPUT);   // sets the digital pin as output 
  
  pinMode (mode2, OUTPUT);     // sets the digital pin as output 
  pinMode (phase2, OUTPUT);    // sets the digital pin as output
  pinMode (enable2, OUTPUT);   // sets the digital pin as output 
    
  pinMode (manualUpDown, INPUT);     // sets the digital pin as output 
  pinMode (manualMotor, INPUT);    // sets the digital pin as output
  pinMode (manualOn, INPUT);   // sets the digital pin as output 

  if (debug == 0) {
    Serial.begin(9600);        // connect to the serial port
  } 
}


void loop() 
{ 

 motor2_down();
  delay(10000);
  while (calibrated1 == 0 || calibrated2 == 0) {
    calibrateIR(); //calibratebothmotors
  }
  
  Serial.println("initial calibrate");
  calibrated1 = 0;
  calibrated2 = 0;
  //done1 = 0;
  //done2 = 0;


  delay(6000);
  

  //stopValue1 = analogRead(stopButton1);
  //stopValue2 = analogRead(stopButton2);
  //val1 = analogRead(co2Pin1);
  //Serial.println(stopValue1);
  //Serial.println(stopValue2);
  //Serial.println(co2Pin1);

  while(1) {
    for (int i=1; i <= 20; i++)
    {
      val1 = analogRead(co2Pin1);    // read the value from the sensor
      Serial.print("analogRead "); Serial.println(val1);

 
      if ((abs (val1  - oldVal1)) >= 2) {
        if (val1 > oldVal1) {
          bothMotorsQuarterDown();
          delay(thirdTimeLengthDown * 200);
          Serial.print("co2 going up "); Serial.println(val1);
        }
  
        else {
          bothMotorsQuarterUp();
          delay(thirdTimeLengthDown * 200);
          Serial.print("co2 going down "); Serial.println(val1);
          stopValue1 = analogRead(stopButton1);
          stopValue2 = analogRead(stopButton2);
          
          if (irVal1 >= 500 || irVal2 >= 400) {
            calibrateIR();
            calibrated1 = 0;
            calibrated2 = 0;
            done1 = 0;
            done2 = 0;
            
            motor1Position = 4;
            motor2Position = 4;         
          }
        }
      
       }
   
      else {
        motorsRandom();
        delay(thirdTimeLengthDown * 200);
        Serial.println(val1);
        Serial.print("co2 stable "); Serial.println(val1);
        
        if (irVal1 >= 400 || irVal2 >= 400) {
            calibrateIR();
            calibrated1 = 0;
            calibrated2 = 0;
            done1 = 0;
            done2 = 0;
            
            motor1Position = 4;
            motor2Position = 4;
         
          }
      }
  
      oldVal1 = val1;
      
    }
    
    while (calibrated1 == 0 || calibrated2 == 0) {
      calibrateIR(); //calibrate both motors
    }
    
    calibrated1 == 0;
    calibrated2 == 0;
    done1 = 0;
    done2 = 0;
    
    motor1Position = 4;
    motor2Position = 4;
  }
}


void motor1QuarterUp()
{
  if (motor1Position < 4) {
    motor1_up();
    delay(thirdTimeLengthUp * 500); //need to split this up because delays can't be a long int
    delay(thirdTimeLengthDown * 500); //wrong on purpose, this motor moves up slightly faster for reason
    motor1_stop();
    motor1Position = motor1Position + 1;
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else if (motor1Position == 4) {
    motor1_stop();
    
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else {
   // motor1_calibrate();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
}

void motor1QuarterDown() {
  if (motor1Position > 1) {
    motor1_down();
    delay(thirdTimeLengthDown * 500);
    delay(thirdTimeLengthDown * 500);
    motor1_stop();
    motor1Position = motor1Position - 1;
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else if (motor1Position == 1) {
    motor1_stop();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else {
   // motor1_calibrate();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
}

void motor2QuarterUp()
{
  if (motor2Position < 4) {
    motor2_up();
    delay(thirdTimeLengthUp * 500); //need to split this up because delays can't be a long int
    delay(thirdTimeLengthUp * 500);
    motor2_stop();
    motor2Position = motor2Position + 1;
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else if (motor2Position == 4) {
    motor2_stop();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else {
   // motor2_calibrate();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
}

void motor2QuarterDown() {
  if (motor2Position > 1) {
    motor2_down();
    delay(thirdTimeLengthDown * 500);
    delay(thirdTimeLengthDown * 500);
    motor2_stop();
    motor2Position = motor2Position - 1;
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else if (motor2Position == 1) {
    motor2_stop();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
  
  else {
   // motor2_calibrate();
    Serial.print(motor1Position);
    Serial.println(motor2Position);
  }
}

void bothMotorsQuarterDown() {
  if (motor1Position > 1) {
    if (motor2Position > 1) {
      motor_bothDown();
      delay(thirdTimeLengthDown * 500);
      delay(thirdTimeLengthDown * 500);
      motor1Position = motor1Position - 1;
      motor2Position = motor2Position - 1;
      allStop();
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    } 
    else {
      motor2_stop();
      motor1QuarterDown();
      motor1Position = motor1Position - 1;
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    }
  }
  else {
    if (motor1Position == 1) {
      if (motor2Position > 1) {
        motor1_stop();
        motor2QuarterDown();
        Serial.print(motor1Position);
        Serial.println(motor2Position);
        motor2Position = motor2Position - 1;
      }
      else {
        allStop();
        delay(thirdTimeLengthDown * 500);
        delay(thirdTimeLengthDown * 500);
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
     }
   }
}

void bothMotorsQuarterUp() {
  if (motor1Position < 4) {
    if (motor2Position < 4) {
      motor_bothUp();
      delay(thirdTimeLengthUp * 500);
      delay(thirdTimeLengthUp * 500);
      motor1Position = motor1Position + 1;
      motor2Position = motor2Position + 1;
      allStop();
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    } 
    else {
      motor2_stop();
      motor1QuarterUp();
      motor1Position = motor1Position + 1;
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    }
  }
  else {
    if (motor1Position == 4) {    //remember greater equal
      if (motor2Position < 4) {
        motor1_stop();
        motor2QuarterUp();
        motor2Position = motor2Position + 1;
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
      else {
        allStop();
        delay(thirdTimeLengthDown * 500);
        delay(thirdTimeLengthDown * 500);
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
     }
   }
}

void bothMotorsUpDown() {    // motor1 up motor2 down
  if (motor1Position < 4) {
    if (motor2Position > 1) {
      motor1_up();
      delay(thirdTimeLengthDif);
      motor2_down();
      delay(thirdTimeLengthDown * 500);
      delay(thirdTimeLengthDown * 500);
      motor1Position = motor1Position + 1;
      motor2Position = motor2Position - 1;
      allStop();
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    } 
    else {
      motor2_stop();
      motor1QuarterUp();
      motor1Position = motor1Position + 1;
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    }
  }
  else {
    if (motor1Position == 4) {    //remember greater equal
      if (motor2Position > 1) {
        motor1_stop();
        motor2QuarterDown();
        motor2Position = motor2Position -1;
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
      else {
        allStop();
        delay(thirdTimeLengthDown * 500);
        delay(thirdTimeLengthDown * 500);
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
     }
   }
}

void bothMotorsDownUp() {    // motor1 up motor2 down
  if (motor1Position > 1) {
    if (motor2Position < 4) {
      motor1QuarterDown();
      motor2QuarterUp();
      delay(thirdTimeLengthDown * 500);   ///flag
      delay(thirdTimeLengthDown * 500);
      motor1Position = motor1Position - 1;
      motor2Position = motor2Position + 1;
      allStop();
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    } 
    else {
      motor2_stop();
      motor1QuarterDown();
      motor1Position = motor1Position - 1;
      Serial.print(motor1Position);
      Serial.println(motor2Position);
    }
  }
  else {
    if (motor1Position == 1) {    //remember greater equal
      if (motor2Position < 4) {
        motor1_stop();
        motor2QuarterUp();
        motor2Position = motor2Position + 1;
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
      else {
        allStop();
        delay(thirdTimeLengthDown * 500);
        delay(thirdTimeLengthDown * 500);
        Serial.print(motor1Position);
        Serial.println(motor2Position);
      }
     }
   }
}

void calibrateIR() {
  irVal1 = analogRead(irPin1);
  Serial.println(irVal1);
  stopValue1 = analogRead(stopButton1);
  
  if (irVal1 >= 500 || stopValue1 <= 700) {
    motor1_down();
    delay(4000);
    motor1_stop();
    calibrated1 = 1;
  }
  
  else if (irVal1 < 500 && calibrated1 == 0) {
    motor1_upCal();
    delay(50);
  
    motor1_stopCal();
    calibrated1 = 0;
  }
  
  else {
    motor1_stop();
  }
 
  irVal2 = analogRead(irPin2);
  stopValue2 = analogRead(stopButton2);
  
  
  
  if (irVal2 >= 400 || stopValue2 <= 700) {
    motor2_down();
    delay(4000);
    motor2_stop();
    calibrated2 = 1;
  }
  
  else if (irVal2 < 400 && calibrated2 == 0) {
    motor2_upCal();
    delay(50);
  
    motor2_stopCal();
    calibrated2 = 0;
  }
  
  else {
    motor2_stop();
  }
}

void calibrateIR2() {
  irVal1 = analogRead(irPin1);
  stopValue1 = analogRead(stopButton1);
  irVal2 = analogRead(irPin2);
  stopValue2 = analogRead(stopButton2);
  
//  if (done1 != 0 || done2 != 0) {
  
    if (irVal1 >= 500 || stopValue1 <= 700) {
      motor1_down();
      delay(4000);
      motor1_stop();
      calibrated1 = 1;
      done1 = 1;
    }
  
    else if (irVal1 < 500 && calibrated1 == 0 && done1 != 1) {
      motor1_upCal();
      delay(50);
  
      motor1_stopCal();
      calibrated1 = 0;
    }
  
    else {
      if (irVal2 >= 400 || stopValue2 <= 700) {
        motor2_down();
        delay(4000);
        motor2_stop();
        calibrated2 = 1;
        done2 = 1;
      }
  
      else if (irVal2 < 400 && calibrated2 == 0) {
        motor2_upCal();
        delay(50);
  
        motor2_stopCal();
        calibrated2 = 0;
      }
  
      else {
        motor2_stop();
      }
    }
  
    irVal1 = analogRead(irPin1);
    Serial.println(irVal1);
    stopValue1 = analogRead(stopButton1);
    irVal2 = analogRead(irPin2);
    stopValue2 = analogRead(stopButton2);
  
    if (irVal2 >= 500 || stopValue2 <= 700) {
      motor2_down();
      delay(4000);
      motor2_stop();
      calibrated2 = 1;
      done2 = 1;
    }
  
    else if (irVal2 < 500 && calibrated2 == 0) {
      motor2_upCal();
      delay(50);
  
      motor2_stopCal();
      calibrated1 = 0;
    }
  
    else {
      if (irVal1 >= 400 || stopValue1 <= 700) {
        motor1_down();
        delay(4000);
        motor1_stop();
        calibrated1 = 1;
        done2 = 1;
      }
  
      else if (irVal1 < 400 && calibrated1 == 0) {
        motor1_upCal();
        delay(50);
  
        motor1_stopCal();
        calibrated1 = 0;
      }
  
      else {
        motor1_stop();
      }
    }
//   }
   
//   else {
     calibrated1 = 1;
     calibrated2 = 1;
//   }
 }

void manual() {
  if (manualControl == 0) {
    while(1)
    {
      upDownState = digitalRead(manualUpDown);  // read input value
      motorState = digitalRead(manualMotor);  // read input value
      onState = digitalRead(manualOn);  // read input value
      stopValue1 = analogRead(stopButton1);
      stopValue2 = analogRead(stopButton2);
      
      Serial.println (upDownState);
      Serial.println (motorState);
      Serial.println (onState);
      Serial.println (stopValue1);
      Serial.println (stopValue2);
      
      if (stopValue1 > LOW) {
         motor1_down;
      }
      
      if (stopValue2 > LOW) {
         motor2_down;
      }
        
        
      
      if (onState == HIGH) {
        if (motorState == HIGH) {
          if (upDownState == HIGH) { 
            motor1_up();
          
           }                 
          else {
            motor1_down();

           }
        }
        
        else {
          if (upDownState == HIGH) { 
            motor2_up();

           }
          else {
            motor2_down();
         }
        }
      }
        
      else {
        motor1_stop();
        motor2_stop();
      }
       
    }
  }
  
  else {
    delay(100);
  }
}

void readStopValue1() {
    stopValue1 = analogRead(stopButton1);
    
    Serial.print ("stopValue1 = "); Serial.println (stopValue1);
    
    if (stopValue1 < 500) {
      motor1_stop();
      delay(5000);
      motor1_down();
      delay(500);
    }
}

void second(int seconds) {
  for(int i = 0; i < seconds; i++) {
    delay(1000);
    Serial.print (seconds);
  }
}

////////////motor control functions/////////////////these work perfectly, so don't touch

void motor1_down() {
    digitalWrite(mode1, 1);
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, 1);
    
    motor1Dir = 1;
    delay(300);
}

void motor1_up() {
    digitalWrite(mode1, 0); 
    digitalWrite(phase1, 1); 
    digitalWrite(enable1, 1);
    
    motor1Dir = 2;
    delay(300);
}

void motor1_upCal() {
    digitalWrite(mode1, 0); 
    digitalWrite(phase1, 1); 
    digitalWrite(enable1, 1);
    
    motor1Dir = 2;
    delay(20);
}

void motor1_stop() {
    digitalWrite(mode1, 0); 
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, 0);
    
    //motor1Dir = 0;
    delay(300);
}

void motor1_stopCal() {
    digitalWrite(mode1, 0); 
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, 0);
    
    //motor1Dir = 0;
    //delay(100);
}

void motor2_down() {
    digitalWrite(mode2, 1);
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, 1);
    
    motor2Dir = 1;
    delay(300);
}

void motor2_up() {
    digitalWrite(mode2, 0);
    digitalWrite(phase2, 1); 
    digitalWrite(enable2, 1);
    
    motor2Dir = 2;
    delay(300);
}

void motor2_upCal() {
    digitalWrite(mode2, 0);
    digitalWrite(phase2, 1); 
    digitalWrite(enable2, 1);
    
    motor2Dir = 2;
    delay(20);
}

void motor2_stop() {
    digitalWrite(mode2, 0);
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, 0);
    
    //motor2Dir = 0;
    delay(300);
}

void motor2_stopCal() {
    digitalWrite(mode2, 0); 
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, 0);
    
    //motor2Dir = 0;
    //delay(100);
}

void motor_bothDown() {
    digitalWrite(mode1, 1);
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, 1);
    digitalWrite(mode2, 1);
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, 1);
    
    motor1Dir = 1;
    motor2Dir = 1;
    delay(300);
}

void motor_bothUp() {
    digitalWrite(mode1, 0);
    digitalWrite(phase1, 1); 
    digitalWrite(enable1, 1);
    digitalWrite(mode2, 0);
    digitalWrite(phase2, 1); 
    digitalWrite(enable2, 1);

    motor1Dir = 2;    
    motor2Dir = 2;
    delay(300);
}

void motor_UpDown() {
    digitalWrite(mode1, 0); 
    digitalWrite(phase1, 1); 
    digitalWrite(enable1, 1);
    digitalWrite(mode2, 1);
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, 1);

    motor1Dir = 2;    
    motor2Dir = 1;
    delay(300);
}

void motor_DownUp() {
    digitalWrite(mode1, 1);
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, 1);
    digitalWrite(mode2, 0);
    digitalWrite(phase2, 1); 
    digitalWrite(enable2, 1);
    
    motor1Dir = 1;
    motor2Dir = 2;
    delay(300);
}

void allStop() {
    digitalWrite(mode1, 0);
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, 0);
    digitalWrite(mode2, 0);
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, 0);
    
    motor1Dir = 0;
    motor2Dir = 0;
    delay(300);
}
  

/* In case varied speed is needed
void motor1_downPWM() {
    digitalWrite(mode1, 1);
    digitalWrite(phase1, 1); 
    digitalWrite(enable1, PWM1);
    
    motor1Dir = 1;
    delay(300);
}

void motor1_upPWM() {
    digitalWrite(mode1, 1);
    digitalWrite(phase1, 0); 
    digitalWrite(enable1, PWM1);
    
    motor1Dir = 2;
    delay(300);
}

void motor2_downPWM() {
    digitalWrite(mode2, 1);
    digitalWrite(phase2, 1); 
    digitalWrite(enable2, PWM2);
    
    motor2Dir = 1;
    delay(300);
}

void motor2_upPWM() {
    digitalWrite(mode2, 1);
    digitalWrite(phase2, 0); 
    digitalWrite(enable2, PWM2);
    
    motor2Dir = 2;
    delay(300);
}

*/

void motor1_AllDown() {
  motor1QuarterDown();
  motor1QuarterDown();
  motor1QuarterDown();
  motor1_stop();
}

void motor2_AllDown() {
  motor2QuarterDown();
  motor2QuarterDown();
  motor2QuarterDown();
  motor2_stop();
}

void motor1_AllUp() {
  motor1QuarterUp();
  motor1QuarterUp();
  motor1QuarterUp();
  motor1_stop();
}

void motor2_AllUp() {
  motor2QuarterUp();
  motor2QuarterUp();
  motor2QuarterUp();
  motor2_stop();
}

void motor_BothAllDown() {
  bothMotorsQuarterDown();
  bothMotorsQuarterDown();
  bothMotorsQuarterDown();
  allStop();
}

void motor_BothAllUp() {
  bothMotorsQuarterUp();
  bothMotorsQuarterUp();
  bothMotorsQuarterUp();
  allStop();
}

void allStop_timed() {
  allStop();
  delay(thirdTimeLengthDown * 500);
  delay(thirdTimeLengthDown * 500);
}

void motorsRandom() {
  randNumber = random(1,9);
  Serial.println(randNumber);
  
  switch (randNumber) {
    case 1:
      motor1QuarterUp();
      break;
    case 2:
      motor1QuarterDown();
      break;
    case 3:
      motor2QuarterUp();
      break;
    case 4:
      motor2QuarterDown();
      break;
    case 5:
      bothMotorsQuarterUp();
      break;
    case 6:
      bothMotorsQuarterDown();
      break;
    case 7:
      bothMotorsUpDown();
      break;
    case 8:
      bothMotorsDownUp();
      break;
    default:
      allStop_timed();
  }
  
  Serial.print(motor1Position);
  Serial.println(motor2Position);
}