Revision 3a82529c
ID | 3a82529c30555598185a1b65103c148465bb97a5 |
Nevermind... didn't need those
D'oh!
quad2/arduino/src/ros_lib/ArduinoHardware-new.h | ||
---|---|---|
1 |
/* |
|
2 |
* Software License Agreement (BSD License) |
|
3 |
* |
|
4 |
* Copyright (c) 2011, Willow Garage, Inc. |
|
5 |
* All rights reserved. |
|
6 |
* |
|
7 |
* Redistribution and use in source and binary forms, with or without |
|
8 |
* modification, are permitted provided that the following conditions |
|
9 |
* are met: |
|
10 |
* |
|
11 |
* * Redistributions of source code must retain the above copyright |
|
12 |
* notice, this list of conditions and the following disclaimer. |
|
13 |
* * Redistributions in binary form must reproduce the above |
|
14 |
* copyright notice, this list of conditions and the following |
|
15 |
* disclaimer in the documentation and/or other materials provided |
|
16 |
* with the distribution. |
|
17 |
* * Neither the name of Willow Garage, Inc. nor the names of its |
|
18 |
* contributors may be used to endorse or promote prducts derived |
|
19 |
* from this software without specific prior written permission. |
|
20 |
* |
|
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
|
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
|
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
32 |
* POSSIBILITY OF SUCH DAMAGE. |
|
33 |
*/ |
|
34 |
|
|
35 |
/* |
|
36 |
* |
|
37 |
* This is a modified version of ArduinoHardware.h to use Serial_ instead of |
|
38 |
* HardwareSerial for the Leonardo |
|
39 |
* |
|
40 |
*/ |
|
41 |
|
|
42 |
#ifndef ROS_ARDUINO_HARDWARE_H_ |
|
43 |
#define ROS_ARDUINO_HARDWARE_H_ |
|
44 |
|
|
45 |
#include "WProgram.h" |
|
46 |
#include <HardwareSerial.h> |
|
47 |
|
|
48 |
class ArduinoHardware { |
|
49 |
public: |
|
50 |
ArduinoHardware(Serial_* io , long baud= 57600){ |
|
51 |
iostream = io; |
|
52 |
baud_ = baud; |
|
53 |
} |
|
54 |
ArduinoHardware() |
|
55 |
{ |
|
56 |
iostream = &Serial; |
|
57 |
baud_ = 57600; |
|
58 |
} |
|
59 |
ArduinoHardware(ArduinoHardware& h){ |
|
60 |
this->iostream = iostream; |
|
61 |
this->baud_ = h.baud_; |
|
62 |
} |
|
63 |
|
|
64 |
void setBaud(long baud){ |
|
65 |
this->baud_= baud; |
|
66 |
} |
|
67 |
|
|
68 |
int getBaud(){return baud_;} |
|
69 |
|
|
70 |
void init(){ |
|
71 |
iostream->begin(baud_); |
|
72 |
} |
|
73 |
|
|
74 |
int read(){return iostream->read();}; |
|
75 |
void write(uint8_t* data, int length){ |
|
76 |
for(int i=0; i<length; i++) |
|
77 |
iostream->write(data[i]); |
|
78 |
} |
|
79 |
|
|
80 |
unsigned long time(){return millis();} |
|
81 |
|
|
82 |
protected: |
|
83 |
Serial_* iostream; |
|
84 |
long baud_; |
|
85 |
}; |
|
86 |
|
|
87 |
#endif |
quad2/arduino/src/ros_lib/ArduinoHardware-org.h | ||
---|---|---|
1 |
/* |
|
2 |
* Software License Agreement (BSD License) |
|
3 |
* |
|
4 |
* Copyright (c) 2011, Willow Garage, Inc. |
|
5 |
* All rights reserved. |
|
6 |
* |
|
7 |
* Redistribution and use in source and binary forms, with or without |
|
8 |
* modification, are permitted provided that the following conditions |
|
9 |
* are met: |
|
10 |
* |
|
11 |
* * Redistributions of source code must retain the above copyright |
|
12 |
* notice, this list of conditions and the following disclaimer. |
|
13 |
* * Redistributions in binary form must reproduce the above |
|
14 |
* copyright notice, this list of conditions and the following |
|
15 |
* disclaimer in the documentation and/or other materials provided |
|
16 |
* with the distribution. |
|
17 |
* * Neither the name of Willow Garage, Inc. nor the names of its |
|
18 |
* contributors may be used to endorse or promote prducts derived |
|
19 |
* from this software without specific prior written permission. |
|
20 |
* |
|
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
|
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
|
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
32 |
* POSSIBILITY OF SUCH DAMAGE. |
|
33 |
*/ |
|
34 |
|
|
35 |
#ifndef ROS_ARDUINO_HARDWARE_H_ |
|
36 |
#define ROS_ARDUINO_HARDWARE_H_ |
|
37 |
|
|
38 |
#include "WProgram.h" |
|
39 |
#include <HardwareSerial.h> |
|
40 |
|
|
41 |
class ArduinoHardware { |
|
42 |
public: |
|
43 |
ArduinoHardware(HardwareSerial* io , long baud= 57600){ |
|
44 |
iostream = io; |
|
45 |
baud_ = baud; |
|
46 |
} |
|
47 |
ArduinoHardware() |
|
48 |
{ |
|
49 |
iostream = &Serial; |
|
50 |
baud_ = 57600; |
|
51 |
} |
|
52 |
ArduinoHardware(ArduinoHardware& h){ |
|
53 |
this->iostream = iostream; |
|
54 |
this->baud_ = h.baud_; |
|
55 |
} |
|
56 |
|
|
57 |
void setBaud(long baud){ |
|
58 |
this->baud_= baud; |
|
59 |
} |
|
60 |
|
|
61 |
int getBaud(){return baud_;} |
|
62 |
|
|
63 |
void init(){ |
|
64 |
iostream->begin(baud_); |
|
65 |
} |
|
66 |
|
|
67 |
int read(){return iostream->read();}; |
|
68 |
void write(uint8_t* data, int length){ |
|
69 |
for(int i=0; i<length; i++) |
|
70 |
iostream->write(data[i]); |
|
71 |
} |
|
72 |
|
|
73 |
unsigned long time(){return millis();} |
|
74 |
|
|
75 |
protected: |
|
76 |
HardwareSerial* iostream; |
|
77 |
long baud_; |
|
78 |
}; |
|
79 |
|
|
80 |
#endif |
quad2/arduino/src/ros_lib/ArduinoHardware.h | ||
---|---|---|
1 |
/* |
|
2 |
* Software License Agreement (BSD License) |
|
3 |
* |
|
4 |
* Copyright (c) 2011, Willow Garage, Inc. |
|
5 |
* All rights reserved. |
|
6 |
* |
|
7 |
* Redistribution and use in source and binary forms, with or without |
|
8 |
* modification, are permitted provided that the following conditions |
|
9 |
* are met: |
|
10 |
* |
|
11 |
* * Redistributions of source code must retain the above copyright |
|
12 |
* notice, this list of conditions and the following disclaimer. |
|
13 |
* * Redistributions in binary form must reproduce the above |
|
14 |
* copyright notice, this list of conditions and the following |
|
15 |
* disclaimer in the documentation and/or other materials provided |
|
16 |
* with the distribution. |
|
17 |
* * Neither the name of Willow Garage, Inc. nor the names of its |
|
18 |
* contributors may be used to endorse or promote prducts derived |
|
19 |
* from this software without specific prior written permission. |
|
20 |
* |
|
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
|
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
|
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
32 |
* POSSIBILITY OF SUCH DAMAGE. |
|
33 |
*/ |
|
34 |
|
|
35 |
#ifndef ROS_ARDUINO_HARDWARE_H_ |
|
36 |
#define ROS_ARDUINO_HARDWARE_H_ |
|
37 |
|
|
38 |
#include "WProgram.h" |
|
39 |
#include <HardwareSerial.h> |
|
40 |
|
|
41 |
class ArduinoHardware { |
|
42 |
public: |
|
43 |
ArduinoHardware(HardwareSerial* io , long baud= 57600){ |
|
44 |
iostream = io; |
|
45 |
baud_ = baud; |
|
46 |
} |
|
47 |
ArduinoHardware() |
|
48 |
{ |
|
49 |
iostream = &Serial; |
|
50 |
baud_ = 57600; |
|
51 |
} |
|
52 |
ArduinoHardware(ArduinoHardware& h){ |
|
53 |
this->iostream = iostream; |
|
54 |
this->baud_ = h.baud_; |
|
55 |
} |
|
56 |
|
|
57 |
void setBaud(long baud){ |
|
58 |
this->baud_= baud; |
|
59 |
} |
|
60 |
|
|
61 |
int getBaud(){return baud_;} |
|
62 |
|
|
63 |
void init(){ |
|
64 |
iostream->begin(baud_); |
|
65 |
} |
|
66 |
|
|
67 |
int read(){return iostream->read();}; |
|
68 |
void write(uint8_t* data, int length){ |
|
69 |
for(int i=0; i<length; i++) |
|
70 |
iostream->write(data[i]); |
|
71 |
} |
|
72 |
|
|
73 |
unsigned long time(){return millis();} |
|
74 |
|
|
75 |
protected: |
|
76 |
HardwareSerial* iostream; |
|
77 |
long baud_; |
|
78 |
}; |
|
79 |
|
|
80 |
#endif |
quad2/arduino/src/ros_lib/duration.cpp | ||
---|---|---|
1 |
/* |
|
2 |
* Software License Agreement (BSD License) |
|
3 |
* |
|
4 |
* Copyright (c) 2011, Willow Garage, Inc. |
|
5 |
* All rights reserved. |
|
6 |
* |
|
7 |
* Redistribution and use in source and binary forms, with or without |
|
8 |
* modification, are permitted provided that the following conditions |
|
9 |
* are met: |
|
10 |
* |
|
11 |
* * Redistributions of source code must retain the above copyright |
|
12 |
* notice, this list of conditions and the following disclaimer. |
|
13 |
* * Redistributions in binary form must reproduce the above |
|
14 |
* copyright notice, this list of conditions and the following |
|
15 |
* disclaimer in the documentation and/or other materials provided |
|
16 |
* with the distribution. |
|
17 |
* * Neither the name of Willow Garage, Inc. nor the names of its |
|
18 |
* contributors may be used to endorse or promote prducts derived |
|
19 |
* from this software without specific prior written permission. |
|
20 |
* |
|
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
|
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
|
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
32 |
* POSSIBILITY OF SUCH DAMAGE. |
|
33 |
*/ |
|
34 |
|
|
35 |
#include <math.h> |
|
36 |
#include "ros/duration.h" |
|
37 |
|
|
38 |
namespace ros |
|
39 |
{ |
|
40 |
void normalizeSecNSecSigned(long &sec, long &nsec) |
|
41 |
{ |
|
42 |
long nsec_part = nsec; |
|
43 |
long sec_part = sec; |
|
44 |
|
|
45 |
while (nsec_part > 1000000000L) |
|
46 |
{ |
|
47 |
nsec_part -= 1000000000L; |
|
48 |
++sec_part; |
|
49 |
} |
|
50 |
while (nsec_part < 0) |
|
51 |
{ |
|
52 |
nsec_part += 1000000000L; |
|
53 |
--sec_part; |
|
54 |
} |
|
55 |
sec = sec_part; |
|
56 |
nsec = nsec_part; |
|
57 |
} |
|
58 |
|
|
59 |
Duration& Duration::operator+=(const Duration &rhs) |
|
60 |
{ |
|
61 |
sec += rhs.sec; |
|
62 |
nsec += rhs.nsec; |
|
63 |
normalizeSecNSecSigned(sec, nsec); |
|
64 |
return *this; |
|
65 |
} |
|
66 |
|
|
67 |
Duration& Duration::operator-=(const Duration &rhs){ |
|
68 |
sec += -rhs.sec; |
|
69 |
nsec += -rhs.nsec; |
|
70 |
normalizeSecNSecSigned(sec, nsec); |
|
71 |
return *this; |
|
72 |
} |
|
73 |
|
|
74 |
Duration& Duration::operator*=(double scale){ |
|
75 |
sec *= scale; |
|
76 |
nsec *= scale; |
|
77 |
normalizeSecNSecSigned(sec, nsec); |
|
78 |
return *this; |
|
79 |
} |
|
80 |
|
|
81 |
} |
quad2/arduino/src/ros_lib/examples/ADC/ADC.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial ADC Example |
|
3 |
* |
|
4 |
* This is a poor man's Oscilloscope. It does not have the sampling |
|
5 |
* rate or accuracy of a commerical scope, but it is great to get |
|
6 |
* an analog value into ROS in a pinch. |
|
7 |
*/ |
|
8 |
|
|
9 |
#include <WProgram.h> |
|
10 |
#include <ros.h> |
|
11 |
#include <rosserial_arduino/Adc.h> |
|
12 |
|
|
13 |
ros::NodeHandle nh; |
|
14 |
|
|
15 |
rosserial_arduino::Adc adc_msg; |
|
16 |
ros::Publisher p("adc", &adc_msg); |
|
17 |
|
|
18 |
void setup() |
|
19 |
{ |
|
20 |
pinMode(13, OUTPUT); |
|
21 |
nh.initNode(); |
|
22 |
|
|
23 |
nh.advertise(p); |
|
24 |
} |
|
25 |
|
|
26 |
//We average the analog reading to elminate some of the noise |
|
27 |
int averageAnalog(int pin){ |
|
28 |
int v=0; |
|
29 |
for(int i=0; i<4; i++) v+= analogRead(pin); |
|
30 |
return v/4; |
|
31 |
} |
|
32 |
|
|
33 |
long adc_timer; |
|
34 |
|
|
35 |
void loop() |
|
36 |
{ |
|
37 |
adc_msg.adc0 = averageAnalog(0); |
|
38 |
adc_msg.adc1 = averageAnalog(1); |
|
39 |
adc_msg.adc2 = averageAnalog(2); |
|
40 |
adc_msg.adc3 = averageAnalog(3); |
|
41 |
adc_msg.adc4 = averageAnalog(4); |
|
42 |
adc_msg.adc5 = averageAnalog(5); |
|
43 |
|
|
44 |
p.publish(&adc_msg); |
|
45 |
|
|
46 |
nh.spinOnce(); |
|
47 |
} |
|
48 |
|
quad2/arduino/src/ros_lib/examples/Blink/Blink.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial Subscriber Example |
|
3 |
* Blinks an LED on callback |
|
4 |
*/ |
|
5 |
|
|
6 |
#include <ros.h> |
|
7 |
#include <std_msgs/Empty.h> |
|
8 |
|
|
9 |
ros::NodeHandle nh; |
|
10 |
|
|
11 |
void messageCb( const std_msgs::Empty& toggle_msg){ |
|
12 |
digitalWrite(13, HIGH-digitalRead(13)); // blink the led |
|
13 |
} |
|
14 |
|
|
15 |
ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb ); |
|
16 |
|
|
17 |
void setup() |
|
18 |
{ |
|
19 |
pinMode(13, OUTPUT); |
|
20 |
nh.initNode(); |
|
21 |
nh.subscribe(sub); |
|
22 |
} |
|
23 |
|
|
24 |
void loop() |
|
25 |
{ |
|
26 |
nh.spinOnce(); |
|
27 |
delay(1); |
|
28 |
} |
|
29 |
|
quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM.pde | ||
---|---|---|
1 |
/* |
|
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 |
} |
|
163 |
|
quad2/arduino/src/ros_lib/examples/BlinkM/BlinkM_funcs.h | ||
---|---|---|
1 |
/* |
|
2 |
* BlinkM_funcs.h -- Arduino 'library' to control BlinkM |
|
3 |
* -------------- |
|
4 |
* |
|
5 |
* |
|
6 |
* Note: original version of this file lives with the BlinkMTester sketch |
|
7 |
* |
|
8 |
* Note: all the functions are declared 'static' because |
|
9 |
* it saves about 1.5 kbyte in code space in final compiled sketch. |
|
10 |
* A C++ library of this costs a 1kB more. |
|
11 |
* |
|
12 |
* 2007-8, Tod E. Kurt, ThingM, http://thingm.com/ |
|
13 |
* |
|
14 |
* version: 20081101 |
|
15 |
* |
|
16 |
* history: |
|
17 |
* 20080101 - initial release |
|
18 |
* 20080203 - added setStartupParam(), bugfix receiveBytes() from Dan Julio |
|
19 |
* 20081101 - fixed to work with Arduino-0012, added MaxM commands, |
|
20 |
* added test script read/write functions, cleaned up some functions |
|
21 |
* 20090121 - added I2C bus scan functions, has dependencies on private |
|
22 |
* functions inside Wire library, so might break in the future |
|
23 |
* 20100420 - added BlinkM_startPower and _stopPower |
|
24 |
* |
|
25 |
*/ |
|
26 |
|
|
27 |
#include "WProgram.h" |
|
28 |
#include "wiring.h" |
|
29 |
#include <Wire.h> |
|
30 |
|
|
31 |
extern "C" { |
|
32 |
#include "utility/twi.h" // from Wire library, so we can do bus scanning |
|
33 |
} |
|
34 |
|
|
35 |
|
|
36 |
// format of light script lines: duration, command, arg1,arg2,arg3 |
|
37 |
typedef struct _blinkm_script_line { |
|
38 |
uint8_t dur; |
|
39 |
uint8_t cmd[4]; // cmd,arg1,arg2,arg3 |
|
40 |
} blinkm_script_line; |
|
41 |
|
|
42 |
|
|
43 |
// Call this first (when powering BlinkM from a power supply) |
|
44 |
static void BlinkM_begin() |
|
45 |
{ |
|
46 |
Wire.begin(); // join i2c bus (address optional for master) |
|
47 |
} |
|
48 |
|
|
49 |
/* |
|
50 |
* actually can't do this either, because twi_init() has THREE callocs in it too |
|
51 |
* |
|
52 |
static void BlinkM_reset() |
|
53 |
{ |
|
54 |
twi_init(); // can't just call Wire.begin() again because of calloc()s there |
|
55 |
} |
|
56 |
*/ |
|
57 |
|
|
58 |
// |
|
59 |
// each call to twi_writeTo() should return 0 if device is there |
|
60 |
// or other value (usually 2) if nothing is at that address |
|
61 |
// |
|
62 |
static void BlinkM_scanI2CBus(byte from, byte to, |
|
63 |
void(*callback)(byte add, byte result) ) |
|
64 |
{ |
|
65 |
byte rc; |
|
66 |
byte data = 0; // not used, just an address to feed to twi_writeTo() |
|
67 |
for( byte addr = from; addr <= to; addr++ ) { |
|
68 |
rc = twi_writeTo(addr, &data, 0, 1); |
|
69 |
callback( addr, rc ); |
|
70 |
} |
|
71 |
} |
|
72 |
|
|
73 |
// |
|
74 |
// |
|
75 |
static int8_t BlinkM_findFirstI2CDevice() |
|
76 |
{ |
|
77 |
byte rc; |
|
78 |
byte data = 0; // not used, just an address to feed to twi_writeTo() |
|
79 |
for( byte addr=1; addr < 120; addr++ ) { // only scan addrs 1-120 |
|
80 |
rc = twi_writeTo(addr, &data, 0, 1); |
|
81 |
if( rc == 0 ) return addr; // found an address |
|
82 |
} |
|
83 |
return -1; // no device found in range given |
|
84 |
} |
|
85 |
|
|
86 |
// FIXME: make this more Arduino-like |
|
87 |
static void BlinkM_startPowerWithPins(byte pwrpin, byte gndpin) |
|
88 |
{ |
|
89 |
DDRC |= _BV(pwrpin) | _BV(gndpin); // make outputs |
|
90 |
PORTC &=~ _BV(gndpin); |
|
91 |
PORTC |= _BV(pwrpin); |
|
92 |
} |
|
93 |
|
|
94 |
// FIXME: make this more Arduino-like |
|
95 |
static void BlinkM_stopPowerWithPins(byte pwrpin, byte gndpin) |
|
96 |
{ |
|
97 |
DDRC &=~ (_BV(pwrpin) | _BV(gndpin)); |
|
98 |
} |
|
99 |
|
|
100 |
// |
|
101 |
static void BlinkM_startPower() |
|
102 |
{ |
|
103 |
BlinkM_startPowerWithPins( PORTC3, PORTC2 ); |
|
104 |
} |
|
105 |
|
|
106 |
// |
|
107 |
static void BlinkM_stopPower() |
|
108 |
{ |
|
109 |
BlinkM_stopPowerWithPins( PORTC3, PORTC2 ); |
|
110 |
} |
|
111 |
|
|
112 |
// General version of BlinkM_beginWithPower(). |
|
113 |
// Call this first when BlinkM is plugged directly into Arduino |
|
114 |
static void BlinkM_beginWithPowerPins(byte pwrpin, byte gndpin) |
|
115 |
{ |
|
116 |
BlinkM_startPowerWithPins(pwrpin,gndpin); |
|
117 |
delay(100); // wait for things to stabilize |
|
118 |
Wire.begin(); |
|
119 |
} |
|
120 |
|
|
121 |
// Call this first when BlinkM is plugged directly into Arduino |
|
122 |
// FIXME: make this more Arduino-like |
|
123 |
static void BlinkM_beginWithPower() |
|
124 |
{ |
|
125 |
BlinkM_beginWithPowerPins( PORTC3, PORTC2 ); |
|
126 |
} |
|
127 |
|
|
128 |
// sends a generic command |
|
129 |
static void BlinkM_sendCmd(byte addr, byte* cmd, int cmdlen) |
|
130 |
{ |
|
131 |
Wire.beginTransmission(addr); |
|
132 |
for( byte i=0; i<cmdlen; i++) |
|
133 |
Wire.send(cmd[i]); |
|
134 |
Wire.endTransmission(); |
|
135 |
} |
|
136 |
|
|
137 |
// receives generic data |
|
138 |
// returns 0 on success, and -1 if no data available |
|
139 |
// note: responsiblity of caller to know how many bytes to expect |
|
140 |
static int BlinkM_receiveBytes(byte addr, byte* resp, byte len) |
|
141 |
{ |
|
142 |
Wire.requestFrom(addr, len); |
|
143 |
if( Wire.available() ) { |
|
144 |
for( int i=0; i<len; i++) |
|
145 |
resp[i] = Wire.receive(); |
|
146 |
return 0; |
|
147 |
} |
|
148 |
return -1; |
|
149 |
} |
|
150 |
|
|
151 |
// Sets the I2C address of the BlinkM. |
|
152 |
// Uses "general call" broadcast address |
|
153 |
static void BlinkM_setAddress(byte newaddress) |
|
154 |
{ |
|
155 |
Wire.beginTransmission(0x00); // general call (broadcast address) |
|
156 |
Wire.send('A'); |
|
157 |
Wire.send(newaddress); |
|
158 |
Wire.send(0xD0); |
|
159 |
Wire.send(0x0D); // dood! |
|
160 |
Wire.send(newaddress); |
|
161 |
Wire.endTransmission(); |
|
162 |
delay(50); // just in case |
|
163 |
} |
|
164 |
|
|
165 |
|
|
166 |
// Gets the I2C address of the BlinKM |
|
167 |
// Kind of redundant when sent to a specific address |
|
168 |
// but uses to verify BlinkM communication |
|
169 |
static int BlinkM_getAddress(byte addr) |
|
170 |
{ |
|
171 |
Wire.beginTransmission(addr); |
|
172 |
Wire.send('a'); |
|
173 |
Wire.endTransmission(); |
|
174 |
Wire.requestFrom(addr, (byte)1); // general call |
|
175 |
if( Wire.available() ) { |
|
176 |
byte b = Wire.receive(); |
|
177 |
return b; |
|
178 |
} |
|
179 |
return -1; |
|
180 |
} |
|
181 |
|
|
182 |
// Gets the BlinkM firmware version |
|
183 |
static int BlinkM_getVersion(byte addr) |
|
184 |
{ |
|
185 |
Wire.beginTransmission(addr); |
|
186 |
Wire.send('Z'); |
|
187 |
Wire.endTransmission(); |
|
188 |
Wire.requestFrom(addr, (byte)2); |
|
189 |
if( Wire.available() ) { |
|
190 |
byte major_ver = Wire.receive(); |
|
191 |
byte minor_ver = Wire.receive(); |
|
192 |
return (major_ver<<8) + minor_ver; |
|
193 |
} |
|
194 |
return -1; |
|
195 |
} |
|
196 |
|
|
197 |
// Demonstrates how to verify you're talking to a BlinkM |
|
198 |
// and that you know its address |
|
199 |
static int BlinkM_checkAddress(byte addr) |
|
200 |
{ |
|
201 |
//Serial.print("Checking BlinkM address..."); |
|
202 |
int b = BlinkM_getAddress(addr); |
|
203 |
if( b==-1 ) { |
|
204 |
//Serial.println("No response, that's not good"); |
|
205 |
return -1; // no response |
|
206 |
} |
|
207 |
//Serial.print("received addr: 0x"); |
|
208 |
//Serial.print(b,HEX); |
|
209 |
if( b != addr ) |
|
210 |
return 1; // error, addr mismatch |
|
211 |
else |
|
212 |
return 0; // match, everything okay |
|
213 |
} |
|
214 |
|
|
215 |
// Sets the speed of fading between colors. |
|
216 |
// Higher numbers means faster fading, 255 == instantaneous fading |
|
217 |
static void BlinkM_setFadeSpeed(byte addr, byte fadespeed) |
|
218 |
{ |
|
219 |
Wire.beginTransmission(addr); |
|
220 |
Wire.send('f'); |
|
221 |
Wire.send(fadespeed); |
|
222 |
Wire.endTransmission(); |
|
223 |
} |
|
224 |
|
|
225 |
// Sets the light script playback time adjust |
|
226 |
// The timeadj argument is signed, and is an additive value to all |
|
227 |
// durations in a light script. Set to zero to turn off time adjust. |
|
228 |
static void BlinkM_setTimeAdj(byte addr, byte timeadj) |
|
229 |
{ |
|
230 |
Wire.beginTransmission(addr); |
|
231 |
Wire.send('t'); |
|
232 |
Wire.send(timeadj); |
|
233 |
Wire.endTransmission(); |
|
234 |
} |
|
235 |
|
|
236 |
// Fades to an RGB color |
|
237 |
static void BlinkM_fadeToRGB(byte addr, byte red, byte grn, byte blu) |
|
238 |
{ |
|
239 |
Wire.beginTransmission(addr); |
|
240 |
Wire.send('c'); |
|
241 |
Wire.send(red); |
|
242 |
Wire.send(grn); |
|
243 |
Wire.send(blu); |
|
244 |
Wire.endTransmission(); |
|
245 |
} |
|
246 |
|
|
247 |
// Fades to an HSB color |
|
248 |
static void BlinkM_fadeToHSB(byte addr, byte hue, byte saturation, byte brightness) |
|
249 |
{ |
|
250 |
Wire.beginTransmission(addr); |
|
251 |
Wire.send('h'); |
|
252 |
Wire.send(hue); |
|
253 |
Wire.send(saturation); |
|
254 |
Wire.send(brightness); |
|
255 |
Wire.endTransmission(); |
|
256 |
} |
|
257 |
|
|
258 |
// Sets an RGB color immediately |
|
259 |
static void BlinkM_setRGB(byte addr, byte red, byte grn, byte blu) |
|
260 |
{ |
|
261 |
Wire.beginTransmission(addr); |
|
262 |
Wire.send('n'); |
|
263 |
Wire.send(red); |
|
264 |
Wire.send(grn); |
|
265 |
Wire.send(blu); |
|
266 |
Wire.endTransmission(); |
|
267 |
} |
|
268 |
|
|
269 |
// Fades to a random RGB color |
|
270 |
static void BlinkM_fadeToRandomRGB(byte addr, byte rrnd, byte grnd, byte brnd) |
|
271 |
{ |
|
272 |
Wire.beginTransmission(addr); |
|
273 |
Wire.send('C'); |
|
274 |
Wire.send(rrnd); |
|
275 |
Wire.send(grnd); |
|
276 |
Wire.send(brnd); |
|
277 |
Wire.endTransmission(); |
|
278 |
} |
|
279 |
// Fades to a random HSB color |
|
280 |
static void BlinkM_fadeToRandomHSB(byte addr, byte hrnd, byte srnd, byte brnd) |
|
281 |
{ |
|
282 |
Wire.beginTransmission(addr); |
|
283 |
Wire.send('H'); |
|
284 |
Wire.send(hrnd); |
|
285 |
Wire.send(srnd); |
|
286 |
Wire.send(brnd); |
|
287 |
Wire.endTransmission(); |
|
288 |
} |
|
289 |
|
|
290 |
// |
|
291 |
static void BlinkM_getRGBColor(byte addr, byte* r, byte* g, byte* b) |
|
292 |
{ |
|
293 |
Wire.beginTransmission(addr); |
|
294 |
Wire.send('g'); |
|
295 |
Wire.endTransmission(); |
|
296 |
Wire.requestFrom(addr, (byte)3); |
|
297 |
if( Wire.available() ) { |
|
298 |
*r = Wire.receive(); |
|
299 |
*g = Wire.receive(); |
|
300 |
*b = Wire.receive(); |
|
301 |
} |
|
302 |
} |
|
303 |
|
|
304 |
// |
|
305 |
static void BlinkM_playScript(byte addr, byte script_id, byte reps, byte pos) |
|
306 |
{ |
|
307 |
Wire.beginTransmission(addr); |
|
308 |
Wire.send('p'); |
|
309 |
Wire.send(script_id); |
|
310 |
Wire.send(reps); |
|
311 |
Wire.send(pos); |
|
312 |
Wire.endTransmission(); |
|
313 |
} |
|
314 |
|
|
315 |
// |
|
316 |
static void BlinkM_stopScript(byte addr) |
|
317 |
{ |
|
318 |
Wire.beginTransmission(addr); |
|
319 |
Wire.send('o'); |
|
320 |
Wire.endTransmission(); |
|
321 |
} |
|
322 |
|
|
323 |
// |
|
324 |
static void BlinkM_setScriptLengthReps(byte addr, byte script_id, |
|
325 |
byte len, byte reps) |
|
326 |
{ |
|
327 |
Wire.beginTransmission(addr); |
|
328 |
Wire.send('L'); |
|
329 |
Wire.send(script_id); |
|
330 |
Wire.send(len); |
|
331 |
Wire.send(reps); |
|
332 |
Wire.endTransmission(); |
|
333 |
} |
|
334 |
|
|
335 |
// Fill up script_line with data from a script line |
|
336 |
// currently only script_id = 0 works (eeprom script) |
|
337 |
static void BlinkM_readScriptLine(byte addr, byte script_id, |
|
338 |
byte pos, blinkm_script_line* script_line) |
|
339 |
{ |
|
340 |
Wire.beginTransmission(addr); |
|
341 |
Wire.send('R'); |
|
342 |
Wire.send(script_id); |
|
343 |
Wire.send(pos); |
|
344 |
Wire.endTransmission(); |
|
345 |
Wire.requestFrom(addr, (byte)5); |
|
346 |
while( Wire.available() < 5 ) ; // FIXME: wait until we get 7 bytes |
|
347 |
script_line->dur = Wire.receive(); |
|
348 |
script_line->cmd[0] = Wire.receive(); |
|
349 |
script_line->cmd[1] = Wire.receive(); |
|
350 |
script_line->cmd[2] = Wire.receive(); |
|
351 |
script_line->cmd[3] = Wire.receive(); |
|
352 |
} |
|
353 |
|
|
354 |
// |
|
355 |
static void BlinkM_writeScriptLine(byte addr, byte script_id, |
|
356 |
byte pos, byte dur, |
|
357 |
byte cmd, byte arg1, byte arg2, byte arg3) |
|
358 |
{ |
|
359 |
#ifdef BLINKM_FUNCS_DEBUG |
|
360 |
Serial.print("writing line:"); Serial.print(pos,DEC); |
|
361 |
Serial.print(" with cmd:"); Serial.print(cmd); |
|
362 |
Serial.print(" arg1:"); Serial.println(arg1,HEX); |
|
363 |
#endif |
|
364 |
Wire.beginTransmission(addr); |
|
365 |
Wire.send('W'); |
|
366 |
Wire.send(script_id); |
|
367 |
Wire.send(pos); |
|
368 |
Wire.send(dur); |
|
369 |
Wire.send(cmd); |
|
370 |
Wire.send(arg1); |
|
371 |
Wire.send(arg2); |
|
372 |
Wire.send(arg3); |
|
373 |
Wire.endTransmission(); |
|
374 |
|
|
375 |
} |
|
376 |
|
|
377 |
// |
|
378 |
static void BlinkM_writeScript(byte addr, byte script_id, |
|
379 |
byte len, byte reps, |
|
380 |
blinkm_script_line* lines) |
|
381 |
{ |
|
382 |
#ifdef BLINKM_FUNCS_DEBUG |
|
383 |
Serial.print("writing script to addr:"); Serial.print(addr,DEC); |
|
384 |
Serial.print(", script_id:"); Serial.println(script_id,DEC); |
|
385 |
#endif |
|
386 |
for(byte i=0; i < len; i++) { |
|
387 |
blinkm_script_line l = lines[i]; |
|
388 |
BlinkM_writeScriptLine( addr, script_id, i, l.dur, |
|
389 |
l.cmd[0], l.cmd[1], l.cmd[2], l.cmd[3]); |
|
390 |
delay(20); // must wait for EEPROM to be programmed |
|
391 |
} |
|
392 |
BlinkM_setScriptLengthReps(addr, script_id, len, reps); |
|
393 |
} |
|
394 |
|
|
395 |
// |
|
396 |
static void BlinkM_setStartupParams(byte addr, byte mode, byte script_id, |
|
397 |
byte reps, byte fadespeed, byte timeadj) |
|
398 |
{ |
|
399 |
Wire.beginTransmission(addr); |
|
400 |
Wire.send('B'); |
|
401 |
Wire.send(mode); // default 0x01 == Play script |
|
402 |
Wire.send(script_id); // default 0x00 == script #0 |
|
403 |
Wire.send(reps); // default 0x00 == repeat infinitely |
|
404 |
Wire.send(fadespeed); // default 0x08 == usually overridden by sketch |
|
405 |
Wire.send(timeadj); // default 0x00 == sometimes overridden by sketch |
|
406 |
Wire.endTransmission(); |
|
407 |
} |
|
408 |
|
|
409 |
|
|
410 |
// Gets digital inputs of the BlinkM |
|
411 |
// returns -1 on failure |
|
412 |
static int BlinkM_getInputsO(byte addr) |
|
413 |
{ |
|
414 |
Wire.beginTransmission(addr); |
|
415 |
Wire.send('i'); |
|
416 |
Wire.endTransmission(); |
|
417 |
Wire.requestFrom(addr, (byte)1); |
|
418 |
if( Wire.available() ) { |
|
419 |
byte b = Wire.receive(); |
|
420 |
return b; |
|
421 |
} |
|
422 |
return -1; |
|
423 |
} |
|
424 |
|
|
425 |
// Gets digital inputs of the BlinkM |
|
426 |
// stores them in passed in array |
|
427 |
// returns -1 on failure |
|
428 |
static int BlinkM_getInputs(byte addr, byte inputs[]) |
|
429 |
{ |
|
430 |
Wire.beginTransmission(addr); |
|
431 |
Wire.send('i'); |
|
432 |
Wire.endTransmission(); |
|
433 |
Wire.requestFrom(addr, (byte)4); |
|
434 |
while( Wire.available() < 4 ) ; // FIXME: wait until we get 4 bytes |
|
435 |
|
|
436 |
inputs[0] = Wire.receive(); |
|
437 |
inputs[1] = Wire.receive(); |
|
438 |
inputs[2] = Wire.receive(); |
|
439 |
inputs[3] = Wire.receive(); |
|
440 |
|
|
441 |
return 0; |
|
442 |
} |
quad2/arduino/src/ros_lib/examples/Clapper/Clapper.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial Clapper Example |
|
3 |
* |
|
4 |
* This code is a very simple example of the kinds of |
|
5 |
* custom sensors that you can easily set up with rosserial |
|
6 |
* and Arduino. This code uses a microphone attached to |
|
7 |
* analog pin 5 detect two claps (2 loud sounds). |
|
8 |
* You can use this clapper, for example, to command a robot |
|
9 |
* in the area to come do your bidding. |
|
10 |
*/ |
|
11 |
|
|
12 |
#include <WProgram.h> |
|
13 |
|
|
14 |
#include <ros.h> |
|
15 |
#include <std_msgs/Empty.h> |
|
16 |
|
|
17 |
ros::NodeHandle nh; |
|
18 |
|
|
19 |
std_msgs::Empty clap_msg; |
|
20 |
ros::Publisher p("clap", &clap_msg); |
|
21 |
|
|
22 |
enum clapper_state { clap1, clap_one_waiting, pause, clap2}; |
|
23 |
clapper_state clap; |
|
24 |
|
|
25 |
int volume_thresh = 200; //a clap sound needs to be: |
|
26 |
//abs(clap_volume) > average noise + volume_thresh |
|
27 |
int mic_pin = 5; |
|
28 |
int adc_ave=0; |
|
29 |
|
|
30 |
void setup() |
|
31 |
{ |
|
32 |
pinMode(13, OUTPUT); |
|
33 |
nh.initNode(); |
|
34 |
|
|
35 |
nh.advertise(p); |
|
36 |
|
|
37 |
//measure the average volume of the noise in the area |
|
38 |
for (int i =0; i<10;i++) adc_ave += analogRead(mic_pin); |
|
39 |
adc_ave /= 10; |
|
40 |
} |
|
41 |
|
|
42 |
long event_timer; |
|
43 |
|
|
44 |
void loop() |
|
45 |
{ |
|
46 |
int mic_val = 0; |
|
47 |
for(int i=0; i<4; i++) mic_val += analogRead(mic_pin); |
|
48 |
|
|
49 |
mic_val = mic_val/4-adc_ave; |
|
50 |
|
|
51 |
switch(clap){ |
|
52 |
case clap1: |
|
53 |
if (abs(mic_val) > volume_thresh){ |
|
54 |
clap = clap_one_waiting; |
|
55 |
event_timer = millis(); |
|
56 |
} |
|
57 |
break; |
|
58 |
case clap_one_waiting: |
|
59 |
if ( (abs(mic_val) < 30) && ( (millis() - event_timer) > 20 ) ) |
|
60 |
{ |
|
61 |
clap= pause; |
|
62 |
event_timer = millis(); |
|
63 |
|
|
64 |
} |
|
65 |
break; |
|
66 |
case pause: // make sure there is a pause between |
|
67 |
// the loud sounds |
|
68 |
if ( mic_val > volume_thresh) |
|
69 |
{ |
|
70 |
clap = clap1; |
|
71 |
|
|
72 |
} |
|
73 |
else if ( (millis()-event_timer)> 60) { |
|
74 |
clap = clap2; |
|
75 |
event_timer = millis(); |
|
76 |
|
|
77 |
} |
|
78 |
break; |
|
79 |
case clap2: |
|
80 |
if (abs(mic_val) > volume_thresh){ //we have got a double clap! |
|
81 |
clap = clap1; |
|
82 |
p.publish(&clap_msg); |
|
83 |
} |
|
84 |
else if ( (millis()-event_timer)> 200) { |
|
85 |
clap= clap1; // no clap detected, reset state machine |
|
86 |
} |
|
87 |
|
|
88 |
break; |
|
89 |
} |
|
90 |
nh.spinOnce(); |
|
91 |
} |
quad2/arduino/src/ros_lib/examples/HelloWorld/HelloWorld.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial Publisher Example |
|
3 |
* Prints "hello world!" |
|
4 |
*/ |
|
5 |
|
|
6 |
#include <ros.h> |
|
7 |
#include <std_msgs/String.h> |
|
8 |
|
|
9 |
ros::NodeHandle nh; |
|
10 |
|
|
11 |
std_msgs::String str_msg; |
|
12 |
ros::Publisher chatter("chatter", &str_msg); |
|
13 |
|
|
14 |
char hello[13] = "hello world!"; |
|
15 |
|
|
16 |
void setup() |
|
17 |
{ |
|
18 |
nh.initNode(); |
|
19 |
nh.advertise(chatter); |
|
20 |
} |
|
21 |
|
|
22 |
void loop() |
|
23 |
{ |
|
24 |
str_msg.data = hello; |
|
25 |
chatter.publish( &str_msg ); |
|
26 |
nh.spinOnce(); |
|
27 |
delay(1000); |
|
28 |
} |
quad2/arduino/src/ros_lib/examples/IrRanger/IrRanger.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial IR Ranger Example |
|
3 |
* |
|
4 |
* This example is calibrated for the Sharp GP2D120XJ00F. |
|
5 |
*/ |
|
6 |
|
|
7 |
#include <ros.h> |
|
8 |
#include <ros/time.h> |
|
9 |
#include <sensor_msgs/Range.h> |
|
10 |
|
|
11 |
ros::NodeHandle nh; |
|
12 |
|
|
13 |
|
|
14 |
sensor_msgs::Range range_msg; |
|
15 |
ros::Publisher pub_range( "range_data", &range_msg); |
|
16 |
|
|
17 |
const int analog_pin = 0; |
|
18 |
unsigned long range_timer; |
|
19 |
|
|
20 |
/* |
|
21 |
* getRange() - samples the analog input from the ranger |
|
22 |
* and converts it into meters. |
|
23 |
*/ |
|
24 |
float getRange(int pin_num){ |
|
25 |
int sample; |
|
26 |
// Get data |
|
27 |
sample = analogRead(pin_num)/4; |
|
28 |
// if the ADC reading is too low, |
|
29 |
// then we are really far away from anything |
|
30 |
if(sample < 10) |
|
31 |
return 254; // max range |
|
32 |
// Magic numbers to get cm |
|
33 |
sample= 1309/(sample-3); |
|
34 |
return (sample - 1)/100; //convert to meters |
|
35 |
} |
|
36 |
|
|
37 |
char frameid[] = "/ir_ranger"; |
|
38 |
|
|
39 |
void setup() |
|
40 |
{ |
|
41 |
nh.initNode(); |
|
42 |
nh.advertise(pub_range); |
|
43 |
|
|
44 |
range_msg.radiation_type = sensor_msgs::Range::INFRARED; |
|
45 |
range_msg.header.frame_id = frameid; |
|
46 |
range_msg.field_of_view = 0.01; |
|
47 |
range_msg.min_range = 0.03; |
|
48 |
range_msg.max_range = 0.4; |
|
49 |
|
|
50 |
} |
|
51 |
|
|
52 |
void loop() |
|
53 |
{ |
|
54 |
// publish the range value every 50 milliseconds |
|
55 |
// since it takes that long for the sensor to stabilize |
|
56 |
if ( (millis()-range_timer) > 50){ |
|
57 |
range_msg.range = getRange(analog_pin); |
|
58 |
range_msg.header.stamp = nh.now(); |
|
59 |
pub_range.publish(&range_msg); |
|
60 |
range_timer = millis() + 50; |
|
61 |
} |
|
62 |
nh.spinOnce(); |
|
63 |
} |
|
64 |
|
quad2/arduino/src/ros_lib/examples/Logging/Logging.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial PubSub Example |
|
3 |
* Prints "hello world!" and toggles led |
|
4 |
*/ |
|
5 |
|
|
6 |
#include <ros.h> |
|
7 |
#include <std_msgs/String.h> |
|
8 |
#include <std_msgs/Empty.h> |
|
9 |
|
|
10 |
ros::NodeHandle nh; |
|
11 |
|
|
12 |
|
|
13 |
std_msgs::String str_msg; |
|
14 |
ros::Publisher chatter("chatter", &str_msg); |
|
15 |
|
|
16 |
char hello[13] = "hello world!"; |
|
17 |
|
|
18 |
|
|
19 |
char debug[]= "debug statements"; |
|
20 |
char info[] = "infos"; |
|
21 |
char warn[] = "warnings"; |
|
22 |
char error[] = "errors"; |
|
23 |
char fatal[] = "fatalities"; |
|
24 |
|
|
25 |
void setup() |
|
26 |
{ |
|
27 |
pinMode(13, OUTPUT); |
|
28 |
nh.initNode(); |
|
29 |
nh.advertise(chatter); |
|
30 |
} |
|
31 |
|
|
32 |
void loop() |
|
33 |
{ |
|
34 |
str_msg.data = hello; |
|
35 |
chatter.publish( &str_msg ); |
|
36 |
|
|
37 |
nh.logdebug(debug); |
|
38 |
nh.loginfo(info); |
|
39 |
nh.logwarn(warn); |
|
40 |
nh.logerror(error); |
|
41 |
nh.logfatal(fatal); |
|
42 |
|
|
43 |
nh.spinOnce(); |
|
44 |
delay(500); |
|
45 |
} |
quad2/arduino/src/ros_lib/examples/Odom/Odom.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial Planar Odometry Example |
|
3 |
*/ |
|
4 |
|
|
5 |
#include <ros.h> |
|
6 |
#include <ros/time.h> |
|
7 |
#include <tf/tf.h> |
|
8 |
#include <tf/transform_broadcaster.h> |
|
9 |
|
|
10 |
ros::NodeHandle nh; |
|
11 |
|
|
12 |
geometry_msgs::TransformStamped t; |
|
13 |
tf::TransformBroadcaster broadcaster; |
|
14 |
|
|
15 |
double x = 1.0; |
|
16 |
double y = 0.0; |
|
17 |
double theta = 1.57; |
|
18 |
|
|
19 |
char base_link[] = "/base_link"; |
|
20 |
char odom[] = "/odom"; |
|
21 |
|
|
22 |
void setup() |
|
23 |
{ |
|
24 |
nh.initNode(); |
|
25 |
broadcaster.init(nh); |
|
26 |
} |
|
27 |
|
|
28 |
void loop() |
|
29 |
{ |
|
30 |
// drive in a circle |
|
31 |
double dx = 0.2; |
|
32 |
double dtheta = 0.18; |
|
33 |
x += cos(theta)*dx*0.1; |
|
34 |
y += sin(theta)*dx*0.1; |
|
35 |
theta += dtheta*0.1; |
|
36 |
if(theta > 3.14) |
|
37 |
theta=-3.14; |
|
38 |
|
|
39 |
// tf odom->base_link |
|
40 |
t.header.frame_id = odom; |
|
41 |
t.child_frame_id = base_link; |
|
42 |
|
|
43 |
t.transform.translation.x = x; |
|
44 |
t.transform.translation.y = y; |
|
45 |
|
|
46 |
t.transform.rotation = tf::createQuaternionFromYaw(theta); |
|
47 |
t.header.stamp = nh.now(); |
|
48 |
|
|
49 |
broadcaster.sendTransform(t); |
|
50 |
nh.spinOnce(); |
|
51 |
|
|
52 |
delay(10); |
|
53 |
} |
quad2/arduino/src/ros_lib/examples/ServiceClient/ServiceClient.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial Service Client |
|
3 |
*/ |
|
4 |
|
|
5 |
#include <ros.h> |
|
6 |
#include <std_msgs/String.h> |
|
7 |
#include <rosserial_arduino/Test.h> |
|
8 |
|
|
9 |
ros::NodeHandle nh; |
|
10 |
using rosserial_arduino::Test; |
|
11 |
|
|
12 |
ros::ServiceClient<Test::Request, Test::Response> client("test_srv"); |
|
13 |
|
|
14 |
std_msgs::String str_msg; |
|
15 |
ros::Publisher chatter("chatter", &str_msg); |
|
16 |
|
|
17 |
char hello[13] = "hello world!"; |
|
18 |
|
|
19 |
void setup() |
|
20 |
{ |
|
21 |
nh.initNode(); |
|
22 |
nh.serviceClient(client); |
|
23 |
nh.advertise(chatter); |
|
24 |
while(!nh.connected()) nh.spinOnce(); |
|
25 |
nh.loginfo("Startup complete"); |
|
26 |
} |
|
27 |
|
|
28 |
void loop() |
|
29 |
{ |
|
30 |
Test::Request req; |
|
31 |
Test::Response res; |
|
32 |
req.input = hello; |
|
33 |
client.call(req, res); |
|
34 |
str_msg.data = res.output; |
|
35 |
chatter.publish( &str_msg ); |
|
36 |
nh.spinOnce(); |
|
37 |
delay(100); |
|
38 |
} |
quad2/arduino/src/ros_lib/examples/ServiceClient/client.py | ||
---|---|---|
1 |
#!/usr/bin/env python |
|
2 |
|
|
3 |
""" |
|
4 |
Sample code to use with ServiceClient.pde |
|
5 |
""" |
|
6 |
|
|
7 |
import roslib; roslib.load_manifest("rosserial_arduino") |
|
8 |
import rospy |
|
9 |
|
|
10 |
from rosserial_arduino.srv import * |
|
11 |
|
|
12 |
def callback(req): |
|
13 |
print "The arduino is calling! Please send it a message:" |
|
14 |
t = TestResponse() |
|
15 |
t.output = raw_input() |
|
16 |
return t |
|
17 |
|
|
18 |
rospy.init_node("service_client_test") |
|
19 |
rospy.Service("test_srv", Test, callback) |
|
20 |
rospy.spin() |
quad2/arduino/src/ros_lib/examples/ServiceServer/ServiceServer.pde | ||
---|---|---|
1 |
/* |
|
2 |
* rosserial Service Server |
|
3 |
*/ |
|
4 |
|
|
5 |
#include <ros.h> |
|
6 |
#include <std_msgs/String.h> |
|
7 |
#include <rosserial_arduino/Test.h> |
|
8 |
|
|
9 |
ros::NodeHandle nh; |
|
10 |
using rosserial_arduino::Test; |
|
11 |
|
|
12 |
int i; |
|
13 |
void callback(const Test::Request & req, Test::Response & res){ |
|
14 |
if((i++)%2) |
|
15 |
res.output = "hello"; |
|
16 |
else |
|
17 |
res.output = "world"; |
|
18 |
} |
|
19 |
|
|
20 |
ros::ServiceServer<Test::Request, Test::Response> server("test_srv",&callback); |
|
21 |
|
|
22 |
std_msgs::String str_msg; |
|
23 |
ros::Publisher chatter("chatter", &str_msg); |
|
24 |
|
|
25 |
char hello[13] = "hello world!"; |
|
26 |
|
|
27 |
void setup() |
|
28 |
{ |
|
29 |
nh.initNode(); |
|
30 |
nh.advertiseService(server); |
|
31 |
nh.advertise(chatter); |
|
32 |
} |
|
33 |
|
|
34 |
void loop() |
|
35 |
{ |
|
36 |
str_msg.data = hello; |
|
37 |
chatter.publish( &str_msg ); |
|
38 |
nh.spinOnce(); |
|
39 |
delay(10); |
|
40 |
} |
Also available in: Unified diff