// linearno_vodilo_IGUS_EMO_V1.6
// https://wokwi.com/projects/352825812586481665
//++++++++++++++++++++++++++++++++++++ 
//  igus nastavitve:
//  tabela premikov:
//  1 - ref
//  2 - home pomik ABS
//  3 - relativen pomik 400mm
//
//+++++++++++++++++++++++++++++++++++

boolean toggle4= LOW;
boolean toggle_start_LED = LOW;
boolean toggle_home_LED = LOW;
String serial_txt="";
String error_code="";

volatile boolean var_varnostna_zavesa=LOW;  // po defaultu 0=error, interupt sprozi HIGH
volatile boolean var_koncno_stikalo_vodilo=LOW; 
volatile boolean var_koncno_stikalo_reference_vodilo_=LOW; // stikalo za ref stikalom IGUSA zaradi dodatne varnosti


// definicije pinov v1.6 sprememba od 1.5!
const int DI_varnostna_zavesa = 2;  //interrupt0Pin 
const int DI_koncno_stikalo_presa = 3;  //interrupt1Pin 

const int DI_program_start_button = 5;
const int DI_program_home_button = 6;   // za rocno vracanje na home pozicijo

const int DI_koncno_stikalo_vodilo = 21; //interrupt2Pin stikalo za dolocanje dolzine profila
const int DI_koncno_stikalo_reference_vodilo = 22;

const int DO_presa_start_pulz = 8; 

// za pozicije porabim analogne porte 
const int DO1_igus_set_polozaj = A0; 
const int DO2_igus_set_polozaj = A1; 
const int DO3_igus_set_polozaj = A2; 
const int DO4_igus_set_polozaj = A3; 
const int DO5_igus_set_polozaj = A4; 
const int DO6_igus_start = A5;
const int DO7_igus_enable = A6;
const int DO10_igus_stop_reset = A7;   // 

// preveri ce delujejo kot inputi
const int DI1_igus_ready = A8; //8;
const int DI2_igus_active = A9; //9;
const int DI3_igus_reference = A10;  // rabim

// led izhodi
const int DO_start_LED =12;    
const int DO_home_LED =13;   

int var_program_stanje=0;   // stanja 1..5

void setup() {
  // put your setup code here, to run once:
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);

  //set pins as outputs
  pinMode(DO1_igus_set_polozaj, OUTPUT);
  pinMode(DO2_igus_set_polozaj, OUTPUT);
  pinMode(DO3_igus_set_polozaj, OUTPUT);
  pinMode(DO4_igus_set_polozaj, OUTPUT);
  pinMode(DO5_igus_set_polozaj, OUTPUT);
  
  pinMode(DI1_igus_ready, INPUT);
  pinMode(DI2_igus_active, INPUT);
  pinMode(DI3_igus_reference, INPUT);



cli();//stop interrupts
 // http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html

// TIMER 1 for interrupt frequency 4 Hz:
TCCR1A = 0; // set entire TCCR1A register to 0
TCCR1B = 0; // same for TCCR1B
TCNT1  = 0; // initialize counter value to 0
// set compare match register for 4 Hz increments
OCR1A = 62499; // = 16000000 / (64 * 4) - 1 (must be <65536)
// turn on CTC mode
TCCR1B |= (1 << WGM12);
// Set CS12, CS11 and CS10 bits for 64 prescaler
TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);
// enable timer compare interrupt
TIMSK1 |= (1 << OCIE1A);

//set timer4 interrupt at 1Hz
 TCCR4A = 0;// set entire TCCR1A register to 0
 TCCR4B = 0;// same for TCCR1B
 TCNT4  = 0;//initialize counter value to 0
 // set compare match register for 1hz increments
 OCR4A = 15624/1;// = (16*10^6) / (1*1024) - 1 (must be <65536)
 // turn on CTC mode
 TCCR4B |= (1 << WGM12);
 // Set CS12 and CS10 bits for 1024 prescaler
 TCCR4B |= (1 << CS12) | (1 << CS10);  
 // enable timer compare interrupt
 TIMSK4 |= (1 << OCIE4A);




