EDPI10 ชุดเรียนรู้+ชุดทดลอง เพนดูลัมผกผันพีไอดี Arduino PID Controled Inverted Pendulum

///////// วงจรขับ TB6612 drive pin ////
#define PWM 9  //ขาส่งสัญญาณ PWM ไปยังวงจรขับ
#define IN2 10 //ขาควบคุมทิศทางการหมุน ไปยังวงจรขับ
#define IN1 11 //ขาควบคุมทิศทางการหมุน ไปยังวงจรขับ

///////// ขาสัญญาณที่อ่านเอ็นโค้ดเดอร์ /////////
#define ENCODER_A 2// Motor encoder pin เฟส A ที่ขา 2
#define ENCODER_B 4// Motor encoder pin เฟส B ที่ขา 4

///////// ขาสัญญาณจากโพเทนชิโอมิเตอร์ที่ขา Analog /////////
#define Analogin 1

#include <PinChangeInt.h>     // ไลเบอรี่สำหรับใช้งานอินเตอร์รัปที่ขาดิจิตอลอินพุต ในที่นี้ใช้สำหรับอ่านพัลส์เอ็นโค้ดเดอร์
#include <MsTimer2.h>         // ไลเบอรี่สำหรับใช้งานไทเมอร์
int Setpoint = 512;           // ค่าตรงกลางของตัวโพเทนชิโอมิเตอร์ ที่ทำให้แกนตั้งฉากกับพื้น

float Position = 1000;         // ตัวแปร Position ค่าจะวิ่งอยู่ในช่วง +/- 500
float Sensor;                  // Angle displacement sensor parameters
float Motor_PWM;               // Motor PWM parameters อยู่ในชว่ง -255 ถึง +255
float Balance_Pwm;             // Pendulum uplight loop PWM parameters ตัวแปรเอาต์พุตสัญญาณ PWM ของการควบคุมแกนเพนดูลัม
float Position_Pwm;            // Position loop PWM parameters
int Target_Position = 1000;    // Target value
static float Position_Int;
int EnablePendulum = 0;    // Start/Stop control pendulum

float Balance_KP = 300; // Balance Gain
float Balance_KI = 0;  // Balance Gain
float Balance_KD = 200;  // Balance Gain

float Position_KP = 100;  // Position Gain
float Position_KI = 0;  // Position Gain
float Position_KD = 100;  // Position Gain  

/**************************************************************************
 Function: PD control
 Entry parameters: Angle
 Return Value: Pendulum uplight Tilt Control PWM
**************************************************************************/
float Balance_Control(float sensor){  
   float Error;                      // ตัวแปรเก็บค่า Error
   static float Last_Error;          // ตัวแปรที่เกี่ยวข้องกับ PD control
   static float D_Error;             // ตัวแปรที่เกี่ยวข้องกับ PD control
   int PD_Output;                    // ตัวแปรเอาต์พุตของ PD control
   
   Error = sensor - Setpoint;        // หาค่า Error
   D_Error = Error - Last_Error;     // หาค่าการเปลี่ยนแปลงชั่วขณะ(เทอม D)
   
   PD_Output = Balance_KP/10 * Error + D_Error * Balance_KD/1000; // PWM PD control for motors that calculate tilt control
   
   Last_Error = Error;                // ทำให้ Errorใหม่ เป็น Error เก่า
   return PD_Output;                  // ส่งค่าเอาต์พุต
}

/**************************************************************************
 Function: Position PD control
 Entry parameters: Encoder
 Return Value: Position control PWM
**************************************************************************/
float Position_Control(int Encoder){  
    static float Position_PWM;          //Define related variables
    static float Last_Position;         //Define related variables
    static float Position_Bias;         //Define related variables
    static float Position_Differential; //Define related variables
    static float Position_Least;        //Define related variables
   
    Position_Least = Target_Position - Encoder;             // Get deviation value
    Position_Bias *= 0.8;                        // first-order low-pass filter
    Position_Bias += Position_Least*0.2;         // first order difference


    Position_Int = Position_Int + Position_Least;
    if(Position_Int > 500)Position_Int = 500;
    if(Position_Int < -500)Position_Int = -500;
   
    Position_Differential = Position_Bias - Last_Position; // Keep the last deviation
   
    Last_Position = Position_Bias;                       // เปลี่ยนค่าล่าสุดเป็นค่าเก่า สำหรับการคำนวณรอบหน้า
   
    Position_PWM = (Position_Bias*Position_KP/1000) + (Position_Int*Position_KI/100) + (Position_Differential*Position_KD/10); // Position control
    return Position_PWM; //Return control
}

