倒立摆控制代码

#include 
#define PinA 2  //中断0
#define PinB 3  //中断1
#define INA  8 //电机正反
#define PWMA  9 //电机速度
int PDAJ = 12; //模式选择
long int anlge;
unsigned long time = 0;
long count = 0; //计数值
long num = 0;
double Setpoint Input Outputsetpoint;
double kp = 0.040ki = 0.0005kd =0.0011;//kp = 0.040ki = 0.0005kd =0.0011;
PID myPID（&Input &Output &Setpointkpkikd REVERSE）;
//初始化
void setup（）
{
        Serial.begin（9600）; //窗口初始化
         
        pinMode（PinAINPUT）; //D2脚为输入
        pinMode（PinBINPUT）; //D3脚为输入
        pinMode（PDAJINPUT_PULLUP）;
         
         
        attachInterrupt（0 blinkA FALLING   ）;  //注册中断0调用函数blinkA
        attachInterrupt（1 blinkB FALLING   ）;  //注册中断1调用函数blinkB
 
        time = micros（）; //时间初值
          myPID.SetTuni