root / quad2 / arduino / src / ros_lib / examples / BlinkM / BlinkM.pde @ c1426757
History | View | Annotate | Download (3.24 KB)
| 1 | c1426757 | Tom Mullins | /* |
|---|---|---|---|
| 2 | * RosSerial BlinkM Example |
||
| 3 | * This program shows how to control a blinkm |
||
| 4 | * from an arduino using RosSerial |
||
| 5 | */ |
||
| 6 | |||
| 7 | #include "WProgram.h" //include the Arduino library |
||
| 8 | #include <stdlib.h> |
||
| 9 | |||
| 10 | |||
| 11 | #include <ros.h> |
||
| 12 | #include <std_msgs/String.h> |
||
| 13 | |||
| 14 | |||
| 15 | //include Wire/ twi for the BlinkM |
||
| 16 | #include <Wire.h> |
||
| 17 | extern "C" {
|
||
| 18 | #include "utility/twi.h" |
||
| 19 | } |
||
| 20 | |||
| 21 | #include "BlinkM_funcs.h" |
||
| 22 | const byte blinkm_addr = 0x09; //default blinkm address |
||
| 23 | |||
| 24 | |||
| 25 | void setLED( bool solid, char color) |
||
| 26 | {
|
||
| 27 | |||
| 28 | if (solid) |
||
| 29 | {
|
||
| 30 | switch (color) |
||
| 31 | {
|
||
| 32 | |||
| 33 | case 'w': // white |
||
| 34 | BlinkM_stopScript( blinkm_addr ); |
||
| 35 | BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0xff); |
||
| 36 | break; |
||
| 37 | |||
| 38 | case 'r': //RED |
||
| 39 | BlinkM_stopScript( blinkm_addr ); |
||
| 40 | BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0); |
||
| 41 | break; |
||
| 42 | |||
| 43 | case 'g':// Green |
||
| 44 | BlinkM_stopScript( blinkm_addr ); |
||
| 45 | BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0); |
||
| 46 | break; |
||
| 47 | |||
| 48 | case 'b':// Blue |
||
| 49 | BlinkM_stopScript( blinkm_addr ); |
||
| 50 | BlinkM_fadeToRGB( blinkm_addr, 0,0,0xff); |
||
| 51 | break; |
||
| 52 | |||
| 53 | case 'c':// Cyan |
||
| 54 | BlinkM_stopScript( blinkm_addr ); |
||
| 55 | BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0xff); |
||
| 56 | break; |
||
| 57 | |||
| 58 | case 'm': // Magenta |
||
| 59 | BlinkM_stopScript( blinkm_addr ); |
||
| 60 | BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0xff); |
||
| 61 | break; |
||
| 62 | |||
| 63 | case 'y': // yellow |
||
| 64 | BlinkM_stopScript( blinkm_addr ); |
||
| 65 | BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0); |
||
| 66 | break; |
||
| 67 | |||
| 68 | default: // Black |
||
| 69 | BlinkM_stopScript( blinkm_addr ); |
||
| 70 | BlinkM_fadeToRGB( blinkm_addr, 0,0,0); |
||
| 71 | break; |
||
| 72 | } |
||
| 73 | } |
||
| 74 | |||
| 75 | |||
| 76 | else |
||
| 77 | {
|
||
| 78 | switch (color) |
||
| 79 | {
|
||
| 80 | case 'r': // Blink Red |
||
| 81 | BlinkM_stopScript( blinkm_addr ); |
||
| 82 | BlinkM_playScript( blinkm_addr, 3,0,0 ); |
||
| 83 | break; |
||
| 84 | case 'w': // Blink white |
||
| 85 | BlinkM_stopScript( blinkm_addr ); |
||
| 86 | BlinkM_playScript( blinkm_addr, 2,0,0 ); |
||
| 87 | break; |
||
| 88 | case 'g': // Blink Green |
||
| 89 | BlinkM_stopScript( blinkm_addr ); |
||
| 90 | BlinkM_playScript( blinkm_addr, 4,0,0 ); |
||
| 91 | break; |
||
| 92 | |||
| 93 | case 'b': // Blink Blue |
||
| 94 | BlinkM_stopScript( blinkm_addr ); |
||
| 95 | BlinkM_playScript( blinkm_addr, 5,0,0 ); |
||
| 96 | break; |
||
| 97 | |||
| 98 | case 'c': //Blink Cyan |
||
| 99 | BlinkM_stopScript( blinkm_addr ); |
||
| 100 | BlinkM_playScript( blinkm_addr, 6,0,0 ); |
||
| 101 | break; |
||
| 102 | |||
| 103 | case 'm': //Blink Magenta |
||
| 104 | BlinkM_stopScript( blinkm_addr ); |
||
| 105 | BlinkM_playScript( blinkm_addr, 7,0,0 ); |
||
| 106 | break; |
||
| 107 | |||
| 108 | case 'y': //Blink Yellow |
||
| 109 | BlinkM_stopScript( blinkm_addr ); |
||
| 110 | BlinkM_playScript( blinkm_addr, 8,0,0 ); |
||
| 111 | break; |
||
| 112 | |||
| 113 | default: //OFF |
||
| 114 | BlinkM_stopScript( blinkm_addr ); |
||
| 115 | BlinkM_playScript( blinkm_addr, 9,0,0 ); |
||
| 116 | break; |
||
| 117 | } |
||
| 118 | |||
| 119 | } |
||
| 120 | } |
||
| 121 | |||
| 122 | void light_cb( const std_msgs::String& light_cmd){
|
||
| 123 | bool solid =false; |
||
| 124 | char color; |
||
| 125 | if (strlen( (const char* ) light_cmd.data) ==2 ){
|
||
| 126 | solid = (light_cmd.data[0] == 'S') || (light_cmd.data[0] == 's'); |
||
| 127 | color = light_cmd.data[1]; |
||
| 128 | } |
||
| 129 | else{
|
||
| 130 | solid= false; |
||
| 131 | color = light_cmd.data[0]; |
||
| 132 | } |
||
| 133 | |||
| 134 | setLED(solid, color); |
||
| 135 | } |
||
| 136 | |||
| 137 | |||
| 138 | |||
| 139 | ros::NodeHandle nh; |
||
| 140 | ros::Subscriber<std_msgs::String> sub("blinkm" , light_cb);
|
||
| 141 | |||
| 142 | |||
| 143 | void setup() |
||
| 144 | {
|
||
| 145 | |||
| 146 | pinMode(13, OUTPUT); //set up the LED |
||
| 147 | |||
| 148 | BlinkM_beginWithPower(); |
||
| 149 | delay(100); |
||
| 150 | BlinkM_stopScript(blinkm_addr); // turn off startup script |
||
| 151 | setLED(false, 0); //turn off the led |
||
| 152 | |||
| 153 | nh.initNode(); |
||
| 154 | nh.subscribe(sub); |
||
| 155 | |||
| 156 | } |
||
| 157 | |||
| 158 | void loop() |
||
| 159 | {
|
||
| 160 | nh.spinOnce(); |
||
| 161 | delay(1); |
||
| 162 | } |