電池是接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)
}