arduino nano学习过程中的相关程序

Arduino Nano相关程序

1.超声波测距

小车加测距（测距引用的库，缺点测的距离为整数）

#include <SPI.h> // 加载SPI库
#include <Wire.h> // 加载Wire库
#include <Adafruit_GFX.h> // 加载Adafruit_GFX库3
#include <Adafruit_SSD1306.h> // 加载Adafruit_SSD1306库
#include "String.h"
// 定义 OLED屏幕的分辨率
#include <DFRobot_URM10.h>//超声波测距
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);
DFRobot_URM10 urm10;
int ceju_0=0;
int ceju_1=0;
void setup() {
   pinMode(2,OUTPUT);
  pinMode(4,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  //初始化各IO,模式为INPUT 输入模式
  //默认电机停止转动，全部设为低电平
  digitalWrite(2,0);
  digitalWrite(4,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  // put your setup code here, to run once:
  Serial.begin(9600); // 设置串口波特率
  Serial.println("OLED FeatherWing test"); // 串口输出
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // 设置OLED的I2C地址
  display.clearDisplay(); // 清空屏幕
  display.setTextSize(1); // 设置字体大小
  display.setTextColor(SSD1306_WHITE); // 设置字体颜色  
  display.display(); // 使更改的显示生效
}

void loop() {
  // put your main code here, to run repeatedly:
  ceju_0=(urm10.getDistanceCM(10, 11)); //超声波测距
  display.clearDisplay(); // 清空屏幕
  xianshi();              //调用显示程序
  ceju();
  display.display(); // 使更改的显示生效

  
}
void xianshi(){
  display.setCursor(0,0); // 设置开始显示文字的坐标
  display.println("ceju");
  display.setCursor(50,0); // 设置开始显示文字的坐标
      if(ceju_1==1){display.println("wuxiao");}
      if(ceju_1==0){display.println(ceju_0);}
  }
void ceju(){
    if(ceju_0<=1100){ceju_1=0;      //当超声波测距小于等于1100时，则ceju_2=0，并执行以下程序
    if(ceju_0<=10) {houtui();Serial.println("后退");}
    if(ceju_0>=15) {qianjin();Serial.println("前进");}
    analogWrite(3,map(ceju_0-10,15,1100,100,255));//左轮速运行,map函数映射
    analogWrite(5,map(ceju_0-10,15,1100,100,255));//右轮速运行
    if(10<ceju_0 and ceju_0<15) {zhidong();Serial.println("制动");}
    }
    if(ceju_0>1100){ceju_1=1;}
  }
void qianjin(){
     digitalWrite(2,0); //给低电平
     digitalWrite(4,1); //给高电平
     digitalWrite(6,1); 
     digitalWrite(7,0);
     Serial.println("前进"); // 串口输出
     }
void houtui(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,0); //给低电平
     digitalWrite(6,0); 
     digitalWrite(7,1);
     Serial.println("后退"); 
    } 
void zhidong(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,1); 
     digitalWrite(6,1); 
     digitalWrite(7,1);
     Serial.println("制动");
   } 

2.蓝牙控制

使用蓝牙控制小车

