## repository: https://github.com/danielsnider/simple_drive
<<PackageHeader(simple_drive)>>

<<TOC(3)>>

== Overview ==

{{attachment:Simple_Drive_Diagram.png|simple_drive|width="750"}}

A simple robot drive system. '''Features include''':

 * skid steering joystick teleoperation with three drive speeds
 * joystick control of a servo (left and right only)
 * dedicated left and right thumbsticks for left and right wheel speeds
 * a cmd_vel multiplexer to support a coexisting autonomous drive system
 * simple Arduino firmware to talk to PWM motors and a servo

This package '''does not do the following''':

 * no [[tf]] publishing
 * no wheel odometry publishing
 * no PID control loop
 * no integration with [[ros_control]]

This package is divided into four parts:
||'''Nodes'''||'''Purpose'''||
||[[#drive_teleop|drive_teleop]]   ||Drive your robot with a joystick in skid steering mode (also known as diff drive or tank drive). Left joystick thumbstick controls left wheels, right thumbstick controls right wheels.      ||
||[[#cmd_vel_mux|cmd_vel_mux]]   ||Motor commands from multiple sources are forwarded with a preference for human control over autonomous commands.      ||
||[[#simple_drive|simple_drive]]   ||Send motor commands to your microcontroller.      ||
||[[#drive_firmware|drive_firmware]]   ||Microcontroller software that controls your motors. Not a ROS node.      ||

<<BR>>

{{attachment:R3 in Utah-low.JPG|R3 Rover|width="475"}}

Package created by Ryerson University students for the [[http://urc.marssociety.org/|University Rover Competition]], summer 2017.

== Quick Start ==
1. Install:

{{{
$ sudo apt-get install ros-kinetic-simple-drive
}}}

2. Launch ROS nodes:
{{{
$ roslaunch simple_drive drive_teleop.launch joy_dev:=/dev/input/js0
$ roslaunch simple_drive cmd_vel_mux.launch
$ roslaunch simple_drive simple_drive.launch serial_dev:=/dev/ttyACM0

OR all-in-one launch:
$ roslaunch simple_drive drive.launch
}}}

3. Install the [[#drive_firmware|drive_firmware]] onto a microcontroller connected to motors and wheels by PWM. The microcontroller must also be connected to the computer running the simple_drive ROS node by a serial connection (ex. USB).

4. Drive your robot around.

== Nodes ==
=== drive_teleop ===
{{attachment:Xbox_Controller.png|R3 Rover|width="475"}}

This diagram is also available in [[https://github.com/danielsnider/ros-rover/blob/master/diagrams/simple_drive_Xbox_Controller.vsdx?raw=true|Visio format]].

This node converts <<MsgLink(sensor_msgs/Joy)>> messages from the [[joy]] node into a variety of commands to drive the robot at low, medium, and high speed, look around with a servo, and cancel move_base goals at any moment. This node simply sends commands to other nodes. Typically the servo is used to move a camera so that the teleoperator can look around the robot.

The button mapping was tested on an Xbox 360 controller and should require little or no modification for similar controllers.

==== Launch example ====
{{{
$ roslaunch simple_drive drive_teleop.launch joy_dev:=/dev/input/js0
}}}

{{{
#!clearsilver CS/NodeAPI
sub {
  0{
  name = joy
  type = sensor_msgs/Joy
  desc = Controller joystick button presses are received on this topic. See [[joy]] node for more info.
  }
}
pub {
  0{
  name = teleop/cmd_vel
  type = geometry_msgs/Twist
  desc = Output motion commands that drive the robot.
  }
  1{
  name = servo_pos
  type = std_msgs/Float32
  desc = Output servo commands that control the angle of a servo.
  }
}
param {
  0{
  name = ~servo_pan_speed
  type = int
  desc = The degrees of servo motion per button press.
  default = 5
  }
  1{
  name = ~servo_pan_max
  type = int
  desc = The maximum angle of servo rotation in degrees. 
  default = 160
  }
  2{
  name = ~servo_pan_min
  type = int
  desc = The minimum angle of servo rotation in degrees.
  default = 0
  }
}
}}}
----
=== cmd_vel_mux ===

The `cmd_vel_mux` node receives movement commands on two <<MsgLink(sensor_msgs/Twist)>> topics, one for teleoperation and one for autonomous control, typically [[move_base]]. Movement commands are multiplexed to a final topic for robot consumption. If any teleoperation command is received autonomous commands are blocked for a set time defined by the `block_duration` parameter.

==== Launch example ====
{{{
$ roslaunch simple_drive cmd_vel_mux.launch
}}}
{{{
#!clearsilver CS/NodeAPI
sub {
  0{
  name = teleop/cmd_vel
  type = geometry_msgs/Twist
  desc = Human generated movement commands from a teleoperation node such as the [[#drive_teleop|drive_teleop]] node.
  }
  1{
  name = move_base/cmd_vel
  type = geometry_msgs/Twist
  desc = Computer generated movement commands from an autonomous system such as [[move_base]] or [[rtabmap_ros]].
  }
}
pub {
  0{
  name = cmd_vel
  type = geometry_msgs/Twist
  desc = Movement commands for consumption by the robot. Autonomous movement commands will be blocked for `block_duration` seconds if any teleoperation command is received.
  }
}
param {
  0{
  name = ~block_duration
  type = int
  desc = The amount of time autonomous movement is blocked after any human movement is received.
  default = 5
  }
}
}}}
----
=== simple_drive ===

This node communicates with the [[#drive_firmware|drive_firmware]] using a custom serial protocol described below. An example serial data packet could be 0,0.5,0.5 which would mean drive motors forward at half speed and rotate at half speed.  

==== Serial Protocol ====

||'''Action''' || '''Serial data packet'''||
||Twist motor command||`(BYTE) 0, (FLOAT) LINEAR_VELOCITY, (FLOAT) ANGULAR_VELOCITY`||
||Servo position command||`(BYTE) 2, (FLOAT) SERVO_ANGLE`||

{i} If your microcontroller supports subscribing to ROS `twist` messages (Arduinos can use [[rosserial_arduino]]) then it would be simpler to do that and skip this node. However, this node is written in python so you could more easily add complex functionality in python and then in your microcontroller do the minimum amount of work necessary.

==== Launch example ====
{{{
$ roslaunch simple_drive simple_drive.launch serial_dev:=/dev/ttyACM0
}}}
{{{
#!clearsilver CS/NodeAPI
sub {
  0{
  name = cmd_vel
  type = geometry_msgs/Twist
  desc = Motor commands received on this topic are encoded and sent over serial to the [[#drive_firmware|drive_firmware]].
  }
  1{
  name = servo_pos
  type = std_msgs/Float32
  desc = Servo position commands received on this topic are encoded and sent over serial to the [[#drive_firmware|drive_firmware]].
  }
}
param {
  0{
  name = ~serial_dev
  type = string
  desc = The microcontroller, often an Arduino, as seen in linux, that this node will send motor commands to.
  default = /dev/ttyACM0
  }
  1{
  name = ~baudrate
  type = int
  desc = Sets the serial device data transmission rate in bits per second (baud).
  default = 9600
  }
}
}}}
----
== drive_firmware ==

The `drive_firmware` microcontroller code does the minimum amount of work possible to receive motor commands from a USB serial connection and output voltages to digital PWM output to be received by motor controllers. 

We connected an Arduino by USB serial to our main robot computer. We tested on a Arduino Mega 2560 and Arduino Due, however many other boards should work with the same code and setup steps thanks to PlatformIO. You may need to change to change the pin numbers.

/!\ Caution! This software does not stop moving the robot if no messages are received for certain period of time. A pull request for this is very welcome.

=== PlatformIO ===

We deploy the `drive_firmware` to an Arduino microcontroller using PlatformIO.

{i} We like PlatformIO: "Single source code. Multiple platforms." PlatformIO supports approximately 200[[http://platformio.org/boards|Embedded Boards]] and all major[[http://docs.platformio.org/en/latest/platforms/index.html#platforms|Development Platforms]]. Learn more on[[http://platformio.org/|platformio.org]].

=== Install and configure drive_firmware ===

This steps were tested on Ubuntu 16.04.

'''1. Install PlatformIO'''

{{{
$ sudo python -c "$(curl -fsSL https://raw.githubusercontent.com/platformio/platformio/master/scripts/get-platformio.py)"

# Enable Access to Serial Ports (USB/UART)
$ sudo usermod -a -G dialout <your username here>
$ curl https://raw.githubusercontent.com/platformio/platformio/develop/scripts/99-platformio-udev.rules  > /etc/udev/rules.d/99-platformio-udev.rules
# After this file is installed, physically unplug and reconnect your board.
$ sudo service udev restart
}}}

More PlatformIO install info: http://docs.platformio.org/en/latest/installation.html#super-quick-mac-linux

'''2. Create a PlatformIO project'''

{{{
$ roscd simple_drive
$ cd ./drive_firmware/
# Find the microcontroller that you have in the list of PlatformIO boards
$ pio boards | grep -i mega2560
# Use the name of your board to initialize your project
$ pio init --board megaatmega2560
}}}

More PlatformIO info: http://docs.platformio.org/en/latest/quickstart.html

'''3. Modify wiring if necessary'''

{{{
$ vim src/main.cpp +4
}}}

Depending on how you want to wire your microcontroller this wiring could be changed:

{{{#!highlight c++
// Pins to Left Wheels
#define pinL1 13
#define pinL2 12
#define pinL3 11
// Pins to Right Wheels
#define pinR1 9
#define pinR2 8
#define pinR3 7
// Pin to the Servo
#define pinServo 5
}}}

'''4. Modify PWM Settings'''

{{{
$ vim src/main.cpp +17
}}}

Edit the following section to match your motor controller PWM specs:

{{{#!highlight c++
// PWM specs of the Spark motor controller. Spark manual:
//      http://www.revrobotics.com/content/docs/LK-ATFF-SXAO-UM.pdf
#define sparkMax 1000 // Default full-reverse input pulse
#define sparkMin 2000 // Default full-forward input pulse}}}

'''5. Deploy Code to Arduino'''

{{{
$ pio run --target upload
}}}

'''6. Drive your robot!''' B-)

Have fun!

== Other Implementation Details ==
=== Tank to Twist Calculation ===

When a left and right joystick inputs are received by the [[#drive_teleop|drive_teleop]] node, representing left and right wheel velocities (ie. skid steering or differential drive), a <<MsgLink(geometry_msgs/Twist)>> with linear and rotational velocities is calculated as:
 Linear velocity (m/s) =::
 :: `(left_speed + right_speed) / 2.0`
 Angular velocity (rad/s) =::
 :: `(right_speed - left_speed) / 2.0`

=== Twist to Tank Calculation ===

When a <<MsgLink(geometry_msgs/Twist)>> containing linear and rotational velocities is received by the [[#drive_firmware|drive_firmware]], wheel velocities are calculated as:
 Left wheel velocity (m/s) =::
 :: `linear_speed + angular_speed`
 Right wheel velocity (m/s) =::
 :: `linear_speed - angular_speed`

== Related Packages ==

'''Similar to [[#simple_drive|simple_drive]]:'''

http://wiki.ros.org/differential_drive - Differential drive software with support for a velocity PID target, a small GUI to control the robot, and more.

http://wiki.ros.org/diff_drive_controller - Differential drive software that is real-time safe, integrates with [[ros_control]], and more.

'''Similar to [[#drive_teleop|drive_teleop]]:'''

http://wiki.ros.org/teleop_twist_joy - This teleop node converts joy messages to twist messages.

http://wiki.ros.org/joy_teleop - This teleop node takes joy messages and publishes topics or calls actions according a configuration file.

'''Similar to [[#cmd_vel_mux|cmd_vel_mux]]:'''

http://wiki.ros.org/twist_mux - Multiplex several velocity command topics with prioritization or disabling according to a configuration file.

== Feedback ==

Feature requests, bug reports, and contributions are welcome at https://github.com/danielsnider/simple_drive.

Special thanks to anyone who sends a picture of this package in action on their robot!