arduino pid麦克纳姆轮小车程序详解

liyuearduino

我想学习一下这个内容

BIRDA2

感谢分享

BIRDA

好帖，学习学习

zwq12345

作者很好，很辛苦

zwq12345

库怎么不能下载啊

BIRDA

求助楼主大佬：
我写的程序是ref=30转速，使用蓝牙控制小车前进时，转速是30.然后让小车停下，第二次控制小车转速就变成60了，停下再次前进，转速变成90了是为什么，调整了好久都还是这样
简单来说就是 前进（转速30）->停下->前进（转速60）->停下->前进（转速90）就这样不停的往上加
下面是我的代码，可以帮我看看问题出在哪里了吗，非常感谢
#include <Servo.h>
#include <ID_v1.h>
#include <Wire.h>
double Va,MA1,Vb,MB1,Vc,MC1,Vd,MD1;
unsigned long t;
volatile long counter_valA=0,counter_valB=0,counter_valC=0,counter_valD=0;  //储存编码器的值，而且中断函数中的变量必须用volatile来定义
//定义引脚名称
#define EN      2  //使能输出引脚，该引脚时高电平才允许控制直流电机，低电平时电机停止
#define MotorA1 3  //PWM输出引脚，控制直流电机A
#define AD      A0 //PWM输入引脚，读取电池电压
#define MotorA2 5  //PWM输出引脚，控制直流电机A
#define MotorB1 6  //PWM输出引脚，控制直流电机B
#define MotorB2 9  //PWM输出引脚，控制直流电机B
#define MotorC1 10 //PWM输出引脚，控制直流电机C
#define MotorC2 11 //PWM输出引脚，控制直流电机C
#define MotorD1 12 //PWM输出引脚，控制直流电机D
#define MotorD2 13 //PWM输出引脚，控制直流电机D
const double ma_in=21; //A轮A相口
const double mb_in=20; //B轮A相口
const double mc_in=19; //C轮A相口
const double md_in=18; //D轮A相口
#define EN      2  //使能输出引脚，该引脚时高电平才允许控制直流电机，低电平时电机停止
double Kp=2.1,Ki=4.3,Kd=0.022,ref=30.0;
PID MAPID(&Va,&MA1,&ref,Kp,Ki,Kd,DIRECT);
PID MBPID(&Vb,&MB1,&ref,Kp,Ki,Kd,DIRECT);
PID MCPID(&Vc,&MC1,&ref,Kp,Ki,Kd,DIRECT);
PID MDPID(&Vd,&MD1,&ref,Kp,Ki,Kd,DIRECT);

char data;//蓝牙端口收到的信号
#define AUTOMATIC 1
#define MANUAL 0
void setup()
{
  //delay(5000);
  //引脚功能初始化
  pinMode(EN, OUTPUT);
  pinMode(MotorA1, OUTPUT);
  pinMode(MotorA2, OUTPUT);
  pinMode(MotorB1, OUTPUT);
  pinMode(MotorB2, OUTPUT);
  pinMode(MotorC1, OUTPUT);
  pinMode(MotorC2, OUTPUT);
  pinMode(MotorD1, OUTPUT);
  pinMode(MotorD2, OUTPUT);
  pinMode(ma_in, INPUT);   //A轮A相输入口
  pinMode(mb_in, INPUT);   //B轮A相输入口
  pinMode(mc_in, INPUT);   //C轮A相输入口
  pinMode(md_in, INPUT);   //D轮A相输入口
  digitalWrite(EN, 1);
  Serial.begin(9600);                //初始化波特率为9600
  Serial2.begin(9600);               //蓝牙端口
  Serial3.begin(9600);               //陀螺仪端口
  attachInterrupt(2, counterA, FALLING);//设置A轮编码器A相位变化沿中断
   attachInterrupt(3, counterB, FALLING);//设置B轮编码器A相位变化沿中断
    attachInterrupt(4, counterC, FALLING);//设置C轮编码器A相位变化沿中断
     attachInterrupt(5, counterD, FALLING);//设置D轮编码器A相位变化沿中断
  MAPID.SetMode(1);使pid进入自动模式
  MBPID.SetMode(1);使pid进入自动模式
  MCPID.SetMode(1);使pid进入自动模式
  MDPID.SetMode(1);使pid进入自动模式
  MAPID.SetSampleTime(30);//设置采样时间为30ms
  MBPID.SetSampleTime(30);//设置采样时间为30ms
  MCPID.SetSampleTime(30);//设置采样时间为30ms
  MDPID.SetSampleTime(30);//设置采样时间为30ms
  MAPID.SetOutputLimits(-255,255);//设置PWM输出范围
  MBPID.SetOutputLimits(-255,255);//设置PWM输出范围
  MCPID.SetOutputLimits(-255,255);//设置PWM输出范围
  MDPID.SetOutputLimits(-255,255);//设置PWM输出范围
  t=millis();
  
}
void loop()
{
  
  if(millis()>t)
  {
  Speed();
  t=millis()+30;
  Serial.println(Vc);
  }

   if(Serial2.available()>0)
   {
    data=Serial2.read();
    if(data=='8')
    runforward();
    if(data=='2')
    runbackward();
    if(data=='4')
    runleftward();
    if(data=='6')
    runrightward();
    if(data=='0')
    stopall ();
   }

   /*while (Serial3.available())
  {
    JY901.CopeSerialData(Serial3.read()); //Call JY901 data cope function
  }*/
  
   
   
}
void stopall()//停止
{
  
  analogWrite(MotorA2,0);
  digitalWrite(MotorA1,0);
  analogWrite(MotorB2,0);
  digitalWrite(MotorB1,0);
  analogWrite(MotorC2,0);
  digitalWrite(MotorC1,0);
  analogWrite(MotorD2,0);
  digitalWrite(MotorD1,0);

}
void runforward()//向前
{

  MAPID.Compute();                //PID计算，得出输出PMW
  MBPID.Compute();
  MCPID.Compute();
  MDPID.Compute();
  analogWrite(MotorA2,MA1);
  digitalWrite(MotorA1,0);
  analogWrite(MotorB2,MB1);
  digitalWrite(MotorB1,0);
  analogWrite(MotorC2,MC1);
  digitalWrite(MotorC1,0);
  analogWrite(MotorD2,MD1);
  digitalWrite(MotorD1,0);
  
}
void runbackward()//向后
{
   
  MAPID.Compute();                //PID计算，得出输出PMW
  MBPID.Compute();
  MCPID.Compute();
  MDPID.Compute();
  analogWrite(MotorA1,MA1);
  digitalWrite(MotorA2,0);
  analogWrite(MotorB1,MB1);
  digitalWrite(MotorB2,0);
  analogWrite(MotorC1,MC1);
  digitalWrite(MotorC2,0);
  analogWrite(MotorD1,MD1);
  digitalWrite(MotorD2,0);
  
}
void runleftward()//向左
{
  MAPID.Compute();                //PID计算，得出输出PMW
  MBPID.Compute();
  MCPID.Compute();
  MDPID.Compute();
  analogWrite(MotorA1,MA1);
  digitalWrite(MotorA2,0);
  analogWrite(MotorB2,MB1);
  digitalWrite(MotorB1,0);
  analogWrite(MotorC1,MC1);
  digitalWrite(MotorC2,0);
  analogWrite(MotorD2,MD1);
  digitalWrite(MotorD1,0);
  
}void runrightward()//向右
{
   
  MAPID.Compute();                //PID计算，得出输出PMW
  MBPID.Compute();
  MCPID.Compute();
  MDPID.Compute();
  analogWrite(MotorA2,MA1);
  digitalWrite(MotorA1,0);
  analogWrite(MotorB1,MB1);
  digitalWrite(MotorB2,0);
  analogWrite(MotorC2,MC1);
  digitalWrite(MotorC1,0);
  analogWrite(MotorD1,MD1);
  digitalWrite(MotorD2,0);
  
}

