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

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

暂无评论

发表评论

相关推荐

笔记3:STM32F103与STM32F030的区别

在替换芯片时,发现STM32f103与STM32F030的区别: 内部结构 下图是103的IO口内部结构: 下图是030的IO口内部结构图: 通过上面的比较可以看出: 两个

2、STM32点亮LED灯

一、寄存器操作 1、新建工程,新建一个目录存放以后所有的工程stmproject,在这个目录下新建文件夹寄存器点灯,文件名为LED。 接下来选择所用的软件包,之前安装软件时安装的&#xff

【零基础学STM32】CubeMx+HAL配置按键点灯实验

主要内容 上一篇文章中采用了EXTI中断的方式控制小灯的亮灭。其实也可以不采用中断,直接使用GPIO配置即可实现按键对应小灯的状态,且效果几乎相同。同样本次例程试验采用正点原子阿波罗STM32F429IGT6开发板

STM32 HAL库学习笔记-(SPI驱动ADXL345)

STM32 HAL库学习笔记-(SPI驱动ADXL345) 最近有项目需要需要用到ADXL345,网上转了一圈都是IIC驱动为主,正好最近在学习HAL库,所以本文将使用SPI驱动ADXL345 CU