163 lines
3.2 KiB
Plaintext
163 lines
3.2 KiB
Plaintext
/*
|
|
* RosSerial BlinkM Example
|
|
* This program shows how to control a blinkm
|
|
* from an arduino using RosSerial
|
|
*/
|
|
|
|
#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);
|
|
}
|
|
|