void Speed()
{
GetMASpeed();
GetMBSpeed();
GetMCSpeed();
GetMDSpeed();
}
void GetMASpeed()   //获取A转速
{
Va=counter_valA;               
counter_valA=0;  
}
void GetMBSpeed()   //获取B转速
{
Vb=counter_valB;
counter_valB=0;  
}
void GetMCSpeed()   //获取C转速
{
Vc=counter_valC;
counter_valC=0;  
}
void GetMDSpeed()   //获取D转速
{
Vd=counter_valD;
counter_valD=0;  
}
//外部中断处理函数
void counterA()
{
     counter_valA++;    //每一个中断加一
}
void counterB()
{
     counter_valB++;    //每一个中断加一
}
void counterC()
{
     counter_valC++;    //每一个中断加一
}
void counterD()
{
     counter_valD++;    //每一个中断加一
}

wzzzq

BIRDA 发表于 2021-5-11 22:08
求助楼主大佬：
我写的程序是ref=30转速，使用蓝牙控制小车前进时，转速是30.然后让小车停下，第二次控制小 ...

程序看上去没有问题,可以去试一下在PID.compute()前先将MA1,MB1...清零

wzzzq

BIRDA 发表于 2021-5-11 22:08
求助楼主大佬：
我写的程序是ref=30转速，使用蓝牙控制小车前进时，转速是30.然后让小车停下，第二次控制小 ...

bool PID::Compute()
{
   if(!inAuto) return false;
   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
          double input = *myInput;
      double error = *mySetpoint - input;
      ITerm+= (ki * error);
      if(ITerm > outMax) ITerm= outMax;
      else if(ITerm < outMin) ITerm= outMin;
      double dInput = (input - lastInput);

      /*Compute PID Output*/
      double output = kp * error + ITerm- kd * dInput;

          if(output > outMax) output = outMax;
      else if(output < outMin) output = outMin;
          *myOutput = output;
          
      /*Remember some variables for next time*/
      lastInput = input;
      lastTime = now;
          return true;
   }
   else return false;
}

按照这个库的写法应该没什么问题,检查一些速度读取是否正确

BIRDA

wzzzq 发表于 2021-5-12 09:42
bool PID::Compute()
{
   if(!inAuto) return false;

感谢楼主答复。还是没办法调整，小车现在是可以以肉眼可见的速度增长
我的ref一直都是30，不知道为什么会自动往上叠加的

wzzzq

BIRDA 发表于 2021-5-12 12:33
感谢楼主答复。还是没办法调整，小车现在是可以以肉眼可见的速度增长
我的ref一直都是30，不知道 ...

速度读取正常吗,能不能发一下具体信息和视频到邮箱2936385857@qq.com