Revision 3a82529c

View differences:

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
}
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff