PID DC motor position by use Absolute encoder.

I have project to control motor position by use PID controller and have absolute encoder to feedback.
when i run my code. it not move to degree i want to move but just move back and forword between degree i want. i dont know wut i miss or mistake in side my code. Thank for helping :smiley:

Equipment

-Arduino mega 2560
-Absolut Encoder 12 bit
-Dc motor
-Drive motor

if you read and think it strange. i sorry I not strong in English language. :sweat_smile:

#include <PID_v1.h>//control position motor by serial monitor,feedback by ancoder

//Abosolute Encoder 12 Bit 4096 

int CLK = 13; // Blu  Pin 10
int DO = 12; // Grn   Pin 11
int CSn = 11; // Ylw  Pin 12

// Drive motor 

const int pwm = 2 ;//initializing pin 2 as pwm  orange
const int in_1 = 8 ;//turn right pin 8 gray
const int in_2 = 9 ;//turn left pin 9 green

//set PID 

double kp = 2, ki = 0, kd = 0;
double input = 0, output = 0, setpoint = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
int Degree = 0;

void setup() {
  // put your setup code here, to run once:  
  Serial.begin(9600);//text for start
  pinMode(CSn, OUTPUT);// Chip select
  pinMode(CLK, OUTPUT);// Serial clock
  pinMode(DO, INPUT_PULLUP);// Serial data IN/OUT
  pinMode(pwm,OUTPUT) ;   //we have to set PWM pin as output
  pinMode(in_1,OUTPUT) ;  //Logic pins are also set as output
  pinMode(in_2,OUTPUT) ;
  digitalWrite(CSn, HIGH);
  digitalWrite(CLK, HIGH);
  TCCR1B = TCCR1B & 0b11111000 | 1; 
  myPID.SetMode(AUTOMATIC); //Autoset PID
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-100, 100);  // max speed value can change later
  digitalWrite(in_1,HIGH) ;
  digitalWrite(in_2,HIGH) ;
  Serial.println();

  while (! Serial); 
  Serial.println("In put your Angle");
}

void loop() {
  
if (Serial.available())
{ 
      int Angle = Serial.parseInt();
      setpoint = Angle;
      
    
 }
    ReadEncoder(); 
    
    input = Degree;
    myPID.Compute(); 
     Serial.println(Degree); Serial.println(","); Serial.println(output);
     delay(100);
    pwmOut(output);

}


void pwmOut(int out) {
  if (out > 0) {
     digitalWrite(in_1,HIGH);//turn right
     digitalWrite(in_2,LOW) ; 
     analogWrite(pwm,out) ;
  }
  else {
       digitalWrite(in_1,LOW) ;//turn left
       digitalWrite(in_2,HIGH) ;   
  }    analogWrite(pwm,out) ;
}



void ReadEncoder() 
{  int Data = ReadSSI();
   Degree = map(Data, 0, 4095, 0, 359); //cange data to Degree
} 
int ReadSSI(void)//read clk cs do from encoder
{ int i, dReading;
  char Resolution = 12;
  unsigned int bitStart = 0x0800;
  dReading = 0;
  digitalWrite(CSn, LOW);
  delayMicroseconds(5);
  digitalWrite(CLK, LOW);
  for (i = (Resolution - 1); i >= 0; i--)
  { digitalWrite(CLK, HIGH);
    delayMicroseconds(5);
    if (digitalRead(DO)) dReading |= bitStart;
    digitalWrite(CLK, LOW);
    delayMicroseconds(5);
    bitStart = bitStart >> 1;
    if (i == 0)
    { digitalWrite(CLK, HIGH);
      if (digitalRead(DO)) dReading |= bitStart;
    }
  }
  digitalWrite(CSn, HIGH);
  return dReading;
}
else {
       digitalWrite(in_1,LOW) ;//turn left
       digitalWrite(in_2,HIGH) ;   
  }    analogWrite(pwm,out) ;  //  is that } in the right place?

If you use autoformat (ctri-t or Tools, Auto Format) the program flow will be clearer.

groundFungus:

else {

digitalWrite(in_1,LOW) ;//turn left
      digitalWrite(in_2,HIGH) ;  
 }    analogWrite(pwm,out) ;  //  is that } in the right place?