sei();//allow interrupts
Serial.begin(9600);
delay(1000);
//###########################################################################################
// kontrola zacetnih vrednosti igusa, start, enable, reset morajo biti na off !, drugace logika zavese ne bo delovala 
// preverjam zacetno stanje,mora biti low 

    pinMode(DO6_igus_start, INPUT);
    pinMode(DO7_igus_enable, INPUT);

    // kontrola zacetnih vrednosti, oboje mora biti na low
    kontrola_zacetnih_nastavitev:
    //Serial.print  ("kontrola zacetnih nastavitev");
    //Serial.print (digitalRead(DO6_igus_start));
    if(digitalRead(DO6_igus_start) or digitalRead(DO7_igus_enable))
    {
      var_program_stanje=0;
      error_code="start,enable nista LOW!";
      goto kontrola_zacetnih_nastavitev;
    }
    error_code="";
    Serial.print ("#####");
    // stanje ok, nadaljuj
    pinMode(DO6_igus_start, OUTPUT);
    pinMode(DO7_igus_enable, OUTPUT);
//###########################################################################################
 delay(500);



}

ISR(TIMER1_COMPA_vect){//timer0 interrupt
 //digitalWrite(12,toggle_start_LED);
 //toggle_start_LED = !toggle_start_LED;

 //stanje
   switch (var_program_stanje) {
    case 0:  // napaka iGUS ni pripravljen
      serial_txt="IGUS start error:";
      
      //digitalWrite(DO_start_LED,toggle_start_LED);
      //digitalWrite(DO_home_LED,toggle_home_LED);
      toggle_start_LED = !toggle_start_LED;
      toggle_home_LED = !toggle_home_LED;

      break;
    case 1:  // IGUS nima reference pripravljen,  cakam komando
      serial_txt="IGUS ni reference:";   // izmenicno utripanje
      toggle_start_LED =!toggle_start_LED;
      toggle_home_LED = !toggle_start_LED;
      break;

    case 2:  // IGUS pripravljen,  cakam komando
      serial_txt="IGUS ready:";
      toggle_start_LED = LOW;
      toggle_home_LED = LOW;
      break;
    case 3:  // komanda pomik naprej, ni pogojev
      serial_txt="komanda naprej, ni pogojev:";
      toggle_start_LED = !toggle_start_LED;
      toggle_home_LED = LOW;
      break;
    case 4:  // komanda pomik naprej
      serial_txt="komanda naprej:";
      toggle_start_LED = HIGH;
      toggle_home_LED = LOW;
      break;
    case 5:  // komanda pomik home, ni pogojev
      serial_txt="komanda home, ni pogojev:";
      toggle_start_LED = LOW;
      toggle_home_LED = !toggle_home_LED;
      break;

    case 6:  // komanda pomik naprej, ni pogojev
      serial_txt="komanda home:";
      toggle_start_LED = LOW;
      toggle_home_LED = HIGH;
      break;  
  } 
   Serial.print(var_program_stanje);
    Serial.print(":");
   Serial.print(serial_txt);
   Serial.print(error_code);
    digitalWrite(DO_start_LED,toggle_start_LED);
    digitalWrite(DO_home_LED,toggle_home_LED);
 // Serial.println();      //change line
}


//ref
ISR(TIMER4_COMPA_vect){//timer1 interrupt 1Hz toggles pin 13 (LED)
//generates pulse wave of frequency 1Hz/2 = 0.5kHz (takes two cycles for full wave- toggle high then toggle low)

 //digitalWrite(13,toggle4);
 //toggle4 = !toggle4;

}

void loop() {
  // put your main code here, to run repeatedly:
 //preverjam referenco
 if(!digitalRead(DI3_igus_reference)){
    var_program_stanje=1;
    digitalWrite(DO7_igus_enable, LOW);}
    else{
    var_program_stanje=2;}
}