/**************************************************************************
 Function: ส่งค่า PWM ออกที่ขา 9 และกำหนดทิศทางการหมุนด้วยขา 10 และ 11
 Entry parameters: PWM
 Return Value: None
**************************************************************************/
void Set_PWM(int motor){
  if (motor< 0)  digitalWrite(IN1, LOW),  digitalWrite(IN2, HIGH);  // ค่าติดลบ
  else           digitalWrite(IN1, HIGH),   digitalWrite(IN2, LOW); // ค่าบวก
  analogWrite(PWM, abs(motor));    // ส่งสัญญาณ PWM ค่ามาก มอเตอร์หมุนเร็ว ค่าน้อย มอเตอร์หมุนช้า
}

/**************************************************************************
 Function: หาค่าเฉลี่ยของค่าที่อ่านได้จากเซนเซอร์ตรวจจับมุมเอียง
 Entry parameters: None
 Return Value: None
**************************************************************************/
 u16 Get_Adc_Average(u8 ch,u8 times){
  unsigned int temp_val=0;
  unsigned char  t;
  for(t=0;t<times;t++){
    temp_val+=analogRead(ch);//อ่านค่าตำแหน่งแกนจาก Adc แล้วบวกในตัวเอง
  }
  return temp_val/times;//ครบแล้วหารด้วยค่า time
}  


/**************************************************************************
 Function: ทำฟังชัน ทุกๆ 5ms control จากไทเมอร์2
 Entry parameters: None
 Return Value: None
**************************************************************************/
void control(){  
      static unsigned char Position_Count;    // Variable for position control frequency division
      sei();                                  // เปิดให้อินเตอร์รัปต์ทำงาน
      Sensor= 1023 - Get_Adc_Average(1,5);    // หาค่าเฉลี่ยของแรงดันที่อ่านได้จาก Adc ต่อเข้าที่ขา A1 หาค่าเฉลี่ย 5 ครั้ง
      Balance_Pwm = Balance_Control(Sensor);  // เรียกฟังก์ชัน PD control เพื่อเลี้ยงแกนให้ตรง
      if(++Position_Count>4)
          {
            Position_Pwm = Position_Control(Position); // Position PD control
            Position_Count = 0;    
          }
      Motor_PWM = Balance_Pwm - Position_Pwm;          // Calculate motor final PWM
     
      if(Motor_PWM>255) Motor_PWM = +255;   //ลิมิตค่า PWM มากสุด, ถ้าค่าเกิน 255 มอเตอร์จะหยุด
      if(Motor_PWM<-255)Motor_PWM = -255;   //ลิมิตค่า PWM น้อยสุด
     
      if(Sensor<(Setpoint-200)||Sensor>(Setpoint+200)) // ถ้ามุมการเอียงมากไปจะส่งให้มอเตอร์หยุดทำงาน
      {    
         Motor_PWM = 0;                                              
         digitalWrite(IN1, LOW);  //TB6612 level control                                        
         digitalWrite(IN2, LOW);  //TB6612 level control        
      }
     
      if(EnablePendulum == 1)
      {
        Set_PWM(Motor_PWM);    //Output motor control
      }
      else
      {
          Set_PWM(0);    //Stop motor control
     }    
 }
/**************************************************************************
 Function: คอนฟิกพอร์ตและกำหนดค่าเริ่มต้นต่างๆ
 Entry parameters: None
 Return Value: None
**************************************************************************/
voidsetup()   {
  int fff = 1;
  TCCR1B =(TCCR1B & 0xF8) | fff;
   
   pinMode(IN1, OUTPUT);            //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต
   pinMode(IN2, OUTPUT);            //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต
   pinMode(PWM, OUTPUT);            //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต
   digitalWrite(IN1, 0);            //ส่งลอจิก 0
   digitalWrite(IN2, 0);            //ส่งลอจิก 0
   digitalWrite(PWM, 0);            //ส่งลอจิก 0
   pinMode(ENCODER_A, INPUT);           // กำหนดขาอ่านสัญญาณเอ็นโค้ดเดอร์ให้เป็น อินพุต
   pinMode(ENCODER_B, INPUT);           // กำหนดขาอ่านสัญญาณเอ็นโค้ดเดอร์ให้เป็น อินพุต
   Serial.begin(115200);                // กำหนดบอตเรตการสื่อสาร serial เพื่อส่งข้อมูลไปยังคอมพิวเตอร์
   delay(200);                          // หน่วงเวลาเล็กน้อยเพื่อให้แน่ใจว่าการตั้งค่าต่างๆ เสร็จเรียบร้อยแล้ว
   MsTimer2::set(5, control);           // Use Timer 2 to set 5ms timer interrupt
   MsTimer2::start();                   // เปิดการทำงานไทเมอร์
  attachInterrupt(0, READ_ENCODER_A, CHANGE);           //เปิดใช้งานอินเตอร์รัปต์สำหรับอ่านค่าเอ็นโค้ดเดอร์ ทุกครั้งที่มีการเปลี่ยนแปลง จาก 0 เป็น 1 หรือ จาก 1 เป็น 0
}