#include <SPI.h> // 加载SPI库
#include <Wire.h> // 加载Wire库
#include <Adafruit_GFX.h> // 加载Adafruit_GFX库3
#include <Adafruit_SSD1306.h> // 加载Adafruit_SSD1306库
#include "String.h"
// 定义 OLED屏幕的分辨率
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);
String comdata = "";//接收到的字符串
int a=100;          //使能1初始脉冲
int a_1=145;        //使能2初始脉冲
int c=0;
int x=0;
void setup() {
  Serial.begin(9600); // 设置串口波特率
  Serial.println("OLED FeatherWing test"); // 串口输出
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // 设置OLED的I2C地址
  display.setTextSize(1); // 设置字体大小
  display.setTextColor(SSD1306_WHITE); // 设置字体颜色
  //初始化各IO,模式为OUTPUT 输出模式
  pinMode(2,OUTPUT);
  pinMode(4,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  //初始化各IO,模式为INPUT 输入模式


  //默认电机停止转动，全部设为低电平
  digitalWrite(2,0);
  digitalWrite(4,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
  }
void loop() {
  // put your main code here, to run repeatedly:
  display.clearDisplay(); // 清空屏幕
  chuankou();
  xianshi();
  analogWrite(3,a);//左轮速运行
  analogWrite(5,a_1);//右轮速运行
  display.display(); // 使更改的显示生效
}
void xianshi(){
  display.setCursor(20,0); // 设置开始显示文字的坐标
  display.println("xiaoche");
  display.setCursor(0,10); // 设置开始显示文字的坐标
  display.println("moshi:");
  display.setCursor(50,10); 
      if(x==0)    {display.println("zhidong");}     //当x不同值时，OLED打印不同状态
      if(x==1)    {display.println("qianjin");}
      if(x==2)    {display.println("houtui");}
      if(x==3)    {display.println("zuozhuan");}
      if(x==4)    {display.println("youzhuan");}
  display.setCursor(0,20); 
  display.println("zuo:");
  display.setCursor(30,20); 
  display.println(String(a));
  display.setCursor(64,20);
  display.println("you:");
  display.setCursor(94,20);
  display.println(String(a_1));
  display.setCursor(0,30);
  display.println("kaiguan:");
  display.setCursor(50,30); 
      if(c==1)     {display.println("ON");}
      if(c==0)     {display.println("OFF");}
  }
void chuankou(){
      while (Serial.available() > 0)     // 串口收到字符数大于零
    {
        comdata += char(Serial.read()); //每次读一个char字符，并相加
        delay(2);
    }
    if (comdata.length() > 0)           //当字符串长度大于零时
    {
        Serial.println(comdata);        //打印接收到的字符串
        if(comdata=="ON"){c=1;}
        if(comdata=="OFF"){c=0;}
        if(c==1){    
        if(comdata=="a"){a=a+5;if(a>255){a=255;}} //接收到a时，则a+5，当a>255时a=255
        if(comdata=="b"){a=a-5;if(a<0){a=0;}}
        if(comdata=="c"){a_1=a_1+5;if(a_1>255){a_1=255;}}
        if(comdata=="d"){a_1=a_1-5;if(a_1<0){a_1=0;}}
        if(comdata=="e"){zhidong();}    //接收到e时，则执行zhidong（）；
        if(comdata=="f"){qianjin();}
        if(comdata=="g"){houtui();}
        if(comdata=="h"){zuozhuan();}
        if(comdata=="r"){youzhuan();}
        }
        comdata = "";   //清空字符串
          }   
    }
void qianjin(){
     digitalWrite(2,0); //给低电平
     digitalWrite(4,1); //给高电平
     digitalWrite(6,1); 
     digitalWrite(7,0);
     Serial.println("前进"); // 串口输出
     x=1;}
void houtui(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,0); //给低电平
     digitalWrite(6,0); 
     digitalWrite(7,1);
     Serial.println("后退"); 
     x=2;} 
void zuozhuan(){
     digitalWrite(2,0); 
     digitalWrite(4,1); //给低电平
     digitalWrite(6,0); //给高电平
     digitalWrite(7,0);
     Serial.println("左转"); 
     x=3;}     
void youzhuan(){
     digitalWrite(2,0); //给高电平
     digitalWrite(4,0); //给低电平
     digitalWrite(6,1); 
     digitalWrite(7,0);
     Serial.println("右转"); 
     x=4;}         
void zhidong(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,1); 
     digitalWrite(6,1); 
     digitalWrite(7,1);
     Serial.println("制动");
     x=0;} 

3.寻迹

小车使用六路传感器寻迹

int y_1,y_2,y_3,y_4,y_5,y_6;
int i;
int x;
int a=100;          //使能1初始脉冲
int a_1=125;        //使能2初始脉冲
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600); // 设置串口波特率
  pinMode(2,OUTPUT);
  pinMode(4,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  //初始化各IO,模式为INPUT 输入模式
  
  //默认电机停止转动，全部设为低电平
  digitalWrite(2,0);
  digitalWrite(4,LOW);
  digitalWrite(6,LOW);
  digitalWrite(7,LOW);
}

