DIY磁悬浮“盗梦陀螺”相关程序.doc

//PINs setting  int adjust1Pin = 1;    //用来调节A的电位器  int adjust2Pin = 2;    //用来调节B的电位器  int read1Pin = 4;      //用来连接输入A传感器  int read2Pin = 3;      //用来连接输入B传感器  int i1Pin = 36;        //连接电机驱动板的I1接口  int i2Pin = 37;        //连接电机驱动板的I2接口  int i3Pin = 39;        //连接电机驱动板的I3接口  int i4Pin = 38;        //连接电机驱动板的I4接口  int power1Pin = 5;     //连接电机驱动板的EA接口  int power2Pin = 6;     //连接电机驱动板的EB接口  int rotatePin = 3;     //用来控制磁场旋转的PMW接口    boolean debug = false;  boolean writeLog = false;  double setKd1 = ;  double setKd2 = ;  double setKp = 22;  int offset = 70;  int delayMs = 1;  int tick = 0;  int myLog[3500];    //PID structure  typedef struct {    double target;    double aver;    double Kp;    double Kd;    int preError;    int power;    boolean flag;    double v;  } PID;    PID Pid1, Pid2;    void setup()  {    pinMode(i1Pin, OUTPUT);     //I1和I2都是数字信号    pinMode(i2Pin, OUTPUT);     //通过设置I1和I2来控制电流方向    pinMode(i3Pin, OUTPUT);     //I1和I2都是数字信号    pinMode(i4Pin, OUTPUT);     //通过设置I1和I2来控制电流方向    pinMode(power1Pin, OUTPUT);  //按占空比方式输出的模拟信号    pinMode(power2Pin, OUTPUT);  //按占空比方式输出的模拟信号    pinMode(rotatePin, OUTPUT);  //按占空比方式输出的模拟信号      //analogWrite(rotatePin, 128);      (9600);          //设置波特率    TCCR0B = 0x01;   // Timer 0: PWM 5 &  6 @ 16 kHz    TCCR1B = 0x01;   // Timer 1: PWM 9 & 10 @ 32 kHz    TCCR2B = 0x01;   // Timer 2: PWM 3 & 11 @ 32 kHz