Position_of_motor

/*Code for position control of motor*/


#define in1              5                             //D5
#define in2              4                             //D4
#define pwm_pin          6                             //D6 
#define md_vcc           12                            //5v supply to motor driver
#define interrupt0       2                             //D2 for Channel A
#define channel_b        3                             //D3

#include <avr/io.h>
#include <avr/interrupt.h>

void Ext_interrupt_Init();
void anti_clockwise();
void clockwise();
void stop_motion();

volatile int counter = 0;                              //counter will count pulses obtained from encoder
int counter_angle = 0;
int angle = 0;                                         //angle variable will store the reading of Robot Arm angle
int set_angle = 180;                                     //assign the required angle to this variable
uint8_t pwm = 0;                                       //assign the base pwm value (initial value) to this variable
float Kp = 1.4160;                                     //proportional constant max can be 1.416 i.e 255/180. 1.0055 is good. 1.4160, 1.8000 is also fine. 2.5000 is faster and has possibility of overshoot.
float Ki = 0.06789;                                    //integral constant
int error = 0;
int integral = 0;                                      //integral will store the area of error curve used to reduce the steady state error (offset)
int correction = 0;
int accuracy = 2;
int check = 0;                                         //used once in program to gradually increase pwm of motor from 0 to required (Kp*error)
                                         
void setup()
{
  pinMode(md_vcc,OUTPUT);                              //md_vcc pin is for 5v supply to motor driver 
  digitalWrite(md_vcc,HIGH);                           //5v VCC to L298 Motor driver given from pin 12
  
  pinMode(in1,OUTPUT);                                 //In1 pin of L298 Motor Driver
  pinMode(in2,OUTPUT);                                 //In2 pin of L298 Motor Driver
  pinMode(pwm_pin,OUTPUT);                             //pwm pin of cytron

//  DDRD&=(0<<PD2)&(0<<PD3);                           //PD2(D2)(used as INT0) for Channel A and PD3(D3)(used as digital) for Channel B of encoder
//  PORTD&=(0<<PD2)&(0<<PD3);                          //pull down INT0 and Channel B pin to read exact pulses

  pinMode(interrupt0,INPUT);                           //External INT0 pin declared as input for Channel A
  pinMode(channel_b,INPUT);                            //D3 declared as input for Channel B
  digitalWrite(interrupt0,HIGH);
  digitalWrite(channel_b,HIGH);
  
  sei();                                               //global interrupt enabled
  Ext_interrupt_Init();                                //initialising External INT0 to trigger flag at rising edge
  Serial.begin(9600);
}
  
void loop()
{
    counter_angle = counter;
    angle = counter_angle * 3/5;                        //angle is counter * 360 / 600
    if (angle > 360)                                    //angle value must be between 0 to 360. Hence if angle is greater than 360, it is set to 360
    angle = 360;
    else if (angle < 0)                                 //if angle is calculated less than 0, it is assigned 0
    angle = 0;
    Serial.print("\nangle = ");
    Serial.print(angle);
    Serial.print("   error = ");
    Serial.print(error);
    Serial.print("   correction = ");
    Serial.print(correction);
    
    if ( check == 0 )
    {
      if (angle < set_angle - accuracy)                   //method to define direction of motion of robotic arm, clockwise or anticlockwise
      {
        error = set_angle - angle;
        if (error > 180)
        {
          error = 360 - error;
          anti_clockwise();
          correction = pwm + Kp*error;
          if ( correction < 30 )
          correction = 30;
          else if ( correction > 255 )
          correction = 255;
          analogWrite(pwm_pin , correction);            //proportional control method used. Kp is proportional constant.
        }
        
        else if (error < 180)
        {
          clockwise();
          correction = pwm + Kp*error;
          if ( correction < 30 )
          correction = 30;
          else if ( correction > 255 )
          correction = 255;
          analogWrite(pwm_pin , correction);            //proportional control method used. Kp is proportional constant.
        }
      }
      else if (angle > set_angle + accuracy)
      {
        error = angle - set_angle;
        if (error > 180)
        {
          error = 360 - error;
          clockwise(); 
          correction = pwm + Kp*error;
          if ( correction < 30 )
          correction = 30;
          else if ( correction > 255 )
          correction = 255;
          analogWrite(pwm_pin , correction);            //proportional control method used. Kp is proportional constant.
        }
        else if (error < 180)
        {
          anti_clockwise(); 
          correction = pwm + Kp*error;
          if ( correction < 30 )
          correction = 30;
          else if ( correction > 255 )
          correction = 255;
          analogWrite(pwm_pin , correction);            //proportional control method used. Kp is proportional constant.
        }
      }
  //    else if ((set_angle - accuracy < angle) && (angle < set_angle + accuracy))
  //    {
  //      if (set_angle > angle)
  //      {
  //        error = set_angle - angle;
  //        integral = integral + error;
  //        anti_clockwise();
  //        correction = pwm + Ki*integral;
  //        if ( correction < 26 )
  //        correction = 26;
  //        else if ( correction > 255 )
  //        correction = 255;
  //        analogWrite(pwm_pin , correction);            //proportional control method used. Kp is proportional constant.
  //      }
  //      else if (set_angle < angle)
  //      {
  //        error = angle - set_angle;
  //        integral = integral + error;
  //        clockwise();
  //        correction = pwm + Ki*integral;
  //        if ( correction < 26 )
  //        correction = 26;
  //        else if ( correction > 255 )
  //        correction = 255;
  //        analogWrite(pwm_pin , correction);            //proportional control method used. Kp is proportional constant.
  //      }
        else if ((set_angle - accuracy < angle) && (angle < set_angle + accuracy))
        {
          stop_motion();
        }
    }
}

void clockwise()
{
  digitalWrite(in1,HIGH);
  digitalWrite(in2,LOW);
//  if (check==0)
//  {
//      for (int i = 0 ; i < Kp*error ; i+=2)
//    {
//      analogWrite(pwm_pin,i);
//      delay(2);
//    }
//    check++;
//  }  
}

void anti_clockwise()
{
  digitalWrite(in1,LOW);
  digitalWrite(in2,HIGH);
//  if (check==0)
//  {
//    for (int i = 0 ; i < Kp*error ; i+=2)
//    {
//      analogWrite(pwm_pin,i);
//      delay(2);
//    }
//    check++;
//  }
}

void stop_motion()
{
  integral = 0;
  error = 0;
  angle = set_angle;
  correction = 0;
  check++;
  digitalWrite(in1,LOW);
  digitalWrite(in2,LOW);
  analogWrite(pwm_pin,correction);
}

void Ext_interrupt_Init()
{
  EICRA|=(1<<ISC01)|(1<<ISC00);                        //Interrupt0 will trigger at rising edge.
  EIMSK|=(1<<INT0);                                    //ext interrupt 0 enabled
  sei();                                               //global interrupt enabled
}

ISR(INT0_vect)
{
  if (digitalRead(channel_b)==LOW)
  {
    counter++;
    if (counter > 600)
    counter = counter - 600;                           //if value of counter becomes more than 600 then it is updated to quantize between 0 to 600
  }
  else if (digitalRead(channel_b)==HIGH)
  {
    counter--;
    if (counter < 0)
    counter = 600 + counter;                           //if value of counter becomes less than 0 then it is updated to quantize between 0 to 600
  }
}