#include Timer.h
#define Kp 3
#define set_point 100
long counter_val[2]={0,0};
byte CurCnt=0;
int j=0;
int output_value=0;
void setup(){
delay(2000);
pinMode(12,OUTPUT);
pinMode(3,OUTPUT);
pinMode(9,OUTPUT);
digitalWrite(9, LOW);
digitalWrite(12, HIGH);
Serial.begin(115200); //初始化鮑率為115200
attachInterrupt(0, counter, RISING);//設置編碼器A相位上升沿中斷
attachInterrupt(1, counter, RISING);//設置編碼器B相位上升沿中斷
Timer1.initialize(5000); //設置定時器中斷時間，單位微秒
Timer1.attachInterrupt(timerIsr); //打开定時器中斷
interrupts(); //打開外部中斷
}
void loop(){
long lTemp = 0; //定義臨時存儲數據變量
if(j==1){ //判斷是否發生定時器中斷，即定時時間是否到达
j=0; //清除定時器中斷標志位
if((CurCnt&0x01) == 0){ //当前使用的是偶數计數器，则上次频率值存放在第二个元素中
lTemp = counter_val[1]; //讀取數组第二个元素中的數值
counter_val[1]=0; //讀完清除原来的數值，以便下次使用
}else{ //当前使用的是奇數计數器，则上次频率值存放在第一个元素中
lTemp = counter_val[0]; //讀取數组第二个元素中的數值
counter_val[0]=0; //讀完清除原来的數值，以便下次使用
}
Serial.print("A"); //發送转速帧头大写A
Serial.print( lTemp); //發送转速數據
//比例计算得到控制量
output_value =( set_point -lTemp)*Kp+ output_value;
if(output_value >255) //限制PWM在0-255范围内
output_value =255;
if(output_value <0) //限制PWM在0-255范围内
output_value =0;
analogWrite(3, output_value); //将计算得到的控制量输出
Serial.print("B"); //發送PWM帧头大写B
Serial.println(output_value); //發送PWM數據
}
}
//外部中斷處理函數
void counter(){
//每一個中斷加一
counter_val[CurCnt& 0x01] += 1;
}
//定時器中斷處理函數
void timerIsr(){
j=1; //定時時間達到標誌
CurCnt++; //切換計數數組
}