PID巡线_arduino[分享借鉴]

1、#include double Setpoint, Input, Output;void PID_MY();double Kp=5, Ki=0.8, Kd=5;void fun1();void fun2();int num1,num2,num3,num4,num5,num6,num7,num8;/8路循迹PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd,P_ON_M, DIRECT);int pinI1=26;/定义I1接口int pinI2=27;/定义I2接口int pinI3=28;int pinI4=29;int pinI5=30;/定义

2、I1接口int pinI6=31;/定义I2接口int pinI7=32; /一high一LOW 正转int pinI8=33;int P1=2;/定义EA(PWM调速)接口int P2=3;/定义EA(PWM调速)接口int P3=4;/定义EA(PWM调速)接口int P4=5;/定义EA(PWM调速)接口/1,3左同侧电机，对应PWM1void FORWARD(int PWM1,int PWM2);void BACK(int PWM1,int PWM2);void LEFT(int PWM1,int PWM2);void RIGHT(int PWM1,int PWM2);void STO

3、P(); void PID_my();void input_hong();void hong();void setup() pinMode(pinI1,OUTPUT); pinMode(pinI2,OUTPUT); pinMode(pinI3,OUTPUT); pinMode(pinI4,OUTPUT); pinMode(pinI5,OUTPUT); pinMode(pinI6,OUTPUT); pinMode(pinI7,OUTPUT); pinMode(pinI8,OUTPUT); pinMode(P1,OUTPUT); pinMode(P2,OUTPUT); pinMode(P3,OUT

4、PUT); pinMode(P4,OUTPUT); /initialize the variables were linked to Input=0; Setpoint = 0; /turn the PID on myPID.SetMode(AUTOMATIC);myPID.SetSampleTime(10); myPID.SetOutputLimits(-255, 255);attachInterrupt(4, fun1,RISING);/当电平上升时,触发中断函数,21attachInterrupt(3, fun2,RISING);/22 pinMode(P1,OUTPUT); pinMo

5、de(P2,OUTPUT); pinMode(pinI1,OUTPUT); pinMode(pinI2,OUTPUT); pinMode(pinI3,OUTPUT); pinMode(pinI4,OUTPUT);/马达 pinMode(19,INPUT); /1 pinMode(20,INPUT); /8 pinMode(48,INPUT); /2 pinMode(46,INPUT); /3 pinMode(44,INPUT); /4 pinMode(36,INPUT); /5 pinMode(38,INPUT); /6 pinMode(40,INPUT); /7delay(4000); PI

6、D_MY();void loop() void PID_MY() int PWM1,PWM2; while(1) hong(); input_hong();/根据红外输入input myPID.Compute(); if(Output=0) PWM1=255; PWM2=(int)(255-Output); else PWM1=(int)(255+Output); PWM2=255; FORWARD(PWM1,PWM2); if(num5=HIGH&num6=HIGH&num7=HIGH) LEFT(255,255); delay(100); if(num3=HIGH&num4=HIGH&nu

7、m5=HIGH&num6=HIGH) detachInterrupt(3); detachInterrupt(4); FORWARD(255,255); delay(500); STOP(); return; void input_hong() if(num2=HIGH) Input = -16; else if(num3=HIGH) Input = -8; else if(num4=HIGH) Input = -0.5; else if(num5=HIGH) Input = 1.5; else if(num6=HIGH) Input = 8; else if(num7=HIGH) Input

8、 = 16; void hong() num1=digitalRead(20); num2=digitalRead(36); num3=digitalRead(38); num4=digitalRead(40); num5=digitalRead(44); num6=digitalRead(46); num7=digitalRead(48); num8=digitalRead(21);/ 用num1-8保存从左到右8个传感器的状态 void fun1() do num4=digitalRead(44); RIGHT(255,255); while(num4!=HIGH); void fun2(

9、) do num5=digitalRead(36); LEFT(255,255); while(num5!=HIGH); void FORWARD(int PWM1,int PWM2) analogWrite(P1,PWM1); digitalWrite(pinI1,LOW); digitalWrite(pinI2,HIGH); analogWrite(P2,PWM2); digitalWrite(pinI3,LOW); digitalWrite(pinI4,HIGH); analogWrite(P3,PWM1); digitalWrite(pinI5,LOW); digitalWrite(p

10、inI6,HIGH); analogWrite(P4,PWM2); digitalWrite(pinI7,LOW); digitalWrite(pinI8,HIGH);void BACK(int PWM1,int PWM2) analogWrite(P1,PWM1); digitalWrite(pinI1,HIGH); digitalWrite(pinI2,LOW); analogWrite(P2,PWM2); digitalWrite(pinI3,HIGH); digitalWrite(pinI4,LOW); analogWrite(P3,PWM1); digitalWrite(pinI5,

11、HIGH); digitalWrite(pinI6,LOW); analogWrite(P4,PWM2); digitalWrite(pinI7,HIGH); digitalWrite(pinI8,LOW);void LEFT(int PWM1,int PWM2) analogWrite(P1,PWM1); digitalWrite(pinI2,LOW); digitalWrite(pinI1,HIGH); analogWrite(P2,PWM2); digitalWrite(pinI4,HIGH); digitalWrite(pinI3,LOW); analogWrite(P3,PWM1);

12、 digitalWrite(pinI6,LOW); digitalWrite(pinI5,HIGH); analogWrite(P4,PWM2); digitalWrite(pinI7,LOW); digitalWrite(pinI8,HIGH);void RIGHT(int PWM1,int PWM2) analogWrite(P1,PWM1); digitalWrite(pinI2,HIGH); digitalWrite(pinI1,LOW); analogWrite(P2,PWM2); digitalWrite(pinI4,LOW); digitalWrite(pinI3,HIGH); analogWrite(P3,PWM1); digitalWrite(pinI6,HIGH); digitalWrite(pinI5,LOW); analogWrite(P4,PWM2); digitalWrite(pinI8,LOW); digitalWrite(pinI7,HIGH);void STOP() digitalWrite(pi