ROS与Arduino-BlinkM指南

BlinkM指南

说明

• BlinkM是非常小的LED灯，适合作为机器人的指示灯
• BlinkM是I2C控制的多颜色的LED灯
• 这个教程展示通过Arduino和rosserial控制BlinkM的多色闪亮或作为固定色的指示灯。

硬件

• Arduino
• BlinkM （BlinkM datasheet , Diagram from the BlinkM datasheet）

连接图

• BlinkM 需要连接到5V, GND, 和Arduino的I2C SDA 和SCL

代码

/*
*  RosSerial BlinkM Example
*  This program shows how to control a blinkm
*  from an arduino using RosSerial
*/

#include "WProgram.h" //include the Arduino library
#include <stdlib.h>


#include <ros.h>
#include <std_msgs/String.h>


//include Wire/ twi for the BlinkM
#include <Wire.h>
extern "C" { 
#include "utility/twi.h" 
}

#include "BlinkM_funcs.h"
const byte blinkm_addr = 0x09; //default blinkm address


void setLED( bool solid,  char color)
{

        if (solid)
        {
           switch (color)
                {

                case 'w':  // white
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0xff);  
                        break;
                        
                case 'r': //RED
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0);  
                        break;

                case 'g':// Green
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0);
                        break;

                case 'b':// Blue
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0,0,0xff);
                        break;

                case 'c':// Cyan
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0xff);
                        break;

                case 'm': // Magenta
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0xff);
                        break;

                case 'y': // yellow
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0);
                        break;

                default: // Black
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_fadeToRGB( blinkm_addr, 0,0,0);
                        break;
                }
        }


        else
        {
                switch (color)
                {
                case 'r':  // Blink Red
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 3,0,0 );
                        break;
                case 'w':  // Blink white
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 2,0,0 );
                        break;
                case 'g':  // Blink Green
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 4,0,0 );
                        break;

                case 'b': // Blink Blue
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 5,0,0 );
                        break;

                case 'c': //Blink Cyan
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 6,0,0 );
                        break;

                case 'm': //Blink Magenta
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 7,0,0 );
                        break;

                case 'y': //Blink Yellow
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 8,0,0 );
                        break;

                default: //OFF
                        BlinkM_stopScript( blinkm_addr );
                        BlinkM_playScript( blinkm_addr, 9,0,0 );
                        break;
                }

        }
}

void light_cb( const std_msgs::String& light_cmd){
        bool solid =false;
        char color; 
        if (strlen( (const char* ) light_cmd.data) ==2 ){
          solid  = (light_cmd.data[0] == 'S') || (light_cmd.data[0] == 's');
          color = light_cmd.data[1];
        }
        else{
          solid=  false;
          color = light_cmd.data[0];
        } 
        
        setLED(solid, color);
}



ros::NodeHandle  nh;
ros::Subscriber<std_msgs::String> sub("blinkm" , light_cb);


void setup()
{
   
    pinMode(13, OUTPUT); //set up the LED

        BlinkM_beginWithPower();
        delay(100);
        BlinkM_stopScript(blinkm_addr);  // turn off startup script
        setLED(false, 0); //turn off the led
        
        nh.initNode();
        nh.subscribe(sub);

}

void loop()
{
 nh.spinOnce();
 delay(1);
}

• 以上代码位于Arduino IDE, File->Examples->ros_lib->blinkm
• 节点订阅主题blinkm 消息类型为std_msgs/String
• 在回调函数，节点传递命令去决定那种颜色或是否点亮
• blinkm颜色有red (r), blue (b), magenta(m), green(g), white(w), cyan(c), and yellow(y)
• 如命令带有'S' or 's' LED 则为固定的颜色
• BlinkM I2C地址为默认的设置
• 如果重新编译或使用多个BlinkM即要更改这个地址

测试

1. 新窗口打开

$ roscore

2. 新窗口打开，/dev/ttyUSB0为Arduino设备端口号

$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0

3. 点亮红色

$ rostopic pub blinkm std_msgs/String "br"

4. 点亮蓝色

$ rostopic pub blinkm std_msgs/String "sb"