////////////////////////////////////////////////////////////////////////////
unsigned long cycleTime;
unsigned int cycleCount;
////////////////////////////////////////////////////////////////////////////

/*-----------------*/
/* pin declaration */
/*-----------------*/
//const int ???_pin = 0; //////// wenn ENABLE an darf VDD nicht getoggelt werden --> vllt programmatisch mit extra taster implementieren??
//const int tb_enable_pin = 13; // test bench enable pin
const int M1_sig0_pin = 2;
const int M1_sig1_pin = 3;
const int M2_sig0_pin = 4;
const int M2_sig1_pin = 9; //5
const int D1_ENABLE_pin = 6;
const int D1_IN1_pin = 7;
const int D1_IN2_pin = 8;
//const int D1_FAULT_pin = 10; //9
const int D2_ENABLE_pin = 5; //10
const int D2_IN1_pin = 11;
const int D2_IN2_pin = 12;
//const int D2_FAULT_pin = 13;
const int limit1_pin = 18;
const int limit2_pin = 19;


[...]


//////////////////////////////////////////////////////////////////////////////////////////////
  cycleCount++;
  if (cycleCount >= 20000)
  {
    Serial.print("Cycle Time: ");
    Serial.print((micros()-cycleTime)/cycleCount);
    Serial.println(" microseconds");
    cycleCount = 0;
    cycleTime = micros();
    
    Serial.print(" Fsoll: ");
    Serial.print(force_set);
    Serial.print(" Fist1: ");
    Serial.print(force1_real);
    Serial.print(" Fist2: ");
    Serial.print(force2_real);
    Serial.print(" PWM-vrgb: ");
    Serial.print(duty_cycle_set);
    Serial.print(" ctrl_PWM1: ");
    Serial.print(duty_cycle_controlled1);
    Serial.print(" ctrl_PWM2: ");
    Serial.print(duty_cycle_controlled2);
    Serial.print(" c_helpM1: ");
    Serial.print(control_helpM1);
    Serial.print(" c_helpM2: ");
    Serial.print(control_helpM2);
    Serial.print(" c_M1: ");
    Serial.print(control_Motor1);
    Serial.print(" c_M2: ");
    Serial.print(control_Motor2);
    
    Serial.print(" ***M1*** ");
    Serial.print(" IN1: ");
    Serial.print(digitalRead(D1_IN1_pin));
    Serial.print(" IN2: ");
    Serial.print(digitalRead(D1_IN2_pin));
    Serial.print(" ENABLE: ");
    Serial.print(digitalRead(D1_ENABLE_pin));
    Serial.print(" ***M2*** ");
    Serial.print(" IN1: ");
    Serial.print(digitalRead(D2_IN1_pin));
    Serial.print(" IN2: ");
    Serial.print(digitalRead(D2_IN2_pin));
    Serial.print(" ENABLE: ");
    Serial.print(digitalRead(D2_ENABLE_pin));
    Serial.print(" ausg: ");
    Serial.print(duty_cycle_set / limit_factor);
    
    Serial.println("");
  }
  if((cycleCount == 10000) || (cycleCount == 15000) || (cycleCount == 5000))
  {
    Serial.print(" Fsoll: ");
    Serial.print(force_set);
    Serial.print(" Fist1: ");
    Serial.print(force1_real);
    Serial.print(" Fist2: ");
    Serial.print(force2_real);
    Serial.print(" PWM-vrgb: ");
    Serial.print(duty_cycle_set);
    Serial.print(" ctrl_PWM1: ");
    Serial.print(duty_cycle_controlled1);
    Serial.print(" ctrl_PWM2: ");
    Serial.print(duty_cycle_controlled2);
    Serial.print(" c_helpM1: ");
    Serial.print(control_helpM1);
    Serial.print(" c_helpM2: ");
    Serial.print(control_helpM2);
    Serial.print(" c_M1: ");
    Serial.print(control_Motor1);
    Serial.print(" c_M2: ");
    Serial.print(control_Motor2);
    
    Serial.print(" ***M1*** ");
    Serial.print(" IN1: ");
    Serial.print(digitalRead(D1_IN1_pin));
    Serial.print(" IN2: ");
    Serial.print(digitalRead(D1_IN2_pin));
    Serial.print(" ENABLE: ");
    Serial.print(digitalRead(D1_ENABLE_pin));
    Serial.print(" ***M2*** ");
    Serial.print(" IN1: ");
    Serial.print(digitalRead(D2_IN1_pin));
    Serial.print(" IN2: ");
    Serial.print(digitalRead(D2_IN2_pin));
    Serial.print(" ENABLE: ");
    Serial.print(digitalRead(D2_ENABLE_pin));
    Serial.print(" ausg: ");
    Serial.print(duty_cycle_set / limit_factor);

    
    
    Serial.println("");
  }
//////////////////////////////////////////////////////////////////////////////////////////////
}

/*-------------------*/
/* control functions */
/*-------------------*/
void control_motor1 (int x1, int w)
{
  e1 = w - x1;
  if((y1 < 1023) && (y1 > -1023)) esum1 = esum1 + e1;
  y1 = (Kp1*e1)+(Ki1*Ta*esum1)+(Kd1*(e1-ealt1)/Ta);
  ealt1 = e1;
  if(y1 > 1023) y1 = 1023;
  else if(y1 < -1023) y1 = -1023;
  duty_cycle_controlled1 = y1;
}

void control_motor2 (int x2, int w)
{
  e2 = w - x2;
  if((y2 < 1023) && (y2 > -1023)) esum2 = esum2 + e2;
  y2 = (Kp2*e2)+(Ki2*Ta*esum2)+(Kd2*(e2-ealt2)/Ta);
  ealt2 = e2;
  if(y2 > 1023) y2 = 1023;
  else if(y2 < -1023) y2 = -1023;
  duty_cycle_controlled2 = y2;
}