## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title =Motor Shield Example
## multi-line description to be displayed in search
## description =In this tutorial, we will use a Mbed and a Motor Controller Shield
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level= BeginnerCategory
## keywords =
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<Version()>>

<<TableOfContents(4)>>

[[rosserial]] allows you to easily integrate Mbed-based hardware with ROS. This tutorial will explain how to use a Motor Controller Shield with an Mbed.

You will need an [[https://developer.mbed.org/platforms|Mbed]], a [[http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html|Motor Control Shield]], and a way to connect your Sensor to your Mbed such as a breadboard or protoboard.

== The Code ==
Open the Motor Shield example placed on your previously downloaded rosserial/rosserial_mbed/examples directory, and open the MotorShield.cpp file in your favorite text editor:

{{{#!cplusplus block=motor
/*
 * rosserial Motor Shield Example
 *
 * This tutorial demonstrates the usage of the
 * Seeedstudio Motor Shield
 * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html
 *
 * Source Code Based on:
 * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/
 */

#include "mbed.h"
#include "MotorDriver.h"
#include <ros.h>
#include <geometry_msgs/Twist.h>

#ifdef TARGET_LPC1768
#define MOTORSHIELD_IN1     p21
#define MOTORSHIELD_IN2     p22
#define MOTORSHIELD_IN3     p23
#define MOTORSHIELD_IN4     p24
#define SPEEDPIN_A          p25
#define SPEEDPIN_B          p26
#elif defined(TARGET_KL25Z) || defined(TARGET_NUCLEO_F401RE)
#define MOTORSHIELD_IN1     D8
#define MOTORSHIELD_IN2     D11
#define MOTORSHIELD_IN3     D12
#define MOTORSHIELD_IN4     D13
#define SPEEDPIN_A          D9
#define SPEEDPIN_B          D10
#else
#error "You need to specify a pin for the sensor"
#endif

MotorDriver motorDriver(MOTORSHIELD_IN1, MOTORSHIELD_IN2, MOTORSHIELD_IN3, MOTORSHIELD_IN4, SPEEDPIN_A, SPEEDPIN_B);
ros::NodeHandle nh;

void messageCb(const geometry_msgs::Twist& msg)
{
  if (msg.angular.z == 0 && msg.linear.x == 0)
  {
    motorDriver.stop();
  }
  else
  {
    if (msg.angular.z < 0)
    {
      int speed = (int)(msg.angular.z * -100);
      motorDriver.setSpeed(speed, MOTORA);
      motorDriver.setSpeed(speed, MOTORB);
      motorDriver.goRight();
    }
    else if (msg.angular.z > 0)
    {
      int speed = (int)(msg.angular.z * 100);
      motorDriver.setSpeed(speed, MOTORA);
      motorDriver.setSpeed(speed, MOTORB);
      motorDriver.goLeft();
    }
    else if (msg.linear.x < 0)
    {
      int speed = (int)(msg.linear.x * -100);
      motorDriver.setSpeed(speed, MOTORA);
      motorDriver.setSpeed(speed, MOTORB);
      motorDriver.goBackward();
    }
    else if (msg.linear.x > 0)
    {
      int speed = (int)(msg.linear.x * 100);
      motorDriver.setSpeed(speed, MOTORA);
      motorDriver.setSpeed(speed, MOTORB);
      motorDriver.goForward();
    }
  }
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
Timer t;

int main()
{
  t.start();
  long vel_timer = 0;
  nh.initNode();
  nh.subscribe(sub);
  motorDriver.init();
  motorDriver.setSpeed(90, MOTORB);
  motorDriver.setSpeed(90, MOTORA);
  while (1)
  {
    if (t.read_ms() > vel_timer)
    {
      motorDriver.stop();
      vel_timer = t.read_ms() + 500;
    }
    nh.spinOnce();
    wait_ms(1);
  }
}
}}}

== Compiling and uploading the Code ==
As seen on the previous tutorial, you must compile the source code by using the makefile in order to generate the .bin file that will be uploaded later to your MBED board. This is no different from uploading any other mbed binary.

== Launching the App ==
Launch roscore
{{{
$ roscore
}}}

Run the serial node with the right serial port corresponding to your MBED board
{{{{{{#!wiki version hydro indigo jade
{{{
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0
}}}
}}}}}}
{{{{{{#!wiki version groovy
{{{
$ rosrun rosserial_python serial_node.py /dev/ttyUSB0
}}}
}}}}}}

And last, start publishing messages into the cmd_vel topic
{{{
$ rostopic pub /cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'
}}}

## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE