# **直流馬達轉角控制**

https://www.youtube.com/watch?v=vx-HHhzYwpA&ab_channel=%E8%98%87%E6%B3%93%E8%88%89

本實驗需要材料:

- 12V外部供電
- DC直流馬達帶霍爾編碼器
- L298N 馬達驅動板
- Arduino Uno

## 馬達的接腳大致如下

1. 電機正(motor+ ) :直接驅動直流馬達的正極
2. 電機負(motor - ) :直接驅動直流馬達的負極
3. 編碼器正(Encoder VCC) : 接入5V提供給編碼器電源
4. 編碼器負(Encoder GND) : 接入GND
5. A(A相)
6. B(B相)

主要透過AB兩相的高低電位的時間來判斷馬達屬於正轉還是反轉

![A、B相電位時序圖
](https://hackmd.io/_uploads/SJ_W-T7UC.png)


馬達接腳圖


實驗佈線圖

## 首先如圖先接好線，輸入以下程式上傳

#define encoder_1A  2
#define encoder_1B  4
volatile long Pos = 0;//實際值
void setup() {
Serial.begin (115200);
pinMode(encoder_1A, INPUT); 
digitalWrite(encoder_1A, HIGH);       // turn on pull-up resistor
pinMode(encoder_1B, INPUT);
attachInterrupt(digitalPinToInterrupt(2), doEncoder1, CHANGE); 
 // 設定中斷腳位、中斷觸發副程式、中斷觸發類型
}
void loop() {
}
void doEncoder1() {
  Serial.println(Pos);
  if (digitalRead(encoder_1A) == digitalRead(encoder_1B)) {   //高電位 低電位
    Pos++; //角度遞增1
  } else {
    Pos--; //角度遞減1
  }
}

https://youtu.be/gbkC43gLbF4

要注意的是，Arduino UNO不是每個腳位都能做外部中斷，可以看官網資料:


# 外部中斷

外部中斷是[單片機](https://baike.baidu.hk/item/%E5%96%AE%E7%89%87%E6%A9%9F/102396)實時地處理外部事件的一種內部機制。當某種外部事件發生時，單片機的[中斷系統](https://baike.baidu.hk/item/%E4%B8%AD%E6%96%B7%E7%B3%BB%E7%B5%B1/10480702)將迫使CPU暫停正在執行的[程序](https://baike.baidu.hk/item/%E7%A8%8B%E5%BA%8F/13831935)，轉而去進行中斷事件的處理；中斷處理完畢後．又返回被中斷的程序處，繼續執行下去。

# 計時中斷

用時間來觸發中斷，在Arduino UNO中可以使用TimerOne.h這個庫，下載後加入zip程式庫

載點:https://www.arduinolibraries.info/libraries/timer-one

以下程式示範了一個設定一中斷事件，使其定期觸發。

#include <TimerOne.h>
void setup() {
Timer1.initialize(1000);    // 0.001 sec interrupt once
Timer1.attachInterrupt(TimerSR); 
}
void TimerSR(void){
/* do something */
}
void loop(){

}

PID說明...待補

完整程式碼:

#include <TimerOne.h>
#define encoder_1A  2
#define encoder_1B  4
#define LEFT1 5
#define LEFT2 6
int cmd_indx = 0;
String str;
char cmd[10];
char *name_ = NULL;
int error1=0,err1=0,ie1=0,de1=0,u1=0;//誤差
float ki1,kd1,f1;
volatile long time_indx = 0;
float kp1;
volatile long set_position=0;//位置設定值
float st1;
float ua1=0;//輸出值
volatile long Pos1 = 0;//1號實際值
volatile long Pos1last = 0;//儲存1號上次的實際值
float dt = 0.001;//時間   
float fac = 3/(dt*780);
void setup() {
  kp1=0.66;
  //kp1=0.66;
  ki1=0.001,kd1=0.08,f1=0.47;
  //ki1=0.042,kd1=0.11,f1=0.45;
pinMode(encoder_1A, INPUT); 
digitalWrite(encoder_1A, HIGH);       // turn on pull-up resistor
pinMode(encoder_1B, INPUT); 
digitalWrite(encoder_1B, HIGH);       // turn on pull-up resistor
attachInterrupt(digitalPinToInterrupt(2), doEncoder1, CHANGE);  // enconder pin on interrupt 0-pin 2
Serial.begin (115200);
pinMode(LEFT1, OUTPUT);
pinMode(LEFT2, OUTPUT);
Timer1.initialize(1000);    // 0.15 sec interrupt once
Timer1.attachInterrupt(TimerSR); 
}
void TimerSR(void){
  
  error1 = set_position-Pos1;      
  u1= PID_control(kp1,ki1,kd1,error1,ie1,err1,f1);
  Pos1last = Pos1;//儲存實際值
  if(Pos1 >30000)
    {   Pos1 =0;//Pos1歸零
        Pos1last = Pos1last-30000;
    }
  else if(Pos1 < -30000)
    { Pos1 =0;//Pos1歸零
      Pos1last = Pos1last+30000;
    }
  motor_control(u1);
  time_indx++;
  if(time_indx%10==0){
  Serial.print(set_position);
  Serial.print(" ");
  Serial.println(Pos1);
  }
  if(time_indx>100000)time_indx=0;
}
void loop() {
if (Serial.available()) {
    cmd_indx = 0;
    // 讀取傳入的字串直到"\n"結尾
    str = Serial.readStringUntil('\n');
    str.toCharArray(cmd, 10);//str 轉char 存入cmd 大小為10
    name_ = strtok(cmd, ",");
    while(name_ != NULL)
    {
        cmd_indx++;
        /*Serial.print(cmd_indx);
        Serial.print("  ");
        Serial.println(name_);*/
        switch(cmd_indx) {
          case 1:
            set_position = String(name_).toInt();
            break;
          case 2:
            kp1 = String(name_).toFloat();
            break;
          case 3:
            ki1 = String(name_).toFloat();
            break;
          case 4:
            kd1 = String(name_).toFloat();
            break;
           default:
          Serial.println("Invalid number");
            
          
        }
        name_ = strtok(NULL, ","); 
    }
    //set_position = str.toInt();
  }
  
}
  void doEncoder1() {
  //Serial.println(Pos1);
  if (digitalRead(encoder_1A) == digitalRead(encoder_1B)) {   //高電位 低電位
    Pos1++; 
  } else {
    Pos1--;
  }
}
int PID_control(float kp, float ki, float kd, int error, int &ie, int &err,float f) {
  int de,ua,u;
  ie = ie + error*dt;         //ie=ie+e*daltT 計算積分的e
  //if (ie > 1280) ie=1280;
  //else if (ie<-1280) ie=-1280;  
  de = (error-err)/dt;              //微分的e
  ua = kp*error+ki*ie+kd*de;       //PID算法公式
  u = (int) (ua*f);           //u*=fac*u
  err = error;           //把這次的誤差存起來下次用
  if(u > 255) u = 255;     //如果u算出來大於255上限 就直接等於最大值255
  else if(u < -255) u = -255;//同上  
  return u;
}
void motor_control(int u11){
    if(u11>0){ 
    analogWrite(LEFT1,u11);
    analogWrite(LEFT2,0);
  }//直接丟入正轉副程式
  else if (u11<0){
    analogWrite(LEFT1,0);
    analogWrite(LEFT2,-u11);
  }//丟入反轉副程式(負負得正)
}