A-A+

Matlab与MakeBlock:通过蓝牙控制电机

2016年07月07日 Other 暂无评论 阅读 4,027 次

本文主要介绍通过蓝牙控制Makeblock的电机的操作步奏,及可能遇到的问题。

起因:近期朋友送了一个Makeblock mbot Ranger(makeblock.com),让我想起了2009年刚入道时玩的arduino机器人。熟悉的Arduino界面,熟悉的setup()和loop(),如同老友重逢,倍感亲切,先来个开箱吧。

1

2

下面是我5年前做的Arduino机器人的一些视频,感兴趣可戳入。

1.自主避障:

http://v.youku.com/v_show/id_XMjI5MzAwOTQ0.html?from=s1.8-1-1.2

2.探测物体方位并跟踪运动(曲线运动)

3.用Processing做的PC控制调试端,增加了超声波可视化

看了下最新款的Makeblock Ranger(游侠),功能已经很强大了!同一个套件可以变身履带车、平衡车(神经鸟)和双轮车(迅猛龙),可以超声避障、自动巡线、加载了MPU6050惯性测量单元,可以实现自平衡等复杂的功能。大致看了下代码,非常新,基本上都是Mark Yan大神2016年新写的。跑了几个简单的例程。效果还是非常赞的!

1

新玩法:

Ranger可以用ArduinoIDE编程,可以用图像化开发环境,可以用手机APP控制。但新机型到了俺们这自然就要有新玩法喽!介于Matlab拥有强大的数据处理、可视化绘图能力以及众多成熟的算法函数,非常适合算法开发;在控制系统设计中,Simulink也是普遍使用的设计和仿真工具。其实呢,最主要的原因还是,笔者手头正拿着这个Matlab大锤,用得也比较6,所以看着啥都像钉子,囧!不过呢,Mark Yan提出了一个思路,可以读取Ranger的声音传感器的数据,在matlab里进行信号处理,模式识别啥的,那想象力空间就很大了!所以我决定先把蓝牙通信跑通!

2

喔对了,机器人能力是否强大,传感器是前提!Ranger配置列表如下:

3

4

系统配置

操作系统:windows10

Matlab2016a,Matlab2015b(其他版本未测试)

Makbloke:硬件型号ranger,

Arduino IDE版本1.6.9:加载 examples里的Firmware_for_Auriga这个例程,它包含了几乎所有的功能,蓝牙通信协议见附件(Auriga软件接口说明.docx)。

注意!在win10中无法使用Arduino IDE进行编译,已上报Arduino开发组,问题有待解决。大家可现在win7或Ubuntu下编译好arduino固件下载至ranger中,不要在win10中浪费时间折腾arduino!

5连接蓝牙

准备工作:如果你用的是windows操作系统,请在“开始”“设置”“设备” “蓝牙”选项里打开蓝牙并配对makeblock

6 7Matlab连接蓝牙设备

打开matlab,首先查看蓝牙属性

instrhwinfo('Bluetooth')

8

使用RemoteNames命令查看蓝牙设备列表.

ans.RemoteNames

9

查看makeblock属性.

instrhwinfo('Bluetooth',' Makeblock')

10

前面是查看设备相关属性。下一步我们建立蓝牙对象bt,就可以通过读写bt,实现与makeblock的通信啦!

bt = Bluetooth('Makeblock', 1)

11

打开设备.

fopen(bt)

发送直流电机指令

12 13

text数据可使用fscanf和fprintf函数

二进制数据可使用fread和fwrite函数

由于指令需要拼字节,我使用fwrite函数

电机1控制程序


function   y=control_motor_1(bt,speed)

% MakeBlock编码电机1控制

% 作者:Top Liu  exbot.net

% 2016.7.6

% bt为蓝牙对象

% speed为电机转速 -255-255  为负数表示为反向转动

%

if speed<0

direction=255;%direction为电机转动方向 0 为正向,255为反向

speed=255+speed;

else

direction=0;

end

 

fwrite(bt,hex2dec('ff'))

fwrite(bt,hex2dec('55'))

fwrite(bt,hex2dec('07'))

fwrite(bt,hex2dec('00'))

fwrite(bt,hex2dec('02'))

fwrite(bt,hex2dec('3d'))

fwrite(bt,hex2dec('00'))

fwrite(bt,hex2dec('01'))%01为电机1,02为电机2

fwrite(bt,speed)

fwrite(bt,direction)

y=speed;

 

end

 

发送指令就可以看到轮子愉快的转动了!

control_motor_1(bt,64) %正转

control_motor_1(bt,255) %全速

control_motor_1(bt,-64) %反转

control_motor_1(bt,0) %停车


通过计时器中断响应实现固定时间周期的控制

离散控制的前提是需要确定控制的时间周期,即两次控制指令之间的时间间隔。我们可通过定时器来实现。

