arduino四路红外PID循迹小车

为了更好的采样我使用了arduino的中断,所以需要手动装一下中断库。主控是我自己画的,集成了驱动在板子上。但是其他驱动像L298N,TB6612都是可以代替的,更改下端口号即可。

代码如下:

#include <MsTimer2.h>
int sensor[5];
int moto1A = 5;   //我是四轮,左边两个电机并联,右边两个电机并联
int moto1B = 6;
int moto2A = 9;
int moto2B = 10;
int base = 100;   //基础速度
float err,last_err;
void moto_run(int x,int y);
void  line();
void  turn_left();
void  turn_right();
int  cacu_pwm();
void flash()           
{
    sensor[0]=digitalRead(A0); 
    sensor[1]=digitalRead(A1);
    sensor[2]=digitalRead(A2);
    sensor[3]=digitalRead(A3); 
    if(sensor[0] == 0&& sensor[1] == 0&& sensor[2] == 1&&sensor[3] == 0)  //0010
      err = 1;
    if(sensor[0] == 0&& sensor[1] == 0&& sensor[2] == 0&&sensor[3] == 1)  //0001
      err = 2;
    if(sensor[0] == 0&& sensor[1] == 1&& sensor[2] == 0&&sensor[3] == 0)  //0100
      err = -1;
    if(sensor[0] == 1&& sensor[1] == 0&& sensor[2] == 0&&sensor[3] == 0)  //1000
      err = -2;
}
int  cacu_pwm()            //pid运算部分
{
    int output;
    float kp=60,ki=0,kd=12;    //pid需要根据实际情况进行调节
    float P,I,D;
    P = err;
    I += err;
    D = err - last_err;
    output = kp*P + ki*I + kd*D; 
    last_err = err;
    return output;
}
void setup()
{
  Serial.begin(9600);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  pinMode(moto1A,OUTPUT);
  pinMode(moto1B,OUTPUT);
  pinMode(moto2A,OUTPUT);
  pinMode(moto2A,OUTPUT);
  MsTimer2::set(5, flash); //开启中断 5ms period
  MsTimer2::start();
}
void moto_run(int x,int y)           //电机控制部分
  {
     if(x>0)
     {
     analogWrite(moto1A,x); 
     analogWrite(moto1B,0); 
     }
     if(x<0)
     {
     analogWrite(moto1A,0); 
     analogWrite(moto1B,-x); 
     }
      if(y>0)
     {
     analogWrite(moto2A,y); 
     analogWrite(moto2B,0); 
     }
      if(y<0)
     {
     analogWrite(moto2A,0); 
     analogWrite(moto2B,-y); 
     }
  }
    void  line()   //直行
  {
    moto_run(100,100);  
  } 
    void  turn_left()   //左转
  {
    moto_run(-100,100);  
  } 
    void  turn_right()   //右转
  {
    moto_run(100,-100);  
  }
void loop()
{
     Serial.println(sensor[0]);
     Serial.println(sensor[1]);
     Serial.println(sensor[2]);
     Serial.println(sensor[3]);
     moto_run(base+cacu_pwm(),base-cacu_pwm());
}

版权声明:本文为CSDN博主「mxyflame871」的原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/qq_53736029/article/details/122673328

生成海报
点赞 0

mxyflame871

我还没有学会写个人说明!

暂无评论

发表评论

相关推荐

stm32——4、中断exti

这里是基于正点原子开发板的学习记录。 首先你要加入固件库 stm32f10x_exti.h 和 stm32f10x_exti.c 1、STM32中每个io口都可以作为外部中断的中断输入口。 2、STM32F103的中断控制器支持19个外部中断/

STM32的GPIO端口

GPIO:General Purpose Input & Output STM32芯片最拥有GPIOA、GPIOB…GPIOG等7组端口,每组端口最多拥有Pin0、Pin1…Pin15共16个引脚。 STM32的每个

STM32外部中断控制LED灯

EXTI简介 EXTI是外部中断控制器,管理了控制器的20个中断/事件。每个中断/事件都对应一个边沿检测器,可以实现输入信号的上升沿检测和下降沿检测。EXTI可以实现对每个中断/事件进行单独配置。 EXTI功能框