void loop() {
  // put your main code here, to run repeatedly:
  xunji();
  analogWrite(3,a);//左轮速运行
  analogWrite(5,a_1);//右轮速运行
}
void xunji(){
    y_1=digitalRead(12);//传感器接收到红外信号输出高电平，传感器没有接收到红外信号输出低电平
    y_2=digitalRead(13);
    if(analogRead(A0)>900){y_3=1;Serial.println("1");}else{y_3=0;Serial.println("2");}
    if(analogRead(A1)>900){y_4=1;Serial.println("1");}else{y_4=0;Serial.println("1");}
    if(analogRead(A2)>900){y_5=1;}else{y_5=0;}
    if(analogRead(A3)>900){y_6=1;}else{y_6=0;}
    Serial.println(y_1);
    Serial.println(y_2);
    if(y_1==0 and y_2==0 and y_5==0 and y_6==0){qianjin();Serial.println("1");}
    if(y_1==0 and y_2==0 and y_5==1 and y_6==0){youzhuan();Serial.println("2");}
    if(y_1==0 and y_2==1 and y_5==0 and y_6==0){zuozhuan();Serial.println("3");}
    if(y_1==0 and y_2==0 and y_5==1 and y_6==1)
    {
      for(i=0;i<50;i++)
      dayouzhuan();
      Serial.println("大左转");
      }
    if(y_1==0 and y_2==1 and y_5==0 and y_6==0){zuozhuan();Serial.println("右转");}
    if(y_1==1 and y_2==1 and y_5==0 and y_6==0)
    {for(i=0;i<50;i++)
      dazuozhuan();
      Serial.println("大右转");
      }
    if(y_1==1 and y_2==1 and y_3==1 and y_4==1){zhidong();delay(1000);Serial.println("制动");}
  }
void qianjin(){
     digitalWrite(2,0); //给低电平
     digitalWrite(4,1); //给高电平
     digitalWrite(6,1); 
     digitalWrite(7,0);
     Serial.println("前进"); // 串口输出
     x=1;}
void houtui(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,0); //给低电平
     digitalWrite(6,0); 
     digitalWrite(7,1);
     Serial.println("后退"); 
     x=2;} 
void zuozhuan(){
     digitalWrite(2,0); 
     digitalWrite(4,1); //给低电平
     digitalWrite(6,0); //给高电平
     digitalWrite(7,0);
     Serial.println("左转"); 
     x=3;} 
void dazuozhuan(){
     digitalWrite(2,0); //给低电平
     digitalWrite(4,1); //给高电平
     digitalWrite(6,0);
     digitalWrite(7,1);
     Serial.println("大左转"); 
     x=5;}      
void youzhuan(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,0); //给低电平
     digitalWrite(6,0); 
     digitalWrite(7,0);
     Serial.println("右转"); 
     x=4;}   
void dayouzhuan(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,0); //给低电平
     digitalWrite(6,1); 
     digitalWrite(7,0);
     Serial.println("大右转"); 
     x=5;}        
void zhidong(){
     digitalWrite(2,1); //给高电平
     digitalWrite(4,1); 
     digitalWrite(6,1); 
     digitalWrite(7,1);
     Serial.println("制动");
     x=0;} 

4.舵机

控制舵机简单运动

#include <Servo.h>//定义头文件，这里有一点要注意，可以直接在Arduino 软件菜单栏单击Sketch>Importlibrary>Servo,调用Servo 函数，也可以直接输入#include <Servo.h>，但是在输入时要注意在#include 与<Servo.h>之间要有空格，否则编译时会报错。
int i;
Servo myservo;//定义舵机变量名
void setup()
{
    Serial.begin(9600); // 设置串口波特率
    myservo.attach(2);//定义舵机接口（9、10 都可以，缺点只能控制2 个）
    myservo.write(0);
} 
void loop()
{
  delay(2000);
  if(i==0){
    for(i=0;i<180;i++)
    {
    delay(100);
    myservo.write(i);//设置舵机旋转的角度
    Serial.println(i);
    }
  }
  if(i==180){
    Serial.println("ssss");
    for(i=180;i>0;i--)
    {
    delay(100);
    myservo.write(i);//设置舵机旋转的角度
    Serial.println(i);
    }
  }
}

5.串口多舵机

使用蓝牙控制舵机（忘记删掉测距了）

#include <Servo.h>//定义头文件，这里有一点要注意，可以直接在Arduino 软件菜单栏单击Sketch>Importlibrary>Servo,调用Servo 函数，也可以直接输入#include <Servo.h>，但是在输入时要注意在#include 与<Servo.h>之间要有空格，否则编译时会报错。
int a_1;
String i,a;
int ceju_0;
String comdata = "";//接收到的字符串
#include <DFRobot_URM10.h>//超声波测距
DFRobot_URM10 urm10;
Servo servo_x, servo_y;//定义舵机变量名
void setup()
{
    Serial.begin(9600); // 设置串口波特率
    servo_x.attach(2);//定义舵机接口
    servo_y.attach(3);//定义舵机接口
    servo_x.write(0);
    servo_y.write(0);
}
 
