为了更好的采样我使用了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
暂无评论