建立计时器中断及回调函数代码如下:

主程序:


[Y,M,D,H,MN,S] = datevec(datestr(now,'mmmm dd, yyyy HH:MM:SS.FFF AM'));

start_time=MN*60+S;%记下开始时间

 

t = timer();

t.Period = 0.1;

t.TasksToExecute = 1200;

t.ExecutionMode = 'fixedRate';

t.StartFcn ={@callback_control_motor_1, bt, start_time};

t.StopFcn = {@callback_stop, bt};

t.TimerFcn = {@callback_control_motor_1, bt, start_time};

start(t)

 

% delete(t)


回调函数callback_control_motor_1


function   y=callback_control_motor_1(obj, event,bt,start_time)

% 计时器回调函数

% 作者:Top Liu  exbot.net

% 2016.7.6

% bt为蓝牙对象

% start_time为绘图用的初试时间

%

dt = datestr(now,'mmmm dd, yyyy HH:MM:SS.FFF AM');

[Y,M,D,H,MN,S] = datevec(dt);

 

% speed=255/60*S;

speed=round(255*sin(5*2*pi/60*S))

control_motor_1(bt,speed);

time=MN*60+S-start_time;

plot(time,speed,'--gs',...

'LineWidth',2,...

'MarkerSize',10,...

'MarkerEdgeColor','b',...

'MarkerFaceColor',[0.5,0.5,0.5])

hold on

y=speed;

end


回调函数callback_stop


function   y=callback_stop(obj, event,bt)

control_motor_1(bt,0);

end


为了便于观察,我在程序里增加了plot绘图语句,便于观察电机指令。

14

读取传感器反馈

15

下面以Ranger板载的MPU6050为例,读取姿态反馈,为闭环控制建立基础。

主程序:

%   bt = Bluetooth('Makeblock', 1)

%   fopen(bt)

flushinput(bt);%清空蓝牙接收数据的缓存

flushoutput(bt);

[Y,M,D,H,MN,S] = datevec(datestr(now,'mmmm dd, yyyy HH:MM:SS.FFF AM'));

start_time=MN*60+S;%记下开始时间

 

t = timer();

t.Period = 0.1;

t.TasksToExecute = 1200;

t.ExecutionMode = 'fixedRate';

t.StartFcn ={@callback_readMPU6050, bt, start_time};

t.StopFcn = {@callback_readMPU6050, bt, start_time};

t.TimerFcn = {@callback_readMPU6050, bt, start_time};

start(t)

% delete(t)


回调函数callback_readMPU6050


function   y=callback_readMPU6050(obj, event,bt,start_time)

% 计时器回调函数

% 作者:Top Liu  exbot.net

% 2016.7.6

% bt为蓝牙对象

% start_time为绘图用的初试时间

%

dt = datestr(now,'mmmm dd, yyyy HH:MM:SS.FFF AM');

[Y,M,D,H,MN,S] = datevec(dt);

 

angle=readMPU6050(bt,1)

time=MN*60+S-start_time;

plot(time,angle,'--gs',...

'LineWidth',1,...

'MarkerSize',2,...

'MarkerEdgeColor','b',...

'MarkerFaceColor',[0.5,0.5,0.5])

grid on

hold on

y=angle;

end


读取MPU6050姿态函数


function   angle=readMPU6050(bt,axe)

% 读取MPU6050姿态

% 作者:Top Liu  exbot.net

% 2016.7.6

% bt为蓝牙对象

% axe为读取姿态角:X轴(01)  Y轴(02)  Z轴(03) 例:01为俯仰角度,低头为正,单位“度”,经测试数据存在误差:

 

%% 发送指令

fwrite(bt,hex2dec('ff'))

fwrite(bt,hex2dec('55'))

fwrite(bt,hex2dec('05'))

fwrite(bt,hex2dec('00'))

fwrite(bt,hex2dec('01'))

fwrite(bt,hex2dec('06'))

fwrite(bt,hex2dec('01'))

fwrite(bt,axe)%俯仰角度,低头为正,单位“度”,经测试数据存在误差:X轴(01)  Y轴(02)  Z轴(03)

%% 读取返回数据,重新编码

A = fread(bt,10);

A5=dec2hex(A(5));

A6=dec2hex(A(6));

A7=dec2hex(A(7));

A8=dec2hex(A(8));

a=[A8,A7,A6,A5];%逆序后字符串拼接

angle=typecast(uint32(hex2dec(a)),'single');%hex转换成float


平衡车控制程序清单

其实本想在matlab试一下闭环控制的,但最近事情比较多,加上编辑催稿了,就先把这篇发出来。

下面是Firmware_for_Auriga里的与平衡车相关的源代码,一个增益很大的PID控制,感兴趣的可自行调试下。

