左右均速

電池是接18650*2串連8.4V

DC直流馬達接測速碼盤,D訊號接中斷接口2跟3

馬達控制用L298N模板

為了觀察目標速度改變時的收斂狀況,使用Serial改變目標速度

DC直流馬達應該先測過速度後配對使用,PWM出力才不會差太多

#include <TimerOne.h>

#define Kp 1

int set_point = 55;

int set_offset = 45;

long counter_val[2]={0,0};

byte CurCnt=0;

int j=0;

int output_value_low = 40;

int output_value_high = 80;

int output_value_l = 40;

int output_value_r = 40;

void setup(){

pinMode(8,OUTPUT);

pinMode(9,OUTPUT);

pinMode(22,OUTPUT);

pinMode(24,OUTPUT); 

digitalWrite(22, LOW);

digitalWrite(24, HIGH);

pinMode(26,OUTPUT);

pinMode(28,OUTPUT); 

digitalWrite(26, LOW);

digitalWrite(28, HIGH);

Serial.begin(115200);                                      //初始化鮑率為115200

attachInterrupt(0, counterL, RISING);          //設置編碼器相位上升沿中斷

attachInterrupt(1, counterR, RISING);          //設置編碼器相位上升沿中斷

Timer1.initialize(1000000);                            //設置定時器中斷時間，單位微秒

Timer1.attachInterrupt(timerIsr);                //打開定時器中斷

interrupts();                                                     //打開外部中斷

}

void loop(){

if(Serial.available() > 0){

set_point = Serial.parseInt();

set_offset = set_point - 10;

}

}

//外部中斷處理函數

void counterL(){

//每一個中斷加一

counter_val[0] += 1;

}

//外部中斷處理函數

void counterR(){

//每一個中斷加一

counter_val[1] += 1;

}

//定時器中斷處理函數

void timerIsr(){

long lTemp = counter_val[0];                              //讀取左輪碼盤計數

long rTemp = counter_val[1];                              //讀取右輪碼盤計數

counter_val[0]=0;                                                  //讀完清除碼盤計數

counter_val[1]=0;                                                  //讀完清除碼盤計數

output_value_l = (set_point -lTemp) * (output_value_high - output_value_low) * Kp / set_point + output_value_l;

output_value_r = (set_point -rTemp) * (output_value_high - output_value_low) * Kp / set_point + output_value_r;

//限制左右輪油門(PWM)在output_value_low-output_value_high範圍内

if(output_value_l > output_value_high)

output_value_l = output_value_high;

if(output_value_l < output_value_low )

output_value_l = output_value_low ;

if(output_value_r > output_value_high)

output_value_r = output_value_high;

if(output_value_r < output_value_low )

output_value_r = output_value_low ;

analogWrite(8, output_value_l);                              //左輪油門輸出(PWM)

analogWrite(9, output_value_r);                              //右輪油門輸出(PWM)

//輸出確認用

Serial.print("TargetSpeed:");

Serial.print(set_point);                                              //預定速度(測速碼盤缺口數/秒)

Serial.print("   Speed,Power   L:");

Serial.print( lTemp);                                                   //左輪速度(測速碼盤缺口數/秒)

Serial.print( ",");

Serial.print( output_value_l);                                   //左輪油門(PWM)

Serial.print("    R:");

Serial.print( rTemp);                                                  //右輪速度(測速碼盤缺口數/秒)

Serial.print( ",");

Serial.println( output_value_r);                                //右輪油門(PWM)

}