用esp32做平衡小车

星空里遗忘昨天 · 发表于 2019-9-3 18:23

我在用ESP32-WROOM-32D做无线控制平衡小车的项目。
我想用blinker去控制平衡小车的运动，我在平衡小车可以完成自平衡之后，加入blinker程序，但是当我在程序中加入blinker的程序之后就出现了问题。当我用外部电源供电的时候小车不会自平衡了，wifi连接上之后过一下子就断开了，小车也不能完成自平衡。我尝试过用充电宝usb线给小车供电，其他元件用外部电源供电。小车就可以实现wifi控制，而且可以完成自平衡，这是什么问题？
下面给出原理图和程序，希望有大佬指导一下。
程序
#define BLINKER_PRINT Serial
#define BLINKER_WIFI
#include <Blinker.h>
#include <MPU6050_tockn.h>
#include <Ticker.h>
#include <analogWrite.h>
#include <Arduino.h>
#include <Wire.h>
#define IN1 25
#define IN3 14
#define IN2 33
#define IN4 27
#define PWMA 32
#define PWMB 12
#define STBY 26
#define ENCODER_L 19  //编码器采集引脚每路2个共4个
#define DIRECTION_L 18
#define ENCODER_R 17
#define DIRECTION_R 5
#define ZHONGZHI 0//小车的机械中值
#define DIFFERENCE 2
typedef unsigned short U16;
//blinker
char auth[] = "f3e8430daf19";
char ssid[] = "Smartisan personal hotspot";
char pswd[] = "1234567891";
BlinkerButton Button1("qian");
BlinkerButton Button2("hou");
BlinkerButton Button3("left");
BlinkerButton Button4("right");
BlinkerButton Button5("stop");

unsigned char Flag_Stop = 1;//停止标志位
//外部中断
const byte interruptPin1 = 19;
const byte interruptPin2 = 17;
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
int Angle;//角度显示
volatile long Velocity_L, Velocity_R = 0; //左右轮编码器数据
int Velocity_Left, Velocity_Right = 0;  //左右轮速度
int Flag_Qian, Flag_Hou, Flag_Left, Flag_Right=0; //遥控相关变量
int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
int Motor1, Motor2;
//PD参数与PI参数
float Balance_Kp=30,Balance_Kd=0.4;
float Velocity_Kp=-3.5,Velocity_Ki=Velocity_Kp/200;

MPU6050 mpu6050(Wire);

