Quadrotor

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

/*

 *

 * This is a modified version of ArduinoHardware.h to use Serial_ instead of

 * HardwareSerial for the Leonardo

 *

 */

#ifndef ROS_ARDUINO_HARDWARE_H_

#define ROS_ARDUINO_HARDWARE_H_

#include "WProgram.h"

#include <HardwareSerial.h>

class ArduinoHardware {

  public:

    ArduinoHardware(Serial_* io , long baud= 57600){

      iostream = io;

      baud_ = baud;

    }

    ArduinoHardware()

    {

      iostream = &Serial;

      baud_ = 57600;

    }

    ArduinoHardware(ArduinoHardware& h){

      this->iostream = iostream;

      this->baud_ = h.baud_;

    }

    void setBaud(long baud){

      this->baud_= baud;

    }

    int getBaud(){return baud_;}

    void init(){

      iostream->begin(baud_);

    }

    int read(){return iostream->read();};

    void write(uint8_t* data, int length){

      for(int i=0; i<length; i++)

        iostream->write(data[i]);

    }

    unsigned long time(){return millis();}

  protected:

    Serial_* iostream;

    long baud_;

};

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef ROS_ARDUINO_HARDWARE_H_

#define ROS_ARDUINO_HARDWARE_H_

#include "WProgram.h"

#include <HardwareSerial.h>

class ArduinoHardware {

  public:

    ArduinoHardware(HardwareSerial* io , long baud= 57600){

      iostream = io;

      baud_ = baud;

    }

    ArduinoHardware()

    {

      iostream = &Serial;

      baud_ = 57600;

    }

    ArduinoHardware(ArduinoHardware& h){

      this->iostream = iostream;

      this->baud_ = h.baud_;

    }

    void setBaud(long baud){

      this->baud_= baud;

    }

    int getBaud(){return baud_;}

    void init(){

      iostream->begin(baud_);

    }

    int read(){return iostream->read();};

    void write(uint8_t* data, int length){

      for(int i=0; i<length; i++)

        iostream->write(data[i]);

    }

    unsigned long time(){return millis();}

  protected:

    HardwareSerial* iostream;

    long baud_;

};

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#ifndef ROS_ARDUINO_HARDWARE_H_

#define ROS_ARDUINO_HARDWARE_H_

#include "WProgram.h"

#include <HardwareSerial.h>

class ArduinoHardware {

  public:

    ArduinoHardware(HardwareSerial* io , long baud= 57600){

      iostream = io;

      baud_ = baud;

    }

    ArduinoHardware()

    {

      iostream = &Serial;

      baud_ = 57600;

    }

    ArduinoHardware(ArduinoHardware& h){

      this->iostream = iostream;

      this->baud_ = h.baud_;

    }

    void setBaud(long baud){

      this->baud_= baud;

    }

    int getBaud(){return baud_;}

    void init(){

      iostream->begin(baud_);

    }

    int read(){return iostream->read();};

    void write(uint8_t* data, int length){

      for(int i=0; i<length; i++)

        iostream->write(data[i]);

    }

    unsigned long time(){return millis();}

  protected:

    HardwareSerial* iostream;

    long baud_;

};

#endif

/* 

 * Software License Agreement (BSD License)

 *

 * Copyright (c) 2011, Willow Garage, Inc.

 * All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 *

 *  * Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 *  * Redistributions in binary form must reproduce the above

 *    copyright notice, this list of conditions and the following

 *    disclaimer in the documentation and/or other materials provided

 *    with the distribution.

 *  * Neither the name of Willow Garage, Inc. nor the names of its

 *    contributors may be used to endorse or promote prducts derived

 *    from this software without specific prior written permission.

 *

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE

 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

 * POSSIBILITY OF SUCH DAMAGE.

 */

#include <math.h>

#include "ros/duration.h"

namespace ros

{

  void normalizeSecNSecSigned(long &sec, long &nsec)

  {

    long nsec_part = nsec;

    long sec_part = sec;

    while (nsec_part > 1000000000L)

    {

      nsec_part -= 1000000000L;

      ++sec_part;

    }

    while (nsec_part < 0)

    {

      nsec_part += 1000000000L;

      --sec_part;

    }

    sec = sec_part;

    nsec = nsec_part;

  }

  Duration& Duration::operator+=(const Duration &rhs)