/**************************************************************************
 Function function: ส่งค่าไปที่ serial port เพื่อพล็อตกราฟ
 Entry parameters: None
 Return Value: None
**************************************************************************/
void loop(){ // แสดงผลกราฟ
 Serial.print("\n");  
 //1
 Serial.print(Motor_PWM);
 Serial.print("\t");
 //2
 Serial.print(Sensor);  
 Serial.print("\t");
 //3
 Serial.print(Setpoint);
 Serial.print("\t");
 //4
 Serial.print(Position);
 Serial.print("\t");
 //5
 Serial.print(Target_Position);
 Serial.print("\t");
 //6
 Serial.print(Balance_KP);
 Serial.print("\t");
 //7
 Serial.print(Position_KP);
 Serial.print("\t");
 //8
 Serial.print(Position_KI);
 Serial.print("\t");
 //9
 Serial.print(Position_KD);
 Serial.print("\t");
 //10
 Serial.print(Balance_KI);
 Serial.print("\t");
 //11
 Serial.print(Balance_KD);
 Serial.print("\t");
 //delay(5);

 switch (Serial.read()) {
//************ Balance ****************        
    case 'c':  
      Setpoint = Setpoint +1;
      break;
    case 'd':    
      Setpoint = Setpoint -1;
      break;
    case 'a':    
      Balance_KP = Balance_KP+1;
      break;
    case 'b':    
      Balance_KP = Balance_KP-1;
      break;
    case 'q':  
      Balance_KI = Balance_KI+1;
      break;
    case 'r':    
      Balance_KI = Balance_KI-1;
      break;
    case 's':  
      Balance_KD = Balance_KD+1;
      break;
    case 't':    
      Balance_KD = Balance_KD-1;
      break;      
       
//************ Position **************  
    case 'e':  
      Target_Position = Target_Position +1;
      break;
    case 'f':    
      Target_Position = Target_Position -1;
      break;  
    case 'g':  
      Position_KP = Position_KP +1;
      break;
    case 'h':    
      Position_KP = Position_KP -1;
      break;  
    case 'i':  
      Position_KI = Position_KI +1;
      break;
    case 'j':    
      Position_KI = Position_KI -1;
      break;  
    case 'k':  
      Position_KD = Position_KD +1;
      break;
    case 'l':    
      Position_KD = Position_KD -1;
      break;
     
//*************** Additional *****************  
    case 'x':  
      EnablePendulum = 1;
      break;
     
    case 'y':    
      EnablePendulum = 0;
      break;  
       
    case 'u':    
      Position = 1000;
      break;  
 }
}

/**************************************************************************
 Function: อ่านสัญญาณเอ็นโค้ดเดอร์ตตามการอินเตอร์รัฟต์  เป็นกรณีที่สัญญาณเฟส A นำเฟส B        
 Entry parameters: None
 Return Value: None
**************************************************************************/
voidREAD_ENCODER_A(){
    if(digitalRead(ENCODER_A) == HIGH)
    {   //เกิดอินเตอร์รัปต์ที่ขอบขาลง  
      if (digitalRead(ENCODER_B) == LOW)      Position--;  //เพิ่มค่า ตัวแปร Position
      else      Position++;
    }
    else
    {  //เกิดอินเตอร์รัปต์ที่ขอบขาขึ้น  
      if (digitalRead(ENCODER_B) == LOW)      Position++; //ลดค่า ตัวแปร Position
      else     Position--;
    }
}

/**************************************************************************
 Function: อ่านสัญญาณเอ็นโค้ดเดอร์ตตามการอินเตอร์รัฟต์  เป็นกรณีที่สัญญาณเฟส B นำเฟส A                    
 Entry parameters: None
 Return Value: None
**************************************************************************/
voidREAD_ENCODER_B(){
    if(digitalRead(ENCODER_B) == LOW)
    {//เกิดอินเตอร์รัปต์ที่ขอบขาลง
      if (digitalRead(ENCODER_A) == LOW)      Position--;//เพิ่มค่า ตัวแปร Position
      else      Position++;
    }
  else
    { //เกิดอินเตอร์รัปต์ที่ขอบขาขึ้น  
      if (digitalRead(ENCODER_A) == LOW)      Position++; //ลดค่า ตัวแปร Position
      else     Position--;
    }
}