//定时中断
Ticker ticker1;
////检测小车是否被拿起
//int Pick_Up(float Acceleration, float Angle, int encoder_left, int encoder_right){
//  static unsigned int flag, count0, count1, count2 ;
//  if (flag == 0) //第一步
//  {
// if (abs(encoder_left) + abs(encoder_right) < 15)       count0++;  //条件1，小车接近静止
// else    count0 = 0;
// if (count0 > 10)    flag = 1, count0 = 0;
//  }
//  if (flag == 1) //进入第二步
//  {
// if (++count1 > 400)    count1 = 0, flag = 0;                      //超时不再等待2000ms
// if (Acceleration > 1 && (Angle > (-14 + ZHONGZHI)) && (Angle < (14 + ZHONGZHI)))  flag = 2; //条件2，小车是在0度附近被拿起
//  }
//  if (flag == 2)  //第三步
//  {
// if (++count2 > 200)    count2 = 0, flag = 0;    //超时不再等待1000ms
// if (abs(encoder_left + encoder_right) > 200)          //条件3，小车的轮胎因为正反馈达到最大的转速
//    {
//       flag = 0;  return 1;
//    }
//  }
//  return 0;
//}
////检测小车是否被放下
//int Put_Down(float Angle, int encoder_left, int encoder_right){
//  static U16 flag, count ;
//  if (Flag_Stop == 0)       return 0;                //防止误检
//  if (flag == 0)
//  {
// if (Angle > (-10 + ZHONGZHI) && Angle < (10 + ZHONGZHI) && encoder_left == 0 && encoder_right == 0)    flag = 1; //条件1，小车是在0度附近的
//  }
//  if (flag == 1)
//  {
// if (++count > 100)    count = 0, flag = 0;  //超时不再等待 500ms
// if (encoder_left > 12 && encoder_right > 12 && encoder_left < 80 && encoder_right < 80) //条件2，小车的轮胎在未上电的时候被人为转动
// {
//    flag = 0;
//    flag = 0;
//    return 1; //检测到小车被放下
// }
//  }
//  return 0;
//}
////异常关闭电机
//unsigned char Turn_Off(float angle)
//{
//  unsigned char temp;
//  if (angle < -40 || angle > 40 || Flag_Stop == 1 )//===倾角大于40度关闭电机//===Flag_Stop置1关闭电机
//  {
// temp = 1;
// analogWrite(PWMA, 0);  //PWM输出为0
// analogWrite(PWMB, 0); //PWM输出为0;
//  }
//  else temp = 0; //不存在异常，返回0
//  return temp;
//}
// 按下按键即会执行该函数
void button1_callback(const String & state) {
   if (state=="press") {key_parse(1);}
   else if(state=="pressup"){key_parse(5);}
}
void button2_callback(const String & state) {
   if (state=="press") {key_parse(2);}
   else if(state=="pressup"){key_parse(5);}
}
void button3_callback(const String & state) {
   if (state=="press") {key_parse(3);}
   else if(state=="pressup"){key_parse(5);}
}
void button4_callback(const String & state) {
   if (state=="press") {key_parse(4);}
   else if(state=="pressup"){key_parse(5);}
}
void button5_callback(const String & state) {
// BLINKER_LOG("get button state: ", state);
   if (state=="on") {key_parse(5);}
}
void key_parse(uint8_t car_mode)
{
            switch (car_mode) {
               case 1: Flag_Qian = 1, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0; break;             //前
               case 2: Flag_Qian = 0, Flag_Hou = 1, Flag_Left = 0, Flag_Right = 0; break;             //后
               case 3: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break;             //左
               case 4: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break;             //右
               case 5: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0; break;
               default: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0; break;
            }

}
//转向控制
int turn(float gyro)//转向控制
{
  static float Turn_Target, Turn, Turn_Convert = 3;
  float Turn_Amplitude = 80, Kp = 2, Kd = -0.5;//2,0.5;  //PD参数
  if(1 == Flag_Left  )          Turn_Target += Turn_Convert;  //根据遥控指令改变转向偏差
  else if (1 == Flag_Right)    Turn_Target -= Turn_Convert;//根据遥控指令改变转向偏差
  else Turn_Target = 0;
  if (Turn_Target > Turn_Amplitude)  Turn_Target = Turn_Amplitude; //===转向速度限幅
  if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;
  Turn = -Turn_Target * Kp + gyro * Kd;       //===结合Z轴陀螺仪进行PD控制
  return Turn;
}
//读取编码器数据
void READ_ENCODER_L() {
  if (digitalRead(ENCODER_L) == LOW) {    //如果是下降沿触发的中断
if (digitalRead(DIRECTION_L) == LOW)    Velocity_L--;  //根据另外一相电平判定方向
else    Velocity_L++;
  }
  else {    //如果是上升沿触发的中断
if (digitalRead(DIRECTION_L) == LOW)    Velocity_L++; //根据另外一相电平判定方向
else    Velocity_L--;
  }

}
void READ_ENCODER_R() {
  if (digitalRead(ENCODER_R) == LOW) { //如果是下降沿触发的中断
if (digitalRead(DIRECTION_R) == LOW)    Velocity_R--;//根据另外一相电平判定方向
else    Velocity_R++;
  }
  else { //如果是上升沿触发的中断
if (digitalRead(DIRECTION_R) == LOW)    Velocity_R++; //根据另外一相电平判定方向
else    Velocity_R--;
  }
}
//PI速度环函数
  int velocity(int encoder_left, int encoder_right){
  static float Velocity, Encoder_Least, Encoder, Movement;
  static float Encoder_Integral, Target_Velocity;
  if    ( 1 == Flag_Qian)Movement = 1600;       //遥控
  else if ( 1 == Flag_Hou)Movement = -1600;
  else //这里是停止的时候反转，让小车尽快停下来
  {
Movement = 0;
if (Encoder_Integral > 300) Encoder_Integral -= 200;
if (Encoder_Integral < -300)  Encoder_Integral += 200; //遥控
  Encoder_Least = (encoder_left + encoder_right) - 0;             //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
  Encoder *= 0.7;                                                 //===一阶低通滤波器
  Encoder += Encoder_Least * 0.3;                                  //===一阶低通滤波器
  Encoder_Integral += Encoder;                                     //===积分出位移积分时间：40ms
  Encoder_Integral = Encoder_Integral - Movement;                //===接收遥控器数据，控制前进后退
  if (Encoder_Integral > 21000) Encoder_Integral = 21000;       //===积分限幅
  if (Encoder_Integral < -21000) Encoder_Integral = -21000;       //===积分限幅
  Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;                //===速度控制
  //if (Turn_Off(mpu6050.getAngleY()) == 1 || Flag_Stop == 1) Encoder_Integral = 0;//小车停止的时候积分清零
  return Velocity;
  }
  }

//中断函数
void control() {
static int Velocity_Count,Turn_Count;
sei();
mpu6050.update();
Angle = mpu6050.getAngleX();
Balance_Pwm = balance(mpu6050.getAngleX(), mpu6050.getGyroX());
if (++Velocity_Count >= 8) //速度控制，控制周期40ms
{
Velocity_Left = Velocity_L; Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
Velocity_Right = Velocity_R; Velocity_R = 0; //读取右轮编码器数据，并清零
Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制，控制周期40ms
Velocity_Count = 0;
}
if (++Turn_Count >= 4)//转向控制，控制周期20ms
{
Turn_Pwm = turn(mpu6050.getGyroZ());
Turn_Count = 0;
}
Motor1 = Balance_Pwm-Velocity_Pwm+ Turn_Pwm;
Motor2 = Balance_Pwm-Velocity_Pwm- Turn_Pwm;
Xianfu_Pwm();
//if (Pick_Up(mpu6050.getAccZ(),mpu6050.getAngleY() , Velocity_Left, Velocity_Right)) Flag_Stop = 1;  //===如果被拿起就关闭电机//===检查是否小车被那起
//if (Put_Down(mpu6050.getAngleY(), Velocity_Left, Velocity_Right))    Flag_Stop = 0;             //===检查是否小车被放下
//if (Turn_Off(mpu6050.getAngleY()) == 0)
Set_Pwm(Motor1, Motor2);//如果不存在异常，赋值给PWM寄存器控制电机
}
//设置PWM函数
void Set_Pwm(int moto1,int moto2)
{
  if (moto1 > 0)    digitalWrite(IN1, LOW),    digitalWrite(IN2, HIGH);  //TB6612的电平控制
  else             digitalWrite(IN1, HIGH),    digitalWrite(IN2, LOW);//TB6612的电平控制
  analogWrite(PWMA, abs(moto1)); //赋值给PWM寄存器
  if (moto2 < 0) digitalWrite(IN3, HIGH),    digitalWrite(IN4, LOW); //TB6612的电平控制
  else       digitalWrite(IN3, LOW),    digitalWrite(IN4, HIGH); //TB6612的电平控制
  analogWrite(PWMB, abs(moto2));//赋值给PWM寄存器
}
//PD函数
int balance(float Angle, float Gyro)
{
  float Bias;
  int balance;
  Bias = Angle - 0; //===求出平衡的角度中值和机械相关
  balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===计算平衡控制的电机PWM  PD控制 kp是P系数 kd是D系数
  return balance;
}
//限制PWM
void Xianfu_Pwm(void)
{
  int Amplitude = 250;  //===PWM满幅是255 限制在250
  if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。
  if(Flag_Hou==1) Motor2-=DIFFERENCE-2;
  if (Motor1 < -Amplitude) Motor1 = -Amplitude;
  if (Motor1 > Amplitude)  Motor1 = Amplitude;
  if (Motor2 < -Amplitude) Motor2 = -Amplitude;
  if (Motor2 > Amplitude)  Motor2 = Amplitude;
}
void setup() {
  //电机引脚设定
  pinMode(IN1, OUTPUT);       //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
  pinMode(IN2, OUTPUT);       //TB6612控制引脚，
  pinMode(IN3, OUTPUT);       //TB6612控制引脚，控制电机2的方向，01为正转，10为反转
  pinMode(IN4, OUTPUT);       //TB6612控制引脚，
  pinMode(PWMA, OUTPUT);       //TB6612控制引脚，电机PWM
  pinMode(PWMB, OUTPUT);       //TB6612控制引脚，电机PWM
  pinMode(STBY, OUTPUT);
  digitalWrite(IN1, 0);       //TB6612控制引脚拉低
  digitalWrite(IN2, 0);       //TB6612控制引脚拉低
  digitalWrite(IN3, 0);       //TB6612控制引脚拉低
  digitalWrite(IN4, 0);       //TB6612控制引脚拉低
  analogWrite(PWMA, 0);       //TB6612控制引脚拉低
  analogWrite(PWMB, 0);       //TB6612控制引脚拉低
  digitalWrite(STBY, 1);
  pinMode(19, INPUT_PULLUP);    //编码器引脚A
  pinMode(18, INPUT);    //编码器引脚B
  pinMode(17, INPUT_PULLUP);    //编码器引脚A
  pinMode(5, INPUT);    //编码器引脚B
  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(3000);
  //MPU
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  delay(20);
// 初始化blinker
  BLINKER_DEBUG.stream(Serial);
  Blinker.begin(auth, ssid, pswd);
  Button1.attach(button1_callback);
  Button2.attach(button2_callback);
  Button3.attach(button3_callback);
  Button4.attach(button4_callback);
  Button5.attach(button5_callback);
  Flag_Qian=0; Flag_Hou=0; Flag_Left=0; Flag_Right=0;
  Blinker.delay(20);
  //中断
  ticker1.attach(0.005, control);
  attachInterrupt(digitalPinToInterrupt(interruptPin1), READ_ENCODER_L , CHANGE);
  attachInterrupt(digitalPinToInterrupt(interruptPin2), READ_ENCODER_R , CHANGE);
}

void loop() {
   Blinker.run();
}

coloz · 发表于 2019-9-3 19:53

从文字描述看，只是供电问题，大功率的设备都需要独立供电。

星空里遗忘昨天 · 发表于 2019-9-3 20:14

我尝试过用3A的LM2596稳压模块去给其他元器件供电，用AMS1117稳压模块给板子供电，用的是同一个电源，但是结果还是一样的。我用的元器件没有啥大功率的吧，用的370直流减速电机带编码器，TB6612驱动模块，MPU6050，如果是大功率的问题的话应该在没有加入blinker的时候也应该不能完成平衡吧，我没加入的时候用12V电源供电是可以完成平衡的。是开启wifi功能之后板子的功率需要大一点吗？

星空里遗忘昨天 · 发表于 2019-9-4 09:45

我现在用usb个开发板供电也不行了。有大佬指导一下什么问题嘛？

搞了好久试了很多方法都不行。

coloz · 发表于 2019-9-4 09:55

星空里遗忘昨天发表于 2019-9-4 09:45
我现在用usb个开发板供电也不行了。有大佬指导一下什么问题嘛？搞了好久试了很多方法都不行。 ...

USB2.0供电，只有5V 500ma，通常一个普通的电机就需要1A的电流。这个真没啥好指导的，用更好的电源或者电池即可。如果需要参数，就要自己根据自己的设备算了。这些问题都和blinker没关系了。

星空里遗忘昨天 · 发表于 2019-9-4 12:03

好的。谢谢。

星空里遗忘昨天 · 发表于 2019-9-4 12:33

我最后试了一下，现在就用上面这个程序，我用usb供电也不行了。然后我把程序中的外部中断给注释了，然后直接用电源供电wifi也正常了，但是编码器就没有用到了。这是什么问题？是wifi的程序和外部中断的程序会冲突吗？

		自动登录	找回密码
密码			立即注册

[已解答] 用esp32做平衡小车