  {

    sec += rhs.sec;

    nsec += rhs.nsec;

    normalizeSecNSecSigned(sec, nsec);

    return *this;

  }

  Duration& Duration::operator-=(const Duration &rhs){

    sec += -rhs.sec;

    nsec += -rhs.nsec;

    normalizeSecNSecSigned(sec, nsec);

    return *this;

  }

  Duration& Duration::operator*=(double scale){

    sec *= scale;

    nsec *= scale;

    normalizeSecNSecSigned(sec, nsec);

    return *this;

  }

}

/* 

 * rosserial ADC Example

 * 

 * This is a poor man's Oscilloscope.  It does not have the sampling 

 * rate or accuracy of a commerical scope, but it is great to get

 * an analog value into ROS in a pinch.

 */

#include <WProgram.h>

#include <ros.h>

#include <rosserial_arduino/Adc.h>

ros::NodeHandle nh;

rosserial_arduino::Adc adc_msg;

ros::Publisher p("adc", &adc_msg);

void setup()

{ 

  pinMode(13, OUTPUT);

  nh.initNode();

  nh.advertise(p);

}

//We average the analog reading to elminate some of the noise

int averageAnalog(int pin){

  int v=0;

  for(int i=0; i<4; i++) v+= analogRead(pin);

  return v/4;

}

long adc_timer;

void loop()

{

  adc_msg.adc0 = averageAnalog(0);

  adc_msg.adc1 = averageAnalog(1);

  adc_msg.adc2 = averageAnalog(2);

  adc_msg.adc3 = averageAnalog(3);

  adc_msg.adc4 = averageAnalog(4);

  adc_msg.adc5 = averageAnalog(5);

  p.publish(&adc_msg);

  nh.spinOnce();

}

/* 

 * rosserial Subscriber Example

 * Blinks an LED on callback

 */

#include <ros.h>

#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

void messageCb( const std_msgs::Empty& toggle_msg){

  digitalWrite(13, HIGH-digitalRead(13));   // blink the led

}

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

void setup()

{ 

  pinMode(13, OUTPUT);

  nh.initNode();

  nh.subscribe(sub);

}

void loop()

{  

  nh.spinOnce();

  delay(1);

}

/*

*  RosSerial BlinkM Example

*  This program shows how to control a blinkm

*  from an arduino using RosSerial

*/

#include "WProgram.h" //include the Arduino library

#include <stdlib.h>

#include <ros.h>

#include <std_msgs/String.h>

//include Wire/ twi for the BlinkM

#include <Wire.h>

extern "C" { 

#include "utility/twi.h" 

}

#include "BlinkM_funcs.h"

const byte blinkm_addr = 0x09; //default blinkm address

void setLED( bool solid,  char color)

{

       	if (solid)

	{

           switch (color)

		{

		case 'w':  // white

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0xff);  

			break;

		case 'r': //RED

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0);  

			break;

		case 'g':// Green

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0);

			break;

		case 'b':// Blue

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0,0,0xff);

			break;

		case 'c':// Cyan

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0,0xff,0xff);

			break;

		case 'm': // Magenta

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0xff,0,0xff);

			break;

		case 'y': // yellow

                        BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0xff,0xff,0);

			break;

		default: // Black

			BlinkM_stopScript( blinkm_addr );

			BlinkM_fadeToRGB( blinkm_addr, 0,0,0);

			break;

		}

	}

	else

	{

               	switch (color)

		{

		case 'r':  // Blink Red

                        BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 3,0,0 );

			break;

		case 'w':  // Blink white

                        BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 2,0,0 );

			break;

		case 'g':  // Blink Green

			BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 4,0,0 );

			break;

		case 'b': // Blink Blue

			BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 5,0,0 );

			break;

		case 'c': //Blink Cyan

			BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 6,0,0 );

			break;

		case 'm': //Blink Magenta

			BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 7,0,0 );

			break;

		case 'y': //Blink Yellow

                        BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 8,0,0 );

			break;

		default: //OFF

			BlinkM_stopScript( blinkm_addr );

			BlinkM_playScript( blinkm_addr, 9,0,0 );

			break;

		}

	}

}

void light_cb( const std_msgs::String& light_cmd){

        bool solid =false;

        char color; 

        if (strlen( (const char* ) light_cmd.data) ==2 ){

          solid  = (light_cmd.data[0] == 'S') || (light_cmd.data[0] == 's');

          color = light_cmd.data[1];

        }

        else{

          solid=  false;

          color = light_cmd.data[0];

        } 

	setLED(solid, color);

}

ros::NodeHandle  nh;

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

void setup()

{

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

	BlinkM_beginWithPower();

	delay(100);

	BlinkM_stopScript(blinkm_addr);  // turn off startup script

	setLED(false, 0); //turn off the led

        nh.initNode();

        nh.subscribe(sub);

}

void loop()

{

 nh.spinOnce();

 delay(1);

}

/*

 * BlinkM_funcs.h -- Arduino 'library' to control BlinkM

 * --------------

 *

 *

 * Note: original version of this file lives with the BlinkMTester sketch

 *

 * Note: all the functions are declared 'static' because 

 *       it saves about 1.5 kbyte in code space in final compiled sketch.  

 *       A C++ library of this costs a 1kB more.

 *

 * 2007-8, Tod E. Kurt, ThingM, http://thingm.com/

 *

 * version: 20081101

 *

 * history:

 *  20080101 - initial release

 *  20080203 - added setStartupParam(), bugfix receiveBytes() from Dan Julio

 *  20081101 - fixed to work with Arduino-0012, added MaxM commands,

 *             added test script read/write functions, cleaned up some functions

 *  20090121 - added I2C bus scan functions, has dependencies on private 

 *             functions inside Wire library, so might break in the future

 *  20100420 - added BlinkM_startPower and _stopPower

 *

 */

#include "WProgram.h"

#include "wiring.h"

#include <Wire.h>

extern "C" { 

#include "utility/twi.h"  // from Wire library, so we can do bus scanning

}

// format of light script lines: duration, command, arg1,arg2,arg3

typedef struct _blinkm_script_line {

  uint8_t dur;

  uint8_t cmd[4];    // cmd,arg1,arg2,arg3

} blinkm_script_line;

// Call this first (when powering BlinkM from a power supply)

static void BlinkM_begin()

{

  Wire.begin();                // join i2c bus (address optional for master)

}

/*

 * actually can't do this either, because twi_init() has THREE callocs in it too

 *

static void BlinkM_reset()

{

  twi_init();  // can't just call Wire.begin() again because of calloc()s there

}

*/

//

// each call to twi_writeTo() should return 0 if device is there

// or other value (usually 2) if nothing is at that address

// 

static void BlinkM_scanI2CBus(byte from, byte to, 

                              void(*callback)(byte add, byte result) ) 

{

  byte rc;

  byte data = 0; // not used, just an address to feed to twi_writeTo()

  for( byte addr = from; addr <= to; addr++ ) {

    rc = twi_writeTo(addr, &data, 0, 1);

    callback( addr, rc );

  }

}

//

//

static int8_t BlinkM_findFirstI2CDevice() 

{

  byte rc;

  byte data = 0; // not used, just an address to feed to twi_writeTo()

  for( byte addr=1; addr < 120; addr++ ) {  // only scan addrs 1-120

    rc = twi_writeTo(addr, &data, 0, 1);

    if( rc == 0 ) return addr; // found an address

  }

  return -1; // no device found in range given

}

// FIXME: make this more Arduino-like

static void BlinkM_startPowerWithPins(byte pwrpin, byte gndpin)

{

  DDRC |= _BV(pwrpin) | _BV(gndpin);  // make outputs

  PORTC &=~ _BV(gndpin);

  PORTC |=  _BV(pwrpin);

}

// FIXME: make this more Arduino-like

static void BlinkM_stopPowerWithPins(byte pwrpin, byte gndpin)

{

  DDRC &=~ (_BV(pwrpin) | _BV(gndpin));

}

//

static void BlinkM_startPower()

{

  BlinkM_startPowerWithPins( PORTC3, PORTC2 );

}

//

static void BlinkM_stopPower()

{

  BlinkM_stopPowerWithPins( PORTC3, PORTC2 );

}

// General version of BlinkM_beginWithPower().

// Call this first when BlinkM is plugged directly into Arduino

static void BlinkM_beginWithPowerPins(byte pwrpin, byte gndpin)

{

  BlinkM_startPowerWithPins(pwrpin,gndpin);

  delay(100);  // wait for things to stabilize

  Wire.begin();

}

// Call this first when BlinkM is plugged directly into Arduino

// FIXME: make this more Arduino-like

static void BlinkM_beginWithPower()

{

  BlinkM_beginWithPowerPins( PORTC3, PORTC2 );

}

// sends a generic command

static void BlinkM_sendCmd(byte addr, byte* cmd, int cmdlen)

{

  Wire.beginTransmission(addr);

  for( byte i=0; i<cmdlen; i++) 

    Wire.send(cmd[i]);

  Wire.endTransmission();

}

// receives generic data

// returns 0 on success, and -1 if no data available

// note: responsiblity of caller to know how many bytes to expect

static int BlinkM_receiveBytes(byte addr, byte* resp, byte len)

{

  Wire.requestFrom(addr, len);

  if( Wire.available() ) {

    for( int i=0; i<len; i++) 

      resp[i] = Wire.receive();

    return 0;

  }

  return -1;

}

// Sets the I2C address of the BlinkM.  

// Uses "general call" broadcast address

static void BlinkM_setAddress(byte newaddress)

{

  Wire.beginTransmission(0x00);  // general call (broadcast address)

  Wire.send('A');

  Wire.send(newaddress);

  Wire.send(0xD0);

  Wire.send(0x0D);  // dood!

  Wire.send(newaddress);

  Wire.endTransmission();

  delay(50); // just in case

}

// Gets the I2C address of the BlinKM

// Kind of redundant when sent to a specific address

// but uses to verify BlinkM communication

static int BlinkM_getAddress(byte addr)

{

  Wire.beginTransmission(addr);

  Wire.send('a');

  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)1);  // general call

  if( Wire.available() ) {

    byte b = Wire.receive();

    return b;

  }

  return -1;

}

// Gets the BlinkM firmware version

static int BlinkM_getVersion(byte addr)

{

  Wire.beginTransmission(addr);

  Wire.send('Z');

  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)2);

  if( Wire.available() ) {

    byte major_ver = Wire.receive();

    byte minor_ver = Wire.receive();

    return (major_ver<<8) + minor_ver;

  }

  return -1;

}

// Demonstrates how to verify you're talking to a BlinkM 

// and that you know its address

static int BlinkM_checkAddress(byte addr)

{

  //Serial.print("Checking BlinkM address...");

  int b = BlinkM_getAddress(addr);

  if( b==-1 ) {

    //Serial.println("No response, that's not good");

    return -1;  // no response

  } 

  //Serial.print("received addr: 0x");

  //Serial.print(b,HEX);

  if( b != addr )

    return 1; // error, addr mismatch 

  else 

    return 0; // match, everything okay

}

// Sets the speed of fading between colors.  

// Higher numbers means faster fading, 255 == instantaneous fading

static void BlinkM_setFadeSpeed(byte addr, byte fadespeed)

{

  Wire.beginTransmission(addr);

  Wire.send('f');

  Wire.send(fadespeed);

  Wire.endTransmission();  

}

// Sets the light script playback time adjust

// The timeadj argument is signed, and is an additive value to all

// durations in a light script. Set to zero to turn off time adjust.

static void BlinkM_setTimeAdj(byte addr, byte timeadj)

{

  Wire.beginTransmission(addr);

  Wire.send('t');

  Wire.send(timeadj);

  Wire.endTransmission();  

}

// Fades to an RGB color

static void BlinkM_fadeToRGB(byte addr, byte red, byte grn, byte blu)

{

  Wire.beginTransmission(addr);

  Wire.send('c');

  Wire.send(red);

  Wire.send(grn);

  Wire.send(blu);

  Wire.endTransmission();

}

// Fades to an HSB color

static void BlinkM_fadeToHSB(byte addr, byte hue, byte saturation, byte brightness)

{

  Wire.beginTransmission(addr);

  Wire.send('h');

  Wire.send(hue);

  Wire.send(saturation);

  Wire.send(brightness);

  Wire.endTransmission();

}

// Sets an RGB color immediately

static void BlinkM_setRGB(byte addr, byte red, byte grn, byte blu)

{

  Wire.beginTransmission(addr);

  Wire.send('n');

  Wire.send(red);

  Wire.send(grn);

  Wire.send(blu);

  Wire.endTransmission();

}

// Fades to a random RGB color

static void BlinkM_fadeToRandomRGB(byte addr, byte rrnd, byte grnd, byte brnd)

{

  Wire.beginTransmission(addr);

  Wire.send('C');

  Wire.send(rrnd);

  Wire.send(grnd);

  Wire.send(brnd);

  Wire.endTransmission();

}

// Fades to a random HSB color

static void BlinkM_fadeToRandomHSB(byte addr, byte hrnd, byte srnd, byte brnd)

{

  Wire.beginTransmission(addr);

  Wire.send('H');

  Wire.send(hrnd);

  Wire.send(srnd);

  Wire.send(brnd);

  Wire.endTransmission();

}

//

static void BlinkM_getRGBColor(byte addr, byte* r, byte* g, byte* b)

{

  Wire.beginTransmission(addr);

  Wire.send('g');

  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)3);

  if( Wire.available() ) {

    *r = Wire.receive();

    *g = Wire.receive();

    *b = Wire.receive();

  }

}

//

static void BlinkM_playScript(byte addr, byte script_id, byte reps, byte pos)

{

  Wire.beginTransmission(addr);

  Wire.send('p');

  Wire.send(script_id);

  Wire.send(reps);

  Wire.send(pos);

  Wire.endTransmission();

}

//

static void BlinkM_stopScript(byte addr)

{

  Wire.beginTransmission(addr);

  Wire.send('o');

  Wire.endTransmission();

}

//

static void BlinkM_setScriptLengthReps(byte addr, byte script_id, 

                                       byte len, byte reps)

{

  Wire.beginTransmission(addr);

  Wire.send('L');

  Wire.send(script_id);

  Wire.send(len);

  Wire.send(reps);

  Wire.endTransmission();

}

// Fill up script_line with data from a script line

// currently only script_id = 0 works (eeprom script)

static void BlinkM_readScriptLine(byte addr, byte script_id, 

                                  byte pos, blinkm_script_line* script_line)

{

  Wire.beginTransmission(addr);

  Wire.send('R');

  Wire.send(script_id);

  Wire.send(pos);

  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)5);

  while( Wire.available() < 5 ) ; // FIXME: wait until we get 7 bytes

  script_line->dur    = Wire.receive();

  script_line->cmd[0] = Wire.receive();

  script_line->cmd[1] = Wire.receive();

  script_line->cmd[2] = Wire.receive();

  script_line->cmd[3] = Wire.receive();

}

//

static void BlinkM_writeScriptLine(byte addr, byte script_id, 

                                   byte pos, byte dur,

                                   byte cmd, byte arg1, byte arg2, byte arg3)

{

#ifdef BLINKM_FUNCS_DEBUG

  Serial.print("writing line:");  Serial.print(pos,DEC);

  Serial.print(" with cmd:"); Serial.print(cmd); 

  Serial.print(" arg1:"); Serial.println(arg1,HEX);

#endif

  Wire.beginTransmission(addr);

  Wire.send('W');

  Wire.send(script_id);

  Wire.send(pos);

  Wire.send(dur);

  Wire.send(cmd);

  Wire.send(arg1);

  Wire.send(arg2);

  Wire.send(arg3);

  Wire.endTransmission();

}

//

static void BlinkM_writeScript(byte addr, byte script_id, 

                               byte len, byte reps,

                               blinkm_script_line* lines)

{

#ifdef BLINKM_FUNCS_DEBUG

  Serial.print("writing script to addr:"); Serial.print(addr,DEC);

  Serial.print(", script_id:"); Serial.println(script_id,DEC);

#endif

  for(byte i=0; i < len; i++) {

    blinkm_script_line l = lines[i];

    BlinkM_writeScriptLine( addr, script_id, i, l.dur,

                            l.cmd[0], l.cmd[1], l.cmd[2], l.cmd[3]);

    delay(20); // must wait for EEPROM to be programmed

  }

  BlinkM_setScriptLengthReps(addr, script_id, len, reps);

}

//

static void BlinkM_setStartupParams(byte addr, byte mode, byte script_id,

                                    byte reps, byte fadespeed, byte timeadj)

{

  Wire.beginTransmission(addr);

  Wire.send('B');

  Wire.send(mode);             // default 0x01 == Play script

  Wire.send(script_id);        // default 0x00 == script #0

  Wire.send(reps);             // default 0x00 == repeat infinitely

  Wire.send(fadespeed);        // default 0x08 == usually overridden by sketch 

  Wire.send(timeadj);          // default 0x00 == sometimes overridden by sketch

  Wire.endTransmission();

} 

// Gets digital inputs of the BlinkM

// returns -1 on failure

static int BlinkM_getInputsO(byte addr)

{

  Wire.beginTransmission(addr);

  Wire.send('i');

  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)1);

  if( Wire.available() ) {

    byte b = Wire.receive();

    return b; 

  }

  return -1;

}

// Gets digital inputs of the BlinkM

// stores them in passed in array

// returns -1 on failure

static int BlinkM_getInputs(byte addr, byte inputs[])

{

  Wire.beginTransmission(addr);

  Wire.send('i');

  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)4);

  while( Wire.available() < 4 ) ; // FIXME: wait until we get 4 bytes

  inputs[0] = Wire.receive();

  inputs[1] = Wire.receive();

  inputs[2] = Wire.receive();

  inputs[3] = Wire.receive();

  return 0;

}

/* 

 * rosserial Clapper Example

 * 

 * This code is a very simple example of the kinds of 

 * custom sensors that you can easily set up with rosserial

 * and Arduino.  This code uses a microphone attached to 

 * analog pin 5 detect two claps (2 loud sounds).

 * You can use this clapper, for example, to command a robot 

 * in the area to come do your bidding.

 */

#include <WProgram.h>

#include <ros.h>

#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

std_msgs::Empty clap_msg;

ros::Publisher p("clap", &clap_msg);

enum clapper_state { clap1, clap_one_waiting,  pause, clap2};

clapper_state clap;

int volume_thresh = 200;  //a clap sound needs to be: 

                          //abs(clap_volume) > average noise + volume_thresh

int mic_pin = 5;

int adc_ave=0;

void setup()

{ 

  pinMode(13, OUTPUT);

  nh.initNode();

  nh.advertise(p);

  //measure the average volume of the noise in the area

  for (int i =0; i<10;i++) adc_ave += analogRead(mic_pin);

  adc_ave /= 10;

}

long event_timer;

void loop()

{

  int mic_val = 0;

  for(int i=0; i<4; i++) mic_val += analogRead(mic_pin);

  mic_val = mic_val/4-adc_ave;

  switch(clap){

   case clap1:

    if (abs(mic_val) > volume_thresh){

       clap = clap_one_waiting; 

       event_timer = millis();

    }

    break;

    case clap_one_waiting:

      if ( (abs(mic_val) < 30) && ( (millis() - event_timer) > 20 ) )  

      {

        clap= pause;

        event_timer = millis();

      }

      break;

    case pause: // make sure there is a pause between 

                // the loud sounds

      if ( mic_val > volume_thresh) 

      {

        clap = clap1;

      }

      else if ( (millis()-event_timer)> 60)  {

        clap = clap2;

        event_timer = millis();

      }

      break;

     case clap2:

        if (abs(mic_val) > volume_thresh){ //we have got a double clap!

            clap = clap1;

            p.publish(&clap_msg);

        }

        else if ( (millis()-event_timer)> 200) {

          clap= clap1; // no clap detected, reset state machine

        }

        break;   

  }

  nh.spinOnce();

}

/*

 * rosserial Publisher Example

 * Prints "hello world!"

 */

#include <ros.h>

#include <std_msgs/String.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;

ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

void setup()

{

  nh.initNode();

  nh.advertise(chatter);

}

void loop()

{

  str_msg.data = hello;

  chatter.publish( &str_msg );

  nh.spinOnce();

  delay(1000);

}

/* 

 * rosserial IR Ranger Example  

 * 

 * This example is calibrated for the Sharp GP2D120XJ00F.

 */

#include <ros.h>

#include <ros/time.h>

#include <sensor_msgs/Range.h>

ros::NodeHandle  nh;

sensor_msgs::Range range_msg;

ros::Publisher pub_range( "range_data", &range_msg);

const int analog_pin = 0;

unsigned long range_timer;

/*

 * getRange() - samples the analog input from the ranger

 * and converts it into meters.  

 */

float getRange(int pin_num){

    int sample;

    // Get data

    sample = analogRead(pin_num)/4;

    // if the ADC reading is too low, 

    //   then we are really far away from anything

    if(sample < 10)

        return 254;     // max range

    // Magic numbers to get cm

    sample= 1309/(sample-3);

    return (sample - 1)/100; //convert to meters

}

char frameid[] = "/ir_ranger";

void setup()