首先进入balanced_model(作者命名似乎有笔误,应该是mode吧)

else if(auriga_mode == BALANCED_MODE)

{

balanced_model();

}

balanced_model函数定义

内环是一个姿态控制,即保证平衡,但实际上由于控制器的P很大,经常会振荡,因而称为“神经鸟”模式。

外环是一个速度控制,可控制平衡车前进。


void balanced_model(void)

{

reset();

if(start_flag == true)

{

if((millis() - lasttime_angle) > 10)

{

PID_angle_compute();

lasttime_angle = millis();

}

if((millis() - lasttime_speed) > 100)

{

PID_speed_compute();

last_turn_setpoint_filter  = last_turn_setpoint_filter * 0.8;

last_turn_setpoint_filter  += PID_turn.Setpoint * 0.2;

PID_turn.Output = last_turn_setpoint_filter;

lasttime_speed = millis();

}

}

else

{

Encoder_1.setMotorPwm(0);

Encoder_2.setMotorPwm(0);

}

}


PID_angle_compute

void PID_angle_compute(void)   //PID

{

CompAngleX = -gyro.getAngleX();

double error = CompAngleX - PID_angle.Setpoint;

PID_angle.Integral += error;

if(abs(CompAngleX - PID_angle.Setpoint) < 1)

{

PID_angle.Integral = 0;

}

 

PID_angle.differential = angle_speed;

PID_angle.Output = PID_angle.P * error + PID_angle.I * PID_angle.Integral + PID_angle.D * PID_angle.differential;

if(PID_angle.Output > 0)

{

PID_angle.Output = PID_angle.Output + PWM_MIN_OFFSET;

}

else

{

PID_angle.Output = PID_angle.Output - PWM_MIN_OFFSET;

}

 

double pwm_left = PID_angle.Output - PID_turn.Output;

double pwm_right = -PID_angle.Output - PID_turn.Output;

 

#ifdef DEBUG_INFO

Serial.print("Relay: ");

Serial.print(PID_angle.Setpoint);

Serial.print(" AngX: ");

Serial.print(CompAngleX);

Serial.print(" Output: ");

Serial.print(PID_angle.Output);

Serial.print(" dif: ");

Serial.println(PID_angle.differential);

#endif

 

pwm_left = constrain(pwm_left, -255, 255);

pwm_right = constrain(pwm_right, -255, 255);

 

Encoder_1.setMotorPwm(pwm_left);

Encoder_2.setMotorPwm(pwm_right);

}


PID_speed_compute


void PID_speed_compute(void)

{

double speed_now = (Encoder_2.GetCurrentSpeed() - Encoder_1.GetCurrentSpeed())/2;

 

last_speed_setpoint_filter  = last_speed_setpoint_filter  * 0.8;

last_speed_setpoint_filter  += PID_speed.Setpoint * 0.2;

 

if((move_flag == true) && (abs(speed_now) < 8) && (PID_speed.Setpoint == 0))

{

move_flag = false;

last_speed_setpoint_filter = 0;

PID_speed.Integral = speed_Integral_average;

}

 

double error = speed_now - last_speed_setpoint_filter;

PID_speed.Integral += error;

 

if(move_flag == true)

{

PID_speed.Integral = constrain(PID_speed.Integral , -1500, 1500);

PID_speed.Output = PID_speed.P * error + PID_speed.I * PID_speed.Integral;

PID_speed.Output = constrain(PID_speed.Output , -15.0, 15.0);

}

else

{

PID_speed.Integral = constrain(PID_speed.Integral , -1500, 1500);

PID_speed.Output = PID_speed.P * speed_now + PID_speed.I * PID_speed.Integral;

PID_speed.Output = constrain(PID_speed.Output , -15.0, 15.0);

speed_Integral_average = 0.8 * speed_Integral_average + 0.2 * PID_speed.Integral;

}

 

#ifdef DEBUG_INFO

Serial.print(speed_now);

Serial.print(",");

Serial.print(PID_speed.Setpoint);

Serial.print(",");

Serial.print(last_speed_error_filter);

Serial.print(",");

Serial.print(last_speed_setpoint_filter);

Serial.print(",");

Serial.print(PID_speed.Integral);

Serial.print(",");

Serial.println(PID_speed.Output);

#endif

PID_angle.Setpoint =  RELAX_ANGLE - PID_speed.Output;

}


附件链接:链接(密码:X1kp)

参考文献:

Matlab文档:Transmitting Data Over the Bluetooth Interface,http://cn.mathworks.com/help/instrument/reading-and-writing-data-over-the-bluetooth-interface.html

Mbot官网:makeblock.com

作者:Top Liu

转载请注明出处exbot.net

标签:

给我留言

Copyright © ExBot易科机器人实验室 保留所有权利.   Theme   Robin modified by poyoten

用户登录

分享到: