你的，桌面自平衡 机器人—Zz
本帖最后由 Mingming.Zhang 于 2018-7-19 10:28 编辑如 何 拥 有 一 台 足 够 把 玩 与 学 习 的 机 器 人 ？
它 集 2 轮 驱 动 、自 平 衡，满 足 机 器 人 的 行 动；
灰 度 巡 线、超 声 波 避 障、oled128x64 的 图 形 显 示；
mpu-6050 模 块 三 轴 加 速 度 + 三 轴 陀 螺 仪，重 心 和 倾 斜 角 测 量；
蓝 牙 无 线 连 接 操 控；当 然 你 还 可 以 附 加 更 多 乐 趣 ，语 音 交 互、 微 型 摄 像 头 配 合 vr 头 盔（fpv 第 一 视 角 驾 驶）
小 小 的 身 躯 —— 清 新 不 油 腻
材料清单 ：
————————————————.n20带编码器金属齿轮减速电机V2 (30:1).miniQ小车橡胶轮.3PI MiniQ N20电机支架.FireBeetle原型扩展板.TB6612FNG微型双路直流电机驱动模块.DFRduino Pro Mini V1.2(16M5V328).3.7锂电池
.3v转5v升压模块
.锂电池充放电模块
———————————————————附加与扩展硬件
.HC-SR04+超声波测距模块
.灰度巡线传感器
硬件篇 Board Power Supply 3.35 -12 V (3.3V model) or 5 - 12 V (5V model)Circuit Operating Voltage 3.3V or 5V (depending on model)
8 MHz (3.3V versions) or 16 MHz (5V versions) Serial: 0 (RX) and 1 (TX). Used to receive (RX) and transmit (TX) TTL serial data. These pins are connected to the TX-0 and RX-1 pins of the six pin header. External Interrupts: 2 and 3. These pins can be configured to trigger an interrupt on a low value, a rising or falling edge, or a change in value. See the attachInterrupt function for details. PWM: 3, 5, 6, 9, 10, and 11. Provide 8-bit PWM output with the analogWrite function. SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK). These pins support SPI communication, which, although provided by the underlying hardware, is not currently included in the Arduino language.
pro mini作为一款非常小巧的微控制器，大体了解到它的特性（蓝色字体标记）以及工作电压5v（供电是一个项目特别要考虑的指标）
Zz的底盘是一块洞洞板，N20电机的输出轴露出洞洞板外（左右对称）；
烧录进代码：
#include "./PinChangeInt.h"
#include "./MsTimer2.h"
//利用测速码盘计数实现速度PID控制
#include "./BalanceCar.h"
#include "./KalmanFilter.h"
//I2Cdev、MPU6050和PID_v1类库需要事先安装在Arduino 类库文件夹下
#include "./I2Cdev.h"
#include "./MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu; //实例化一个 MPU6050 对象，对象名称为 mpu
BalanceCar balancecar;
KalmanFilter kalmanfilter;
int16_t ax, ay, az, gx, gy, gz;
//驱动模块控制信号
#define IN1M A3
#define IN2M 7
#define IN3M A0
#define IN4M 8
#define PWMA A2
#define PWMB A1
#define STBY 6
#define PinA_left 3//中断0
#define PinA_right 2 //中断1
//声明自定义变量
int time;
byte inByte; //串口接收字节
int num;
double Setpoint; //角度DIP设定点，输入，输出
double Setpoints, Outputs = 0; //速度DIP设定点，输入，输出
double kp = 40, ki = 0.1, kd = 0.3; //需要你修改的参数
double kp_speed =1.8, ki_speed = 0.05, kd_speed = 0; // 需要你修改的参数
double kp_turn = 0, ki_turn = 0, kd_turn = 0; //旋转PID设定
const double PID_Original = {38, 0.0, 0.58,4.0, 0.12, 0.0}; //恢复默认PID参数
//转向PID参数
double setp0 = 0, dpwm = 0, dl = 0; //角度平衡点，PWM差，死区，PWM1，PWM2
float value;
//********************angle data*********************//
float Q;
float Angle_ax; //由加速度计算的倾斜角度
float Angle_ay;
float K1 = 0.05; // 对加速度计取值的权重
float angle0 = 0.00; //机械平衡角
int slong;
//********************angle data*********************//
//***************Kalman_Filter*********************//
float Q_angle = 0.001, Q_gyro = 0.005; //角度数据置信度,角速度数据置信度
float R_angle = 0.5 , C_0 = 1;
float timeChange = 5; //滤波法采样时间间隔毫秒
float dt = timeChange * 0.001; //注意：dt的取值为滤波器采样时间
//***************Kalman_Filter*********************//
//*********************************************
//******************** speed count ************
//*********************************************
volatile long count_right = 0;//使用volatile lon类型是为了外部中断脉冲计数值在其他函数中使用时，确保数值有效
volatile long count_left = 0;//使用volatile lon类型是为了外部中断脉冲计数值在其他函数中使用时，确保数值有效
int speedcc = 0;
//////////////////////脉冲计算/////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int sumam;
/////////////////////脉冲计算////////////////////////////
//////////////转向、旋转参数///////////////////////////////
int turncount = 0; //转向介入时间计算
float turnoutput = 0;
//////////////转向、旋转参数///////////////////////////////
//////////////Wifi控制量///////////////////
#define run_car '1'//按键前
#define back_car '2'//按键后
#define left_car '3'//按键左
#define right_car '4'//按键右
#define stop_car '0'//按键停
/*小车运行状态枚举*/
enum {
enSTOP = 0,
enRUN,
enBACK,
enLEFT,
enRIGHT,
enTLEFT,
enTRIGHT
} enCarState;
int incomingByte = 0; // 接收到的 data byte
String inputString = ""; // 用来储存接收到的内容
boolean newLineReceived = false; // 前一次数据结束标志
boolean startBit= false;//协议开始标志
int g_carstate = enSTOP; //1前2后3左4右0停止
String returntemp = ""; //存储返回值
boolean g_autoup = false;
int g_uptimes = 5000;
int front = 0;//前进变量
int back = 0;//后退变量
int turnl = 0;//左转标志
int turnr = 0;//右转标志
int spinl = 0;//左旋转标志
int spinr = 0;//右旋转标志
int bluetoothvalue;//蓝牙控制量
//////////////蓝牙控制量///////////////////
//////////////////超声波速度//////////////////
int chaoshengbo = 0;
int tingzhi = 0;
int jishi = 0;
//////////////////超声波速度//////////////////
void Code_left();
void Code_right();
//////////////////////脉冲计算///////////////////////
void countpluse()
{
lz = count_left;
rz = count_right;
count_left = 0;
count_right = 0;
lpluse = lz;
rpluse = rz;
if ((balancecar.pwm1 < 0) && (balancecar.pwm2 < 0)) //小车运动方向判断 后退时（PWM即电机电压为负） 脉冲数为负数
{
rpluse = -rpluse;
lpluse = -lpluse;
}
else if ((balancecar.pwm1 > 0) && (balancecar.pwm2 > 0)) //小车运动方向判断 前进时（PWM即电机电压为正） 脉冲数为负数
{
rpluse = rpluse;
lpluse = lpluse;
}
else if ((balancecar.pwm1 < 0) && (balancecar.pwm2 > 0)) //小车运动方向判断 前进时（PWM即电机电压为正） 脉冲数为负数
{
rpluse = rpluse;
lpluse = -lpluse;
}
else if ((balancecar.pwm1 > 0) && (balancecar.pwm2 < 0)) //小车运动方向判断 左旋转 右脉冲数为负数 左脉冲数为正数
{
rpluse = -rpluse;
lpluse = lpluse;
}
//提起判断
balancecar.stopr += rpluse;
balancecar.stopl += lpluse;
//每5ms进入中断时，脉冲数叠加
balancecar.pulseright += rpluse;
balancecar.pulseleft += lpluse;
sumam = balancecar.pulseright + balancecar.pulseleft;
}
////////////////////脉冲计算///////////////////////
//////////////////角度PD////////////////////
void angleout()
{
balancecar.angleoutput = kp * (kalmanfilter.angle + angle0) + kd * kalmanfilter.Gyro_x;//PD 角度环控制
}
//////////////////角度PD////////////////////
//////////////////////////////////////////////////////////
//////////////////中断定时 5ms定时中断////////////////////
/////////////////////////////////////////////////////////
void inter()
{
sei();
countpluse(); //脉冲叠加子函数
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //IIC获取MPU6050六轴数据 ax ay az gx gy gz
kalmanfilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1); //获取angle 角度和卡曼滤波
angleout(); //角度环 PD控制
speedcc++;
if (speedcc >= 8) //40ms进入速度环控制
{
Outputs = balancecar.speedpiout(kp_speed, ki_speed, kd_speed, front, back, setp0);
speedcc = 0;
}
turncount++;
if (turncount > 4) //40ms进入旋转控制
{
turnoutput = balancecar.turnspin(turnl, turnr, spinl, spinr, kp_turn, kd_turn, kalmanfilter.Gyro_z); //旋转子函数
turncount = 0;
}
balancecar.posture++;
balancecar.pwma(Outputs, turnoutput, kalmanfilter.angle, kalmanfilter.angle6, turnl, turnr, spinl, spinr, front, back, kalmanfilter.accelz, IN1M, IN2M, IN3M, IN4M, PWMA, PWMB); //小车总PWM输出
}
//////////////////////////////////////////////////////////
//////////////////中断定时 5ms定时中断///////////////////
/////////////////////////////////////////////////////////
void SendAutoUp()
{
g_uptimes --;
if ((g_autoup == true) && (g_uptimes == 0))
{
//自动上报
String CSB, VT;
char temp={0};
float fgx;
float fay;
float leftspeed;
float rightspeed;
fgx = gx;//传给局部变量
fay = ay;//传给局部变量
leftspeed = balancecar.pwm1;
rightspeed = balancecar.pwm2;
double Gx = (double)((fgx - 128.1f) / 131.0f); //角度转换
double Ay = ((double)fay / 16384.0f) * 9.8f;
if(leftspeed > 255 || leftspeed < -255)
return;
if(rightspeed > 255 || rightspeed < -255)
return;
if((Ay < -20) || (Ay > 20))
return;
if((Gx < -3000) || (Gx > 3000))
return;
returntemp = "";
memset(temp, 0x00, sizeof(temp));
//sprintf(temp, "%3.1f", leftspeed);
dtostrf(leftspeed, 3, 1, temp);// 相當於 %3.2f
String LV = temp;
memset(temp, 0x00, sizeof(temp));
//sprintf(temp, "%3.1f", rightspeed);
dtostrf(rightspeed, 3, 1, temp);// 相當於 %3.1f
String RV = temp;
memset(temp, 0x00, sizeof(temp));
//sprintf(temp, "%2.2f", Ay);
dtostrf(Ay, 2, 2, temp);// 相當於 %2.2f
String AC = temp;
memset(temp, 0x00, sizeof(temp));
//sprintf(temp, "%4.2f", Gx);
dtostrf(Gx, 4, 2, temp);// 相當於 %4.2f
String GY = temp;
CSB = "0.00";
VT = "0.00";
//AC =
returntemp = "$LV" + LV + ",RV" + RV + ",AC" + AC + ",GY" + GY + ",CSB" + CSB + ",VT" + VT + "#";
Serial.print(returntemp); //返回协议数据包
}
if (g_uptimes == 0)
g_uptimes = 5000;
}
// === 初始设置 ===
void setup() {
// TB6612FNGN驱动模块控制信号初始化
pinMode(IN1M, OUTPUT); //控制电机1的方向，01为正转，10为反转
pinMode(IN2M, OUTPUT);
pinMode(IN3M, OUTPUT); //控制电机2的方向，01为正转，10为反转
pinMode(IN4M, OUTPUT);
pinMode(PWMA, OUTPUT); //左电机PWM
pinMode(PWMB, OUTPUT); //右电机PWM
pinMode(STBY, OUTPUT); //TB6612FNG使能
//初始化电机驱动模块
digitalWrite(IN1M, 0);
digitalWrite(IN2M, 1);
digitalWrite(IN3M, 1);
digitalWrite(IN4M, 0);
digitalWrite(STBY, 1);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
pinMode(PinA_left, INPUT);//测速码盘输入
pinMode(PinA_right, INPUT);
// 加入I2C总线
Wire.begin(); //加入 I2C 总线序列
Serial.begin(9600); //开启串口，设置波特率为 115200
delay(1500);
mpu.initialize(); //初始化MPU6050
delay(2);
balancecar.pwm1 = 0;
balancecar.pwm2 = 0;
//5ms定时中断设置使用timer2 注意：使用timer2会对pin3 pin11的PWM输出有影响，因为PWM使用的是定时器控制占空比，所以在使用timer的时候要注意查看对应timer的pin口。
MsTimer2::set(5, inter);
MsTimer2::start();
}
////////////////////////////////////////turn//////////////////////////////////
void ResetPID()
{
kp = PID_Original;
ki =PID_Original;
kd =PID_Original; //需要你修改的参数
kp_speed =PID_Original;
ki_speed =PID_Original;
kd_speed =PID_Original;// 需要你修改的参数
}
void ResetCarState()
{
turnl = 0;
turnr = 0;
front = 0;
back = 0;
spinl = 0;
spinr = 0;
turnoutput = 0;
}
// === 主循环程序体 ===
void loop() {
String returnstr = "$0,0,0,0,0,0,0,0,0,0,0,0cm,8.2V#";//默认发送
//主函数中循环检测及叠加脉冲 测定小车车速使用电平改变既进入脉冲叠加 增加电机的脉冲数，保证小车的精确度。
attachInterrupt(0, Code_left, CHANGE);
attachPinChangeInterrupt(PinA_right, Code_right, CHANGE);
//kongzhi(); //蓝牙接口
//
//Serial.println(kalmanfilter.angle);
//Serial.print("\t");
//Serial.print(bluetoothvalue);
//Serial.print("\t");
// Serial.print( balancecar.angleoutput);
// Serial.print("\t");
// Serial.print(balancecar.pwm1);
// Serial.print("\t");
// Serial.println(balancecar.pwm2);
// Serial.print("\t");
// Serial.println(balancecar.stopr);
// Serial.print("\t");
if (newLineReceived)
{
switch (inputString)
{
case run_car: g_carstate = enRUN; break;
case back_car:g_carstate = enBACK;break;
case left_car:g_carstate = enLEFT;break;
case right_car: g_carstate = enRIGHT; break;
case stop_car:g_carstate = enSTOP;break;
default: g_carstate = enSTOP; break;
}
//判断协议是否有丢包
/* if(inputString.length() < 21)
{
//恢复默认
inputString = ""; // clear the string
newLineReceived = false;
//Serial.print(returnstr);
goto a;
}*/
if (inputString == '1' && inputString.length() == 21) //左摇
{
g_carstate = enTLEFT;
//Serial.print(returnstr);
}
else if (inputString == '2' && inputString.length() == 21) //右摇
{
g_carstate = enTRIGHT;
// Serial.print(returnstr);
}
if (inputString == '1') //查询PID
{
char charkp, charkd, charkpspeed, charkispeed;
dtostrf(kp, 3, 2, charkp);// 相當於 %3.2f
dtostrf(kd, 3, 2, charkd);// 相當於 %3.2f
dtostrf(kp_speed, 3, 2, charkpspeed);// 相當於 %3.2f
dtostrf(ki_speed, 3, 2, charkispeed);// 相當於 %3.2f
String strkp = charkp; String strkd = charkd; String strkpspeed = charkpspeed; String strkispeed = charkispeed;
returntemp = "$0,0,0,0,0,0,AP" + strkp + ",AD" + strkd + ",VP" + strkpspeed + ",VI" + strkispeed + "#";
Serial.print(returntemp); //返回协议数据包
}
else if (inputString == '2') //恢复PID
{
ResetPID();
Serial.print("$OK#"); //返回协议数据包
}
if (inputString == '1') //自动上报
{
g_autoup = true;
Serial.print("$OK#"); //返回协议数据包
}
else if (inputString == '2') //停止自动上报
{
g_autoup = false;
Serial.print("$OK#"); //返回协议数据包
}
if (inputString == '1') //角度环更新 $0,0,0,0,1,1,AP23.54,AD85.45,VP10.78,VI0.26#
{
int i = inputString.indexOf("AP");
int ii = inputString.indexOf(",", i);
if(ii > i)
{
String m_skp = inputString.substring(i + 2, ii);
m_skp.replace(".", "");
int m_kp = m_skp.toInt();
kp = (double)( (double)m_kp / 100.0f);
}
i = inputString.indexOf("AD");
ii = inputString.indexOf(",", i);
if(ii > i)
{
//ki = inputString.substring(i+2, ii);
String m_skd = inputString.substring(i + 2, ii);
m_skd.replace(".", "");
int m_kd = m_skd.toInt();
kd = (double)( (double)m_kd / 100.0f);
}
Serial.print("$OK#"); //返回协议数据包
}
if (inputString == '1') //速度环更新
{
int i = inputString.indexOf("VP");
int ii = inputString.indexOf(",", i);
if(ii > i)
{
String m_svp = inputString.substring(i + 2, ii);
m_svp.replace(".", "");
int m_vp = m_svp.toInt();
kp_speed = (double)( (double)m_vp / 100.0f);
}
i = inputString.indexOf("VI");
ii = inputString.indexOf("#", i);
if(ii > i)
{
String m_svi = inputString.substring(i + 2, ii);
m_svi.replace(".", "");
int m_vi = m_svi.toInt();
ki_speed = (double)( (double)m_vi / 100.0f);
Serial.print("$OK#"); //返回协议数据包
}
}
//恢复默认
inputString = ""; // clear the string
newLineReceived = false;
}
a: switch (g_carstate)
{
case enSTOP: turnl = 0; turnr = 0;front = 0; back = 0; spinl = 0; spinr = 0; turnoutput = 0; break;
case enRUN: ResetCarState();front = 250; break;
case enLEFT: turnl = 1; break;
case enRIGHT: turnr = 1; break;
case enBACK: ResetCarState();back = -250; break;
case enTLEFT: spinl = 1; break;
case enTRIGHT: spinr = 1; break;
default: front = 0; back = 0; turnl = 0; turnr = 0; spinl = 0; spinr = 0; turnoutput = 0; break;
}
//增加自动上报
SendAutoUp();
}
////////////////////////////////////////pwm///////////////////////////////////
//////////////////////////脉冲中断计算/////////////////////////////////////
void Code_left() {
count_left ++;
} //左测速码盘计数
void Code_right() {
count_right ++;
} //右测速码盘计数
//////////////////////////脉冲中断计算/////////////////////////////////////
//serialEvent()是IDE1.0及以后版本新增的功能，不清楚为什么大部份人不愿意用，这个可是相当于中断功能一样的啊!
int num1 = 0;
void serialEvent()
{
while (Serial.available())
{
incomingByte = Serial.read(); //一个字节一个字节地读，下一句是读到的放入字符串数组中组成一个完成的数据包
if (incomingByte == '$')
{
num1 = 0;
startBit = true;
}
if (startBit == true)
{
num1++;
inputString += (char) incomingByte; // 全双工串口可以不用在下面加延时，半双工则要加的//
}
if (startBit == true && incomingByte == '#')
{
newLineReceived = true;
startBit = false;
}
if(num1 >= 80)
{
num1 = 0;
startBit = false;
newLineReceived = false;
inputString = "";
}
}
}
/*备份*/
#if 0
char chartemp;
dtostrf(ax, 3, 2, chartemp);// 相當於 %3.2f
String strax = chartemp;
strax = "\nax:" + strax;
memset(chartemp, 0x00, 7);
dtostrf(ay, 3, 2, chartemp);// 相當於 %3.2f
String stray = chartemp;
stray = "\nay:" + stray;
memset(chartemp, 0x00, 7);
dtostrf(az, 3, 2, chartemp);// 相當於 %3.2f
String straz = chartemp;
straz = "\naz:" + straz;
memset(chartemp, 0x00, 7);
dtostrf(gx, 3, 2, chartemp);// 相當於 %3.2f
String strgx = chartemp;
strgx = "\ngx:" + strgx;
memset(chartemp, 0x00, 7);
dtostrf(gy, 3, 2, chartemp);// 相當於 %3.2f
String strgy = chartemp;
strgy = "\ngy:" + strgy;
memset(chartemp, 0x00, 7);
dtostrf(gz, 3, 2, chartemp);// 相當於 %3.2f
String strgz = chartemp;
strgz = "\ngz:" + strgz;
Serial.print(strax + stray + straz + strgx + strgy + strgz); //返回协议数据包
#endif
完成！https://v.youku.com/v_show/id_XMzczMTkxNjQ5Mg==.html?spm=a2hzp.8244740.0.0
由于摆放十分整齐好看,所以我看到了,
锂电池一块,
两个排针,
排线,
SR-04超声波
3.7v-5v升压模块
promini,
MPU6050
3.7v充电板
TB6612
两套N20带霍尔的电机,轮子. robo027 发表于 2018-6-10 15:14
由于摆放十分整齐好看,所以我看到了,
锂电池一块,
两个排针,
厉害了，我觉得可以来一场 看图识别硬件的比赛{:5_179:} Mingming.Zhang 发表于 2018-6-10 19:40
厉害了，我觉得可以来一场 看图识别硬件的比赛
怎么不继续写啊
robo027 发表于 2018-6-11 05:56
怎么不继续写啊
等等，在写{:5_153:} 可不可以把程序发一下
赵翔飞 发表于 2018-6-11 12:34
可不可以把程序发一下
可以呀， 会立马整理好上传的。敬请期待！ 摆好凳子，吃着西瓜等更新 期待出彩，不容错过{:5_148:} dsweiliang 发表于 2018-6-11 15:32
摆好凳子，吃着西瓜等更新
哈哈哈哈，谢谢支持。 看来我接下来要边吃西瓜边搞事 安卓机器人 发表于 2018-6-11 19:26
期待出彩，不容错过
感觉到了压力，我要好好写了{:5_182:} 快更新！吃瓜吃瓜，板凳已经坐烂了:lol luna 发表于 2018-6-12 09:48
快更新！吃瓜吃瓜，板凳已经坐烂了
{:5_139:} 感觉这一人个好玩的大工程。{:5_116:} 好棒，搞一个。。。。。。。。。。。 好玩。想要一个 好想要一个啊啊emmmm算过了还是自己做吧
帖子的颜值永远最高 平衡力好像不咋地呢
页:
[1]