Page 1 of 1

esp32做wifi平衡小车

Posted: Tue Sep 03, 2019 1:06 pm
by happyday7
我在用ESP32-WROOM-32D做无线控制平衡小车的项目。用的是esp32-arduino。tb6612,mpu6050,370编码直流减速电机。
我想用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();
}

Re: esp32做wifi平衡小车

Posted: Wed Sep 04, 2019 2:39 am
by ESP_Alvin
Hi, happyday7,

您发的帖子中未见原理图,原理图能重发下么?谢谢。

Alvin

Re: esp32做wifi平衡小车

Posted: Wed Sep 04, 2019 4:04 am
by happyday7
好的,原理图在这

Re: esp32做wifi平衡小车

Posted: Wed Sep 04, 2019 4:06 am
by happyday7
我又尝试了一下。现在连usb供电都不可以了。是我程序有问题还是电源的问题呢?

Re: esp32做wifi平衡小车

Posted: Wed Sep 04, 2019 4:54 am
by happyday7
我把程序中的外部中断给注释了,然后直接用电源供电wifi也正常了,但是编码器就没有用到了。这是什么问题?是wifi的程序和外部中断的程序会冲突吗?