{

  nh.initNode();

  nh.advertise(pub_range);

  range_msg.radiation_type = sensor_msgs::Range::INFRARED;

  range_msg.header.frame_id =  frameid;

  range_msg.field_of_view = 0.01;

  range_msg.min_range = 0.03;

  range_msg.max_range = 0.4;

}

void loop()

{

  // publish the range value every 50 milliseconds

  //   since it takes that long for the sensor to stabilize

  if ( (millis()-range_timer) > 50){

    range_msg.range = getRange(analog_pin);

    range_msg.header.stamp = nh.now();

    pub_range.publish(&range_msg);

    range_timer =  millis() + 50;

  }

  nh.spinOnce();

}

/*

 * rosserial PubSub Example

 * Prints "hello world!" and toggles led

 */

#include <ros.h>

#include <std_msgs/String.h>

#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;

ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

char debug[]= "debug statements";

char info[] = "infos";

char warn[] = "warnings";

char error[] = "errors";

char fatal[] = "fatalities";

void setup()

{

  pinMode(13, OUTPUT);

  nh.initNode();

  nh.advertise(chatter);

}

void loop()

{

  str_msg.data = hello;

  chatter.publish( &str_msg );

  nh.logdebug(debug);

  nh.loginfo(info);

  nh.logwarn(warn);

  nh.logerror(error);

  nh.logfatal(fatal);

  nh.spinOnce();

  delay(500);

}

/* 

 * rosserial Planar Odometry Example

 */

#include <ros.h>

#include <ros/time.h>

#include <tf/tf.h>

#include <tf/transform_broadcaster.h>

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;

tf::TransformBroadcaster broadcaster;

double x = 1.0;

double y = 0.0;

double theta = 1.57;

char base_link[] = "/base_link";

char odom[] = "/odom";

void setup()

{

  nh.initNode();

  broadcaster.init(nh);

}

void loop()

{  

  // drive in a circle

  double dx = 0.2;

  double dtheta = 0.18;

  x += cos(theta)*dx*0.1;

  y += sin(theta)*dx*0.1;

  theta += dtheta*0.1;

  if(theta > 3.14)

    theta=-3.14;

  // tf odom->base_link

  t.header.frame_id = odom;

  t.child_frame_id = base_link;

  t.transform.translation.x = x;

  t.transform.translation.y = y;

  t.transform.rotation = tf::createQuaternionFromYaw(theta);

  t.header.stamp = nh.now();

  broadcaster.sendTransform(t);

  nh.spinOnce();

  delay(10);

}

/*

 * rosserial Service Client

 */

#include <ros.h>

#include <std_msgs/String.h>

#include <rosserial_arduino/Test.h>

ros::NodeHandle  nh;

using rosserial_arduino::Test;

ros::ServiceClient<Test::Request, Test::Response> client("test_srv");

std_msgs::String str_msg;

ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

void setup()

{

  nh.initNode();

  nh.serviceClient(client);

  nh.advertise(chatter);

  while(!nh.connected()) nh.spinOnce();

  nh.loginfo("Startup complete");

}

void loop()

{

  Test::Request req;

  Test::Response res;

  req.input = hello;

  client.call(req, res);

  str_msg.data = res.output;

  chatter.publish( &str_msg );

  nh.spinOnce();

  delay(100);

}

#!/usr/bin/env python

""" 

Sample code to use with ServiceClient.pde

"""

import roslib; roslib.load_manifest("rosserial_arduino")

import rospy

from rosserial_arduino.srv import *

def callback(req):

    print "The arduino is calling! Please send it a message:"

    t = TestResponse()

    t.output = raw_input()

    return t

rospy.init_node("service_client_test")

rospy.Service("test_srv", Test, callback)

rospy.spin()

/*

 * rosserial Service Server

 */

#include <ros.h>

#include <std_msgs/String.h>

#include <rosserial_arduino/Test.h>

ros::NodeHandle  nh;

using rosserial_arduino::Test;

int i;

void callback(const Test::Request & req, Test::Response & res){

  if((i++)%2)

    res.output = "hello";

  else

    res.output = "world";

}

ros::ServiceServer<Test::Request, Test::Response> server("test_srv",&callback);

std_msgs::String str_msg;

ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

void setup()

{

  nh.initNode();

  nh.advertiseService(server);

  nh.advertise(chatter);

}

void loop()

{

  str_msg.data = hello;

  chatter.publish( &str_msg );

  nh.spinOnce();

  delay(10);

}