If you use autoformat (ctri-t or Tools, Auto Format) the program flow will be clearer.

Thank I just write new one it can change to angle I want but would like to put PID but I don t know where to put it

#include <PID_v1.h>

//Abosolute Encoder 12 Bit 4096 
int CLK = 13; // Blu  Pin 10
int DO = 12; // Grn   Pin 11
int CSn = 11; // Ylw  Pin 12

const int pwm = 2 ;  //initializing pin 2 as pwm
const int in_1 = 8 ;
const int in_2 = 9 ;

double kp = 20, ki = 0, kd = 0;
double pid_p = 0, pid_i = 0, pid_d = 0;
double E  = 0,sp = 180,lastE =0,pv = 0;
double elapsedTime, Time, timePrev;
int Degree = 0;
int now = 0;
int PID_value = 0;

void setup() {

  pinMode(pwm,OUTPUT) ;   //we have to set PWM pin as output
  pinMode(in_1,OUTPUT) ;  //Logic pins are also set as output
  pinMode(in_2,OUTPUT) ;

   TCCR1B = TCCR1B & 0b11111000 | 1; 
  // put your setup code here, to run once:  
  Serial.begin(9600);//text for start
  pinMode(CSn, OUTPUT);// Chip select
  pinMode(CLK, OUTPUT);// Serial clock
  pinMode(DO, INPUT_PULLUP);// Serial data IN/OUT
  digitalWrite(CSn, HIGH);
  digitalWrite(CLK, HIGH);

    Time = millis(); 
 
 
 
}

void loop() {
  // put your main code here, to run repeatedly:
      

       ReadEncoder(); 
        if (Serial.available())
{ 
      int Angle = Serial.parseInt();
     
                   sp = 180+Angle;     
                 

    
 }

       
       pv = Degree;
       E = sp - pv;
       lastE = E;
       
       pid_p = kp*E;
       pid_i = ki*E;
    

       timePrev = Time;  
       Time = millis();                     
       elapsedTime = (Time - timePrev)/ 1000; 
  
        pid_d = kd*(E/elapsedTime);
        
          now = Degree - 180;
         PID_value =  pid_p + pid_i + pid_d;
        Serial.print( PID_value);   Serial.print(" ");   Serial.print(E);   Serial.print(" ");  Serial.println(now);

 
          if(E>0)      { if(E  = 0){
                                      digitalWrite(in_1,HIGH) ;digitalWrite(in_2,HIGH) ;
                                      analogWrite(pwm,100);
                                   }
                         else      {
                                      digitalWrite(in_1,HIGH) ;digitalWrite(in_2,LOW) ;
                                      analogWrite(pwm,30) ;
                                     
                                   }
                       }
                     
          else if(E<0) { if(E  = 0){
                                       digitalWrite(in_1,HIGH) ;digitalWrite(in_2,HIGH) ;
                                       analogWrite(pwm,100) ;
                                    }
                          else      {
                                       digitalWrite(in_1,LOW) ;digitalWrite(in_2,HIGH) ;
                                       analogWrite(pwm,30);
                                       
                                     }
                        }
          else          {    
                                        digitalWrite(in_1,HIGH) ;digitalWrite(in_2,HIGH) ;
                                        analogWrite(pwm,100) ;
                         }   
                                         lastE = E;
                                         
                                        

                     }
       
       








void ReadEncoder() 
{  int Data = ReadSSI();
   Degree = map(Data, 0, 4095, 0, 359); //cange data to Degree
   
} 
int ReadSSI(void)//read clk cs do from encoder
{ int i, dReading;
  char Resolution = 12;
  unsigned int bitStart = 0x0800;
  dReading = 0;
  digitalWrite(CSn, LOW);
  delayMicroseconds(5);
  digitalWrite(CLK, LOW);
  for (i = (Resolution - 1); i >= 0; i--)
  { digitalWrite(CLK, HIGH);
    delayMicroseconds(5);
    if (digitalRead(DO)) dReading |= bitStart;
    digitalWrite(CLK, LOW);
    delayMicroseconds(5);
    bitStart = bitStart >> 1;
    if (i == 0)
    { digitalWrite(CLK, HIGH);
      if (digitalRead(DO)) dReading |= bitStart;
    }
  }
  digitalWrite(CSn, HIGH);
  return dReading;
}