titrator-robotev.cpp

/*

robotev.com 

*/

//#########################################################################################################

const int _nom_cycles = 3;    /// Broii izmerwania ; Minimalen 2;   Maksimalen borii 30

const int _timeSemp = 3000;   /// [ms]Pauza mejdu otchitaniata na PH met yra

//#########################################################################################################


#include <Omega_PHH37.h>

Omega_PHH37 Meter = Omega_PHH37();


// Parameters #########################################################################################

const unsigned int _Motor1Speed = 600; // [us]   0.9 [deg/_Motor1Speed]    !!! Minimum 500us !!!
const unsigned int _Motor2Speed = 600; // [us]   0.9 [deg/_Motor2Speed]    !!! Minimum 500us !!!

const  long _Delta_Motor1_limit = 50000; // [steps]
const  long _Delta_Motor2_limit = 50000; // [steps]

// ######################################################################################## Parameters



// I/O Pins ##########################################################################################

const int _limitSwitchMotor1A = 14;
const int _limitSwitchMotor1B = 15;
const int _limitSwitchMotor2A = 16;
const int _limitSwitchMotor2B = 17;

const int _Motor1Step = 11;
const int _Motor1DIR = 12;
const int _Motor1EN = 10;

const int _Motor2Step = 8;
const int _Motor2DIR = 9;
const int _Motor2EN = 7;

const int _startCycle = 6;
const int _status_led = 13;

// ######################################################################################### I/O Pins


const int _startCycle_index = 0;
const int _Motor1CCW_index = 1; 
const int _Motor2CW_index = 2;
const int _Motor2CCW_index = 3;

boolean buttonArray[] = {false,false,false,false};
boolean buttonHold[] = {false,false,false,false};
boolean bCycleBegin = false;
boolean bReactionStable = false;

double intReadings[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
double intCurrentReading = 0;
double intLastReading = 0;

long intStepCounterMotor1 = _Delta_Motor1_limit;
long intStepCounterMotor2 = _Delta_Motor2_limit;

int intCycleCounter = 0;
int intStableCounter = 0;

boolean Motor1DIR = false;
boolean Motor2DIR = false;

boolean Motor1EN = false;
boolean Motor2EN = false;

unsigned long uslLastMicrosMotor1 = 0;
unsigned long uslLastMicrosMotor2 = 0;

unsigned long uloMillisLED = 0;

unsigned long uslLastMillisButtonPressed[] = {0,0,0,0};

// Functions ##########################################################

void readButtons(){

  for (int i = 0; i <= 3; i++)
    if (uslLastMillisButtonPressed[i] > millis())
      uslLastMillisButtonPressed[i] = millis();
  
// Released ======================================================

  if (digitalRead(_startCycle) == HIGH && buttonArray[_startCycle_index]){
    buttonArray[_startCycle_index] = false;
  }


// Pressed ========================================================

// ------------- _startCycle ----------------------------------------
  if (digitalRead(_startCycle) == LOW && !buttonArray[_startCycle_index]){

    if ( !buttonHold[_startCycle_index] ){
      uslLastMillisButtonPressed[_startCycle_index] = millis();
      buttonHold[_startCycle_index] = true;
    }

    if ( (digitalRead(_startCycle) == LOW) && (millis() - uslLastMillisButtonPressed[_startCycle_index] >= 16) ){
      buttonArray[_startCycle_index] = true;
      buttonHold[_startCycle_index] = false;
    }

  }



}


void Motor1Control(){
  if (micros() < uslLastMicrosMotor1)
    uslLastMicrosMotor1 = micros();
  
  if ( (micros() - uslLastMicrosMotor1 >= _Motor1Speed) && !Motor1EN && ( abs(intStepCounterMotor1) != _Delta_Motor1_limit) ){
      uslLastMicrosMotor1 = micros(); 

//      if(!digitalRead(_limitSwitchMotor1A) || !digitalRead(_limitSwitchMotor1B)){
//        Motor1EN = true;
//          digitalWrite(_Motor1EN,Motor1EN);
//              
//        return;
//      }
     
     
      digitalWrite(_Motor1DIR,Motor1DIR);
      digitalWrite(_Motor1EN,Motor1EN);
      digitalWrite(_Motor1Step,HIGH);
      delayMicroseconds(5);
      digitalWrite(_Motor1Step,LOW);
      
      if (!Motor1DIR){
        intStepCounterMotor1++;
      }else{
        intStepCounterMotor1--;
      }
     
  }
  if ( abs(intStepCounterMotor1) == _Delta_Motor1_limit ){
    Motor1EN = true;
    digitalWrite(_Motor1EN,Motor1EN);
  }
}


void Motor2Control(){
  if (micros() < uslLastMicrosMotor2)
    uslLastMicrosMotor2 = micros();
    
  if ( (micros() - uslLastMicrosMotor2 >= _Motor2Speed) && !Motor2EN && ( abs(intStepCounterMotor2) != _Delta_Motor2_limit) ){
      uslLastMicrosMotor2 = micros(); 
      
//      if(!digitalRead(_limitSwitchMotor2A) || !digitalRead(_limitSwitchMotor2B)){
//        Motor2EN = true;
//          digitalWrite(_Motor2EN,Motor2EN);
//              
//
//        return;
//      }
      
      
      digitalWrite(_Motor2DIR,Motor2DIR);
      digitalWrite(_Motor2EN,Motor2EN);
     
 
      digitalWrite(_Motor2Step,HIGH);
      delayMicroseconds(5);
      digitalWrite(_Motor2Step,LOW);

      if (!Motor2DIR){
        intStepCounterMotor2++;
      }else{
        intStepCounterMotor2--;
      }
    }
    
    if ( abs(intStepCounterMotor2) == _Delta_Motor2_limit ){
      Motor2EN = true;
      digitalWrite(_Motor2EN,Motor2EN);
    }
    
 }


void CW_Motor1(){
    uslLastMicrosMotor1 = millis();

          Motor1DIR = false;
            Motor1EN = false;
              intStepCounterMotor1 = 0;

//                      Serial.println("Motor 1: Button_CW pressed");
//                        Serial.println("---------------------------------");
}


void CCW_Motor1(){
    uslLastMicrosMotor1 = millis();

          Motor1DIR = true;
            Motor1EN = false;
              intStepCounterMotor1 = 0;

//                        Serial.println("Motor 1 : Button_CCW pressed");
//                          Serial.println("-----------------------------");
}


void CW_Motor2(){
    uslLastMicrosMotor2 = millis();

          Motor2DIR = false;
            Motor2EN = false;
              intStepCounterMotor2 = 0;
            
//                      Serial.println("Motor 2: Button_CW pressed");
//                        Serial.println("---------------------------------");
}

void CCW_Motor2(){
    uslLastMicrosMotor2 = millis();

          Motor2DIR = true;
            Motor2EN = false;
              intStepCounterMotor2 = 0;
            
//                    Serial.println("Motor 2 : Button_CCW pressed");
//                       Serial.println("---------------------------------");
}

void homePosition(){
  
  if ( digitalRead(_limitSwitchMotor2A) ){
    CW_Motor2();
    
    while( digitalRead(_limitSwitchMotor2A) ){
      Motor2Control();
    }
      
      Motor2EN = true;
      digitalWrite(_Motor2EN,Motor2EN);
  
  }


  if (digitalRead(_limitSwitchMotor1B)){
    CCW_Motor1();
    while(digitalRead(_limitSwitchMotor1B)){
      Motor1Control(); 
    }
     Motor1EN = true; 
     digitalWrite(_Motor1EN,Motor1EN);
  }
  


}

void stopMotors(){

  Motor2EN = true;
  digitalWrite(_Motor2EN,Motor2EN);
  Motor1EN = true; 
  digitalWrite(_Motor1EN,Motor1EN);
     
}
 // ########################################################## Functions


void setup(){

  Serial.begin(9600);
  Meter.begin();
  
  
  
//  Serial.println("**| Motor 1 |*********************************************");
//  Serial.print("Speed: ");
//  Serial.print(" 0.9 deg / ");
//  Serial.print(_Motor1Speed);
//  Serial.println("us");
//  Serial.print("Maximum delta steps: ");
//  Serial.println(_Delta_Motor1_limit);
//  Serial.println();
//  Serial.println("**| Motor 2 |*********************************************");
//  Serial.print("Speed: ");
//  Serial.print(" 0.9 deg / ");
//  Serial.print(_Motor2Speed);
//  Serial.println("us");
//  Serial.print("Maximum delta steps: ");
//  Serial.println(_Delta_Motor2_limit);
//  Serial.println();
//  Serial.println("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~");
  
  pinMode(_limitSwitchMotor1A,INPUT);
    pinMode(_limitSwitchMotor1B,INPUT);
      pinMode(_limitSwitchMotor2A,INPUT);
        pinMode(_limitSwitchMotor2B,INPUT);
        
          digitalWrite(_limitSwitchMotor1A,HIGH);
            digitalWrite(_limitSwitchMotor1B,HIGH);
              digitalWrite(_limitSwitchMotor2A,HIGH);
                digitalWrite(_limitSwitchMotor2B ,HIGH);
   
  pinMode(_startCycle,INPUT);  
    digitalWrite(_startCycle,HIGH);
    
  pinMode(_status_led, OUTPUT);
          

  pinMode(_Motor1Step,OUTPUT);
    pinMode(_Motor1DIR,OUTPUT);
      pinMode(_Motor1EN,OUTPUT);
        pinMode(_Motor2Step,OUTPUT);
          pinMode(_Motor2DIR,OUTPUT);
            pinMode(_Motor2EN,OUTPUT);      
  
   digitalWrite(_Motor1Step,LOW);
     digitalWrite(_Motor1DIR,LOW);
       digitalWrite(_Motor1EN,HIGH);
         digitalWrite(_Motor2Step,LOW);
           digitalWrite(_Motor2DIR,LOW);
             digitalWrite(_Motor2EN,HIGH);
             
             homePosition();

             delay(2000);

}



void loop(){
    
  readButtons();
  
  if (!bCycleBegin && buttonArray[_startCycle_index]){
     bCycleBegin = true;
     intCycleCounter = 0;
     intStableCounter = 0;
     homePosition();
  
  }
  
  if (bCycleBegin){
       homePosition();
    
    CW_Motor1();    
    while(digitalRead(_limitSwitchMotor1A))
      Motor1Control();
    
    delay(1000);
    
    CCW_Motor2();
    while(digitalRead(_limitSwitchMotor2B))
      Motor2Control();
      
      delay(1000);

    CCW_Motor1();    
    while(digitalRead(_limitSwitchMotor1B))
       Motor1Control();
      
      delay(1000);
      
 
    intCycleCounter++;
  //  bCycleBegin = false;   
  
  delay(1000);
  
   intReadings[ intCycleCounter + 1 ] = waitStablize();
    
   if (intCycleCounter == _nom_cycles){
     bCycleBegin = false;     
   stopMotors();
   printData();
   intCycleCounter = 0;
   }     

  
  }

}


void printData(){
 Serial.println();
 Serial.println("=================================================");
 Serial.println("Results");
 Serial.println("=================================================");
 Serial.println("N        ml        mV");

 
 for (int i = 1; i <= _nom_cycles; i++){
   Serial.print(i);
   Serial.print("       ");
   Serial.print(i);
   Serial.print("       ");
   Serial.print(intReadings[i+1]);
   Serial.println("");
 }
 Serial.println("==================================================");
 }
  





double waitStablize(){
  
  uloMillisLED = millis();
  intStableCounter = 0;
  while(intStableCounter < 4){
      blinkLED();
      
      if (millis() - uloMillisLED >= _timeSemp){        
         if(Meter.read()){
           intLastReading = intCurrentReading;
           intCurrentReading = (double)Meter.mV();
         
         Serial.print(intLastReading);
         Serial.print(" / " );
         Serial.println(intCurrentReading);
         
         
           if (intLastReading == intCurrentReading){
             intStableCounter++;
           }else{
             intStableCounter = 0;
           }
           
         }
         uloMillisLED = millis();
      }
  
  }
  return intCurrentReading;
}


void blinkLED(){
  digitalWrite(_status_led,HIGH);
  delay(60);
  digitalWrite(_status_led,LOW);
  delay(60);
}



// YBS 30.01.2012 ////////////////