void loop()
{
    ceju_0=(urm10.getDistanceCM(10, 11)); //超声波测距
    delay(2000);
    chuankou();
    if(i=="a"){
        delay(100);
        servo_x.write(a_1);//设置舵机旋转的角度
        Serial.println(a_1);
    }
    if(i=="b"){
        delay(100);
        servo_y.write(a_1);
        Serial.println(a_1);
    }
} 
void chuankou(){
  while (Serial.available() > 0)     // 串口收到字符数大于零
    {
        comdata += char(Serial.read()); //每次读一个char字符，并相加
        delay(2);
    }
    if (comdata.length() > 0)           //当字符串长度大于零时
    {
        Serial.println(comdata);        //打印接收到的字符串
        i=comdata.charAt(0);			//截取第零位
        a=comdata.substring(1);			//第一位及以后
        a_1=a.toInt();
        comdata = "";   //清空字符串
          }   
  }

6.6050陀螺仪

卡尔曼滤波结合6050陀螺仪控制舵机

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
MPU6050 accelgyro;
Servo servo_x, servo_y;//定义舵机变量名
unsigned long now, lastTime = 0;
float dt;                                   //微分时间
 
int16_t ax, ay, az, gx, gy, gz;             //加速度计陀螺仪原始数据
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0;    //角度变量
long axo = 0, ayo = 0, azo = 0;             //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0;             //陀螺仪偏移量
 
float pi = 3.1415926;
float AcceRatio = 16384.0;                  //加速度计比例系数
float GyroRatio = 131.0;                    //陀螺仪比例系数
 
uint8_t n_sample = 8;                       //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0};         //x,y轴采样队列
long aax_sum, aay_sum,aaz_sum;                      //x,y轴采样和
 
float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; //加速度计协方差计算队列
float Px=1, Rx, Kx, Sx, Vx, Qx;             //x轴卡尔曼变量
float Py=1, Ry, Ky, Sy, Vy, Qy;             //y轴卡尔曼变量
float Pz=1, Rz, Kz, Sz, Vz, Qz;             //z轴卡尔曼变量
 
void setup()
{
    Wire.begin();
    Serial.begin(115200);
    servo_x.attach(2);//定义舵机接口
    servo_y.attach(3);//定义舵机接口
    servo_x.write(0);
    servo_y.write(0);
    accelgyro.initialize();                 //初始化
 
    unsigned short times = 200;             //采样次数
    for(int i=0;i<times;i++)
    {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
        axo += ax; ayo += ay; azo += az;      //采样和
        gxo += gx; gyo += gy; gzo += gz;
    
    }
    
    axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
    gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
}
void lvbo(){
      unsigned long now = millis();             //当前时间(ms)
    dt = (now - lastTime) / 1000.0;           //微分时间(s)
    lastTime = now;                           //上一次采样时间(ms)
 
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
 
    float accx = ax / AcceRatio;              //x轴加速度
    float accy = ay / AcceRatio;              //y轴加速度
    float accz = az / AcceRatio;              //z轴加速度
 
    aax = atan(accy / accz) * (-180) / pi;    //y轴对于z轴的夹角
    aay = atan(accx / accz) * 180 / pi;       //x轴对于z轴的夹角
    aaz = atan(accz / accy) * 180 / pi;       //z轴对于y轴的夹角
 
    aax_sum = 0;                              // 对于加速度计原始数据的滑动加权滤波算法
    aay_sum = 0;
    aaz_sum = 0;
  
    for(int i=1;i<n_sample;i++)
    {
        aaxs[i-1] = aaxs[i];
        aax_sum += aaxs[i] * i;
        aays[i-1] = aays[i];
        aay_sum += aays[i] * i;
        aazs[i-1] = aazs[i];
        aaz_sum += aazs[i] * i;
    
    }
    
    aaxs[n_sample-1] = aax;
    aax_sum += aax * n_sample;
    aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90°
    aays[n_sample-1] = aay;                        //此处应用实验法取得合适的系数
    aay_sum += aay * n_sample;                     //本例系数为9/7
    aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
    aazs[n_sample-1] = aaz; 
    aaz_sum += aaz * n_sample;
    aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
 
    float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
    float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
    float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
    agx += gyrox;                             //x轴角速度积分
    agy += gyroy;                             //x轴角速度积分
    agz += gyroz;
    
    /* kalman start */
    Sx = 0; Rx = 0;
    Sy = 0; Ry = 0;
    Sz = 0; Rz = 0;
    
    for(int i=1;i<10;i++)
    {                 //测量值平均值运算
        a_x[i-1] = a_x[i];                      //即加速度平均值
        Sx += a_x[i];
        a_y[i-1] = a_y[i];
        Sy += a_y[i];
        a_z[i-1] = a_z[i];
        Sz += a_z[i];
    
    }
    
    a_x[9] = aax;
    Sx += aax;
    Sx /= 10;                                 //x轴加速度平均值
    a_y[9] = aay;
    Sy += aay;
    Sy /= 10;                                 //y轴加速度平均值
    a_z[9] = aaz;
    Sz += aaz;
    Sz /= 10;
 
    for(int i=0;i<10;i++)
    {
        Rx += sq(a_x[i] - Sx);
        Ry += sq(a_y[i] - Sy);
        Rz += sq(a_z[i] - Sz);
    
    }
    
    Rx = Rx / 9;                              //得到方差
    Ry = Ry / 9;                        
    Rz = Rz / 9;
  
    Px = Px + 0.0025;                         // 0.0025在下面有说明...
    Kx = Px / (Px + Rx);                      //计算卡尔曼增益
    agx = agx + Kx * (aax - agx);             //陀螺仪角度与加速度计速度叠加
    Px = (1 - Kx) * Px;                       //更新p值
 
    Py = Py + 0.0025;
    Ky = Py / (Py + Ry);
    agy = agy + Ky * (aay - agy); 
    Py = (1 - Ky) * Py;
  
    Pz = Pz + 0.0025;
    Kz = Pz / (Pz + Rz);
    agz = agz + Kz * (aaz - agz); 
    Pz = (1 - Kz) * Pz;
 
    /* kalman end */
 
    Serial.print(agx);Serial.print(",");
    Serial.print(agy);Serial.print(",");
    Serial.print(agz);
    Serial.println();
  }
