Position control of a DC motor using Arduino micro. DC motor type: Faulhaber - Series 2607 ... SR IE2-16. Motor driver: L293DNE

position_ctr.ino

#include <Encoder.h>
#define ENCODER_USE_INTERRUPTS

int driverEN =  11;    //Enable input to the driver
int motorPWM1 =  8;    //PWM signal to the motor pin of the driver
int motorPWM2 = 9;
Encoder myenc(3,7);

void setup(){
  Serial.begin(9600);
  pinMode(driverEN, OUTPUT);  pinMode(motorPWM1, OUTPUT);  pinMode(motorPWM2,OUTPUT);
}
float ton; float toff; float theta = 0; int ref_n = 0; float ref;
float errPos; float pasterrPos = 0; float pastuc = 0; float Tsamp = 0.001; float uc1; float uc;

boolean dir1 = true; boolean dir2 = false; 
int PWMfreq = 25000; 

volatile long newPosition = 0; float DutyCycle = 0; int n = 0; float pi = 3.14159265359; float revs_DutyCycle;
int Vnom = 5; float ref_theta = 3.14;

void loop(){   
  if (ref_theta < 0) revs_DutyCycle = -0.01;
  else revs_DutyCycle = 0.01;
  if (n < ref_n)      runmotor(revs_DutyCycle);
  else                runmotor(DutyCycle);
    encoderstuff();
    theta = 2*pi*newPosition/64;
    ref = ref_theta;
    uc = controller(ref,theta);
    DutyCycle = (uc)/Vnom;
    Serial.println(theta);  Serial.print("\t"); 
    Serial.print(n);  Serial.print("\t"); 
    Serial.print(ref);  Serial.print("\t"); 
  
}

float runmotor(float D){
  if (D>=0){    dir1 = true;     dir2 = false;  }
  else {    dir1 = false;    dir2 = true;  }
  ton = abs(D)*PWMfreq;    toff = (PWMfreq-ton);
  digitalWrite(driverEN, HIGH);
  digitalWrite(motorPWM1,dir1);  digitalWrite(motorPWM2,dir2);
  delayMicroseconds(ton);
  digitalWrite(motorPWM1,LOW);  digitalWrite(motorPWM2,LOW);
  delayMicroseconds(toff);
}

void encoderstuff() {
  newPosition = myenc.read();
    if (newPosition <= -64 || newPosition >= 64){
    myenc.write(0);
    n = n+1;
  }   
}

float controller(float ref,float theta){
  float ctroutput; int kp = 1; int ki = 0.9; static float pastE = 0; static float pastU = 0;

  errPos = ref - theta;
  ctroutput = (2*pastU-(2*kp+Tsamp*ki)*pastE-(Tsamp*ki-2*kp)*errPos)/2;
  ctroutput = constrain(ctroutput, -Vnom, Vnom);
  pastE = errPos;
  pastU = ctroutput;
  return ctroutput;
}