PID巡線_arduino

1、#include <PID_v1.h>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;i

2、nt pinI4=29;int pinI5=30;/定義I1接口int pinI6=31;/定義I2接口int pinI7=32; /一high一LOW 正轉(zhuǎn)int pinI8=33;int P1=2;/定義EA(PWM調(diào)速)接口int P2=3;/定義EA(PWM調(diào)速)接口int P3=4;/定義EA(PWM調(diào)速)接口int P4=5;/定義EA(PWM調(diào)速)接口/1,3左同側(cè)電機(jī)，對應(yīng)PWM1void FORWARD(int PWM1,int PWM2);void BACK(int PWM1,int PWM2);void LEFT(int PWM1,int PWM2);void RIGHT

3、(int PWM1,int PWM2);void STOP(); 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); pinMod

4、e(P2,OUTPUT); pinMode(P3,OUTPUT); pinMode(P4,OUTPUT); /initialize the variables we're linked to Input=0; Setpoint = 0; /turn the PID on myPID.SetMode(AUTOMATIC);myPID.SetSampleTime(10); myPID.SetOutputLimits(-255, 255);attachInterrupt(4, fun1,RISING);/當(dāng)電平上升時(shí),觸發(fā)中斷函數(shù),21attachInterrupt(3, fun2,RISI

5、NG);/22 pinMode(P1,OUTPUT); pinMode(P2,OUTPUT); pinMode(pinI1,OUTPUT); pinMode(pinI2,OUTPUT); pinMode(pinI3,OUTPUT); pinMode(pinI4,OUTPUT);/馬達(dá) 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 pin

6、Mode(40,INPUT); /7delay(4000); PID_MY();void loop() void PID_MY() int PWM1,PWM2; while(1) hong(); input_hong();/根據(jù)紅外輸入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=HIG

7、H) LEFT(255,255); delay(100); if(num3=HIGH&&num4=HIGH&&num5=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

8、 if(num5=HIGH) Input = 1.5; else if(num6=HIGH) Input = 8; else if(num7=HIGH) Input = 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個(gè)傳感器的狀態(tài)

9、void fun1() do num4=digitalRead(44); RIGHT(255,255); while(num4!=HIGH); void fun2() 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); digit

10、alWrite(pinI4,HIGH); analogWrite(P3,PWM1); digitalWrite(pinI5,LOW); digitalWrite(pinI6,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); digitalWri

11、te(pinI3,HIGH); digitalWrite(pinI4,LOW); analogWrite(P3,PWM1); digitalWrite(pinI5,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(

12、P2,PWM2); digitalWrite(pinI4,HIGH); digitalWrite(pinI3,LOW); analogWrite(P3,PWM1); 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() digit