void loop()
{
lvbo();
servo_x.write(int(agy)+90);
servo_y.write(int(agx)+90);
}

7.学习pid

7.1

pid控制直流电机

#define AIN1  3
#define AIN2  4
#define PWMA  5
#define AA  2   //编码器相位A                                                   
int valA=0;     //脉冲数
float  n;       //单位时间内转速
int flag=0;
int pwm=70;     //设置初始PWM值
unsigned long starttime,stoptime;   
void timer()
{
  valA++;
  stoptime=millis(); 
   if((stoptime-starttime)>100)     //100微秒计数一次
   { 
    detachInterrupt(0);             //禁用0号中断号的所有中断
    flag=1;
   }
}
void setup() {
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(PWMA,OUTPUT);
  pinMode(AA,INPUT);               
  Serial.begin(9600);
  attachInterrupt(0,timer,RISING); 
  starttime=millis();
  Serial.println(starttime); 
}
void loop() {
  digitalWrite(AIN1,HIGH); 
  digitalWrite(AIN2,LOW);
  analogWrite(PWMA,pwm); 
  
    if(flag==1)         //if()前面加while（1）不行
  
    { 
      n=valA/13.000/226*1000;  //计算一秒内转速，霍尔编码器（每圈13个信号单相），减速比226   
      valA=0;
      flag=0;
      pwm=pwm+(25-n)*2;        //增量式
      if(pwm>255)
        pwm=255;
      if(pwm<0)
        pwm=0;
      delay(0);
      starttime=millis();
      attachInterrupt(0,timer,RISING); 
    }
	Serial.println(n);
}

7.2

pid控制滑轨上的小球

#define AIN1  3
#define AIN2  4
#define PWMA  5
#define AA  2   //编码器相位A                                                   
int valA=0;     //脉冲数
float  distance;       //单位时间内转速
int flag=0;
int pwm=70;     //设置初始PWM值
unsigned long starttime,stoptime;   
//PID constants****************************************************************
float setPoint=25;                       //滑轨中心与测距模块的距离
float error;                             //当前误差
float previous_error;                    //上一时刻误差，用来计算D
float kp=2;                              //10
float ki=0.0001;                              //0.05
float kd=200;                              //200 
int   dt=50;                             //每50毫秒进行一次计算                       
float P,I,D, PID;
//*****************************************************************************

void timer()
{
  valA++;
  stoptime=millis(); 
   if((stoptime-starttime)>100)     //100微秒计数一次
   { 
    detachInterrupt(0);             //禁用0号中断号的所有中断
    flag=1;
   }
}
void setup() {
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(PWMA,OUTPUT);
  pinMode(AA,INPUT);               
  Serial.begin(9600);
  attachInterrupt(0,timer,RISING); 
  starttime=millis();
  Serial.println(starttime); 
}
void loop() {
  digitalWrite(AIN1,HIGH); 
  digitalWrite(AIN2,LOW);
  analogWrite(PWMA,pwm); 
  
    if(flag==1)         //if()前面加while（1）不行
    { 
      distance=valA/13.000/226*1000;  //计算一秒内转速，霍尔编码器（每圈13个信号单相），减速比226   
      valA=0;
      flag=0;
    // PID Calculation, PID计算*************************************************
    error = setPoint-distance;             //计算误差
    P = kp*error;                          //P项
    if(-4 < error && error < 4){
       I += ki*error;    }                 //I项
    else{
       I = 0;            }   
    D = kd*((error - previous_error)/dt);  //D项，
    PID=P + I + D ;                        //PID
    if (PID>200){PID=200;}                 //限幅
    if (PID<-200){PID=-200;}               //限幅

    previous_error=error;                  //更新上一时刻误差
    pwm=pwm+PID;
    // Serial Display,在串口监视器显示每个项，有助于调试*****************************
//     Serial.print("Distance: "); Serial.print(distance); Serial.print(" cm  "); 
     Serial.print("Error: ");    Serial.println(error);    //Serial.print(" cm  ");
//     Serial.print("   P: ");     Serial.print(P); 
//     Serial.print("   D: ");     Serial.print(D);
//     Serial.print("   I: ");     Serial.print(I);
//     Serial.print("   PID: ");   Serial.println(PID);
    
    // Servo Control，用计算结果控制舵机*******************************************
    //pwm = map(PID, -200,200,70,255);
      delay(0);
      starttime=millis();
      attachInterrupt(0,timer,RISING); 
    }
}

7.3

pid控制滑轨上的小球（改进版）

/* PID control Demo, PID 控制算法演示
 * Last Edited: March.12th.2021 by Mun Kim
 * Contact: Robotix.Kim@gmail.com
 */

#include <Servo.h>

Servo servo;   
int angle = 0;                           //舵机角度

#define echo 12                           //HC-SR04的Echo接 Arduino D2
#define trig 11                           //HC-SR04的Trig接 Arduino D3
float time, duration, distance;

//PID constants****************************************************************
float setPoint=12;                       //滑轨中心与测距模块的距离
float error;                             //当前误差
float previous_error;                    //上一时刻误差，用来计算D
float kp=10;                              //10
float ki=0.08;                              //0.05
float kd=200;                              //200 
int   dt=10;                             //每50毫秒进行一次计算                       
float P,I,D, PID;
//*****************************************************************************

void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT); 
  pinMode(echo, INPUT); 
  servo.attach(2);                 
  time = millis();
}

void loop() {
    if (millis() > time + dt)
  {
    time = millis();    

    // HC-SR04 Distance Measuremnt, 通过超声波模块测量胶带的位置*******************
    digitalWrite(trig, LOW); 
    delayMicroseconds(2); 
    digitalWrite(trig, HIGH); 
    delayMicroseconds(10); 
    digitalWrite(trig, LOW); 

    duration = pulseIn(echo, HIGH);        //读取反射信号 
    distance = (duration*0.0343)/2;        //测距公式，单位为cm
    
    // PID Calculation, PID计算*************************************************
    error = distance-setPoint;             //计算误差
    if(error<=1 and error>=-1){Serial.println("budong "); }
    else{
    P = kp*error;                          //P项
//    if(-4 < error && error < 4){
//       I += ki*error;    }                 //I项
//    else{
//       I = 0;            }  
I += ki*error; 
    D = kd*((error - previous_error)/dt);  //D项，
    PID=P + I + D ;                        //PID
    if (PID>200){PID=200;}                 //限幅
    if (PID<-200){PID=-200;}               //限幅

    previous_error=error;                  //更新上一时刻误差
    
    // Serial Display,在串口监视器显示每个项，有助于调试*****************************
     Serial.print("Distance: "); Serial.print(distance); Serial.print(" cm  "); 
     Serial.print("Error: ");    Serial.print(error);    Serial.print(" cm  ");
     Serial.print("   P: ");     Serial.print(P); 
     Serial.print("   D: ");     Serial.print(D);
     Serial.print("   I: ");     Serial.print(I);
     Serial.print("   PID: ");   Serial.println(PID);
    
    // Servo Control，用计算结果控制舵机*******************************************
    if(-200<PID and PID<0){angle = map(PID, -200,0,70,110);}
    if(0<PID and PID<200){angle = map(PID, 0,200,110,120);}
    servo.write(angle); 
    }
  }
}