controlling N20 Micro gear motor with encoder using arduino

#include <PID_V1.h>

#define MotEnable  6

#define MotFwd 3

#define MotorRev 4

String readString; // store  the user_Input data

int user_Input = 0;

int Encoder_pin_1 = 2,   Encoder_pin_2 =3;

volatile int lastEncodered = 0; // Here updated value of encoder store

volatile  long  Encodervalue =0; //Raw encoder value

 

int PPR= 1600;  //encoder pulse per revolution

int Angle = 360;  //maximum degree of  motion

int REV =0;  //setpoint  reduired encoder value

int lastMSB=0;

int lastLSB=0;

double Kp=5,  Ki==1,  Kd=0.01;

double  input=0,  output=0,  setpoint=0;

PID myPID( &input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

 

void setup( ) {

pinMode(MotorFwd, OUTPUT);

pinMode(MotorRev, OUTPUT);

pinMode(MotEnable, OUTPUT);

Serial.begin(9600);

 

pinMode(Encoder_pin_1,  INPUT_PULLUP);

pinMode(Encoder_pin_2,  INPUT_PULLUP);

attachInterrupt(0,updateEncoder, CHANGE);

attachInterrupt(1,updateEncoder, CHANGE);

 

TCCR1B = TCCR1B  & ob11111001  | 1; //set 31khz  PWm to prevent motor noise

myPID.SetMode(AUTOMATIC);

myPID.SetSampleTime(1); //refresh  rate of pid controller

myPID.SetOutputLimit(-125, 125); // this is max PWM value to move motor 

                                            //here change in the value reflect in speed of motor

}

 

void loop(  ) {

while(Serial.avaiable() )

{delay(3);  char c = Serial.read( ) ;  readString += c;}

if(readString.length( ) > 0)

{Serial.println(readString.toInt( ));  User_Input = readString.toInt();}

REV= map(User_Input, 0 ,360, 0, 1600);    setpoint = REV;

Input = Encodervalue;

myPID.Compute( );  pwmout(  output );

}

 

 

void pwmout( int out)

{  if(out>0)  { analogWrite( MotEnable ,out);  forward(  );}

   else{  analogWrite( MotEnable ,  abs(out));  reverse(  );}

readString = "  "; //clean user_Input data   read for new Input

}

 

 

void forward( )

{ digitalWrite(MotorFwd,HIGH);  digitalWrite(MotoRev,LOW);

Serial.print("Forward");  Serial.println(Encodervalue);

}

 

void reverse(  )

{ digitalWrite(MotorFwd,LOW);  digitalWrite(MotoRev,HIGH);

Serial.print("Reverse");  Serial.println(Encodervalue);

}

 

void finish(  )

{ digitalWrite(MotorFwd,LOW);  digitalWrite(MotoRev,LOW);

Serial.print("Stop motor");  Serial.println(Encodervalue);

}

 

void updateEncoder( ) {

int MSB=digitalRead(Encoder_pin_1),  LSB=digitalRead(Encoder_pin_2);

int Encoded =(MSB<<1 ) | LSB;  //converting the 2 pin value to single number

int sum=(lastEncoded << 2) | Encoded;  //adding it to the previous encoded value

if(sum == ob1101  || sum == ob0100 || sum == ob0010 || sum == ob1011)

Encodervalue--;

if(sum == ob1110  || sum == ob0111 || sum == ob0001 || sum == ob1000)

Encodervalue ++;

lastEncoded = Encoded;

}