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: [email protected]
*/
#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);
}
}
}