-
-
-
@@ -20,7 +17,6 @@
-
@@ -29,7 +25,6 @@
-
@@ -38,14 +33,12 @@
-
-
@@ -69,7 +62,6 @@
-
@@ -77,7 +69,6 @@
-
diff --git a/catkin_ws/src/00-infrastructure/duckietown/launch/joystick_old.launch b/catkin_ws/src/00-infrastructure/duckietown/launch/old/joystick_old.launch
similarity index 100%
rename from catkin_ws/src/00-infrastructure/duckietown/launch/joystick_old.launch
rename to catkin_ws/src/00-infrastructure/duckietown/launch/old/joystick_old.launch
diff --git a/catkin_ws/src/00-infrastructure/duckietown/launch/open_loop_intersection_controller.launch b/catkin_ws/src/00-infrastructure/duckietown/launch/open_loop_intersection_controller.launch
index 557c723852..ea195f026d 100644
--- a/catkin_ws/src/00-infrastructure/duckietown/launch/open_loop_intersection_controller.launch
+++ b/catkin_ws/src/00-infrastructure/duckietown/launch/open_loop_intersection_controller.launch
@@ -1,26 +1,20 @@
-
-
-
-
@@ -34,24 +31,15 @@
-
-
-
-
-
-
-
-
-
@@ -65,7 +53,6 @@
-
@@ -75,7 +62,6 @@
-
diff --git a/catkin_ws/src/00-infrastructure/duckietown/launch/vehicle_camera_detection_filter_control_joystick.launch b/catkin_ws/src/00-infrastructure/duckietown/launch/vehicle_camera_detection_filter_control_joystick.launch
index abd7fd3500..efdce624ad 100644
--- a/catkin_ws/src/00-infrastructure/duckietown/launch/vehicle_camera_detection_filter_control_joystick.launch
+++ b/catkin_ws/src/00-infrastructure/duckietown/launch/vehicle_camera_detection_filter_control_joystick.launch
@@ -1,18 +1,12 @@
-
-
-
-
-
-
@@ -41,20 +35,10 @@
-
-
-
-
-
-
-
-
-
-
@@ -64,7 +48,6 @@
-
diff --git a/catkin_ws/src/00-infrastructure/duckietown/launch/wheels_driver.launch b/catkin_ws/src/00-infrastructure/duckietown/launch/wheels_driver.launch
deleted file mode 100644
index f2fffe97e3..0000000000
--- a/catkin_ws/src/00-infrastructure/duckietown/launch/wheels_driver.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/joy_mapper/.autogenerated.md b/catkin_ws/src/01-interfaces/joy_mapper/.autogenerated.md
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/.autogenerated.md
rename to catkin_ws/src/01-interfaces/joy_mapper/.autogenerated.md
diff --git a/catkin_ws/src/05-teleop/joy_mapper/CMakeLists.txt b/catkin_ws/src/01-interfaces/joy_mapper/CMakeLists.txt
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/CMakeLists.txt
rename to catkin_ws/src/01-interfaces/joy_mapper/CMakeLists.txt
diff --git a/catkin_ws/src/05-teleop/joy_mapper/README.md b/catkin_ws/src/01-interfaces/joy_mapper/README.md
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/README.md
rename to catkin_ws/src/01-interfaces/joy_mapper/README.md
diff --git a/catkin_ws/src/05-teleop/joy_mapper/launch/joy_node.launch b/catkin_ws/src/01-interfaces/joy_mapper/launch/joy_mapper_node.launch
similarity index 54%
rename from catkin_ws/src/05-teleop/joy_mapper/launch/joy_node.launch
rename to catkin_ws/src/01-interfaces/joy_mapper/launch/joy_mapper_node.launch
index 2628068d0e..892fd73bc9 100644
--- a/catkin_ws/src/05-teleop/joy_mapper/launch/joy_node.launch
+++ b/catkin_ws/src/01-interfaces/joy_mapper/launch/joy_mapper_node.launch
@@ -1,18 +1,18 @@
-
-
+
+
+
-
-
-
+
+
-
+
-
+
\ No newline at end of file
diff --git a/catkin_ws/src/05-teleop/joy_mapper/package.xml b/catkin_ws/src/01-interfaces/joy_mapper/package.xml
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/package.xml
rename to catkin_ws/src/01-interfaces/joy_mapper/package.xml
diff --git a/catkin_ws/src/05-teleop/joy_mapper/src/copyOF_joy_mapper_node.py b/catkin_ws/src/01-interfaces/joy_mapper/src/copyOF_joy_mapper_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/src/copyOF_joy_mapper_node.py
rename to catkin_ws/src/01-interfaces/joy_mapper/src/copyOF_joy_mapper_node.py
diff --git a/catkin_ws/src/05-teleop/joy_mapper/src/joy_mapper_node.py b/catkin_ws/src/01-interfaces/joy_mapper/src/joy_mapper_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/src/joy_mapper_node.py
rename to catkin_ws/src/01-interfaces/joy_mapper/src/joy_mapper_node.py
diff --git a/catkin_ws/src/05-teleop/joy_mapper/src/joy_mapper_node2.easy_node.yaml b/catkin_ws/src/01-interfaces/joy_mapper/src/joy_mapper_node2.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/src/joy_mapper_node2.easy_node.yaml
rename to catkin_ws/src/01-interfaces/joy_mapper/src/joy_mapper_node2.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/joy_mapper/src/joy_mapper_node2.py b/catkin_ws/src/01-interfaces/joy_mapper/src/joy_mapper_node2.py
similarity index 100%
rename from catkin_ws/src/05-teleop/joy_mapper/src/joy_mapper_node2.py
rename to catkin_ws/src/01-interfaces/joy_mapper/src/joy_mapper_node2.py
diff --git a/catkin_ws/src/05-teleop/00_part.md b/catkin_ws/src/05-teleop/00_part.md
deleted file mode 100644
index af5ea0e388..0000000000
--- a/catkin_ws/src/05-teleop/00_part.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# Packages - Teleoperation {#part:teleoperation}
-
-TODO: to write
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/.autogenerated.md b/catkin_ws/src/05-teleop/adafruit_drivers/.autogenerated.md
deleted file mode 100644
index 0f556ecfc6..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/.autogenerated.md
+++ /dev/null
@@ -1,23 +0,0 @@
-
-
-
-
-
-## Package information
-
-[Link to package on Github](github:org=duckietown,repo=Software,path=05-teleop/adafruit_drivers,branch=master18)
-
-### Essentials {nonumber="1"}
-
-Author: [Dmitry Yershov](mailto:yershov@mit.edu)
-
-Maintainer: [Mack](mailto:mack@duckietown.org)
-
-### Description {nonumber="1"}
-
-TODO: Add a description of package `adafruit_drivers` in `package.xml`.
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/CMakeLists.txt b/catkin_ws/src/05-teleop/adafruit_drivers/CMakeLists.txt
deleted file mode 100644
index 7d1a3aa382..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/CMakeLists.txt
+++ /dev/null
@@ -1,17 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(adafruit_drivers)
-
-find_package(catkin REQUIRED COMPONENTS
- # roscpp
- rospy
- # duckietown_msgs # Every duckietown packages should use this.
-)
-
-catkin_python_setup()
-
-
-catkin_package()
-
-include_directories(
- ${catkin_INCLUDE_DIRS}
-)
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/README.md b/catkin_ws/src/05-teleop/adafruit_drivers/README.md
deleted file mode 100644
index b105f52e3e..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/README.md
+++ /dev/null
@@ -1,8 +0,0 @@
-# Package `adafruit_drivers` {#adafruit_drivers}
-
-
-
-
-These are the Adafruit drivers.
-
-TODO: What is the original location of this package?
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_ADS1x15/Adafruit_ADS1x15.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_ADS1x15/Adafruit_ADS1x15.py
deleted file mode 100644
index eb58087d81..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_ADS1x15/Adafruit_ADS1x15.py
+++ /dev/null
@@ -1,726 +0,0 @@
-#!/usr/bin/python
-
-import time
-import smbus
-from Adafruit_I2C import Adafruit_I2C
-
-# ===========================================================================
-# ADS1x15 Class
-#
-# Originally written by K. Townsend, Adafruit (https://github.com/adafruit/Adafruit-Raspberry-Pi-Python-Code/tree/master/Adafruit_ADS1x15)
-# Updates and new functions implementation by Pedro Villanueva, 03/2013.
-# The only error in the original code was in line 57:
-# __ADS1015_REG_CONFIG_DR_920SPS = 0x0050
-# should be
-# __ADS1015_REG_CONFIG_DR_920SPS = 0x0060
-#
-# NOT IMPLEMENTED: Conversion ready pin, page 15 datasheet.
-# ===========================================================================
-
-class ADS1x15:
- i2c = None
-
- # IC Identifiers
- __IC_ADS1015 = 0x00
- __IC_ADS1115 = 0x01
-
- # Pointer Register
- __ADS1015_REG_POINTER_MASK = 0x03
- __ADS1015_REG_POINTER_CONVERT = 0x00
- __ADS1015_REG_POINTER_CONFIG = 0x01
- __ADS1015_REG_POINTER_LOWTHRESH = 0x02
- __ADS1015_REG_POINTER_HITHRESH = 0x03
-
- # Config Register
- __ADS1015_REG_CONFIG_OS_MASK = 0x8000
- __ADS1015_REG_CONFIG_OS_SINGLE = 0x8000 # Write: Set to start a single-conversion
- __ADS1015_REG_CONFIG_OS_BUSY = 0x0000 # Read: Bit = 0 when conversion is in progress
- __ADS1015_REG_CONFIG_OS_NOTBUSY = 0x8000 # Read: Bit = 1 when device is not performing a conversion
-
- __ADS1015_REG_CONFIG_MUX_MASK = 0x7000
- __ADS1015_REG_CONFIG_MUX_DIFF_0_1 = 0x0000 # Differential P = AIN0, N = AIN1 (default)
- __ADS1015_REG_CONFIG_MUX_DIFF_0_3 = 0x1000 # Differential P = AIN0, N = AIN3
- __ADS1015_REG_CONFIG_MUX_DIFF_1_3 = 0x2000 # Differential P = AIN1, N = AIN3
- __ADS1015_REG_CONFIG_MUX_DIFF_2_3 = 0x3000 # Differential P = AIN2, N = AIN3
- __ADS1015_REG_CONFIG_MUX_SINGLE_0 = 0x4000 # Single-ended AIN0
- __ADS1015_REG_CONFIG_MUX_SINGLE_1 = 0x5000 # Single-ended AIN1
- __ADS1015_REG_CONFIG_MUX_SINGLE_2 = 0x6000 # Single-ended AIN2
- __ADS1015_REG_CONFIG_MUX_SINGLE_3 = 0x7000 # Single-ended AIN3
-
- __ADS1015_REG_CONFIG_PGA_MASK = 0x0E00
- __ADS1015_REG_CONFIG_PGA_6_144V = 0x0000 # +/-6.144V range
- __ADS1015_REG_CONFIG_PGA_4_096V = 0x0200 # +/-4.096V range
- __ADS1015_REG_CONFIG_PGA_2_048V = 0x0400 # +/-2.048V range (default)
- __ADS1015_REG_CONFIG_PGA_1_024V = 0x0600 # +/-1.024V range
- __ADS1015_REG_CONFIG_PGA_0_512V = 0x0800 # +/-0.512V range
- __ADS1015_REG_CONFIG_PGA_0_256V = 0x0A00 # +/-0.256V range
-
- __ADS1015_REG_CONFIG_MODE_MASK = 0x0100
- __ADS1015_REG_CONFIG_MODE_CONTIN = 0x0000 # Continuous conversion mode
- __ADS1015_REG_CONFIG_MODE_SINGLE = 0x0100 # Power-down single-shot mode (default)
-
- __ADS1015_REG_CONFIG_DR_MASK = 0x00E0
- __ADS1015_REG_CONFIG_DR_128SPS = 0x0000 # 128 samples per second
- __ADS1015_REG_CONFIG_DR_250SPS = 0x0020 # 250 samples per second
- __ADS1015_REG_CONFIG_DR_490SPS = 0x0040 # 490 samples per second
- __ADS1015_REG_CONFIG_DR_920SPS = 0x0060 # 920 samples per second
- __ADS1015_REG_CONFIG_DR_1600SPS = 0x0080 # 1600 samples per second (default)
- __ADS1015_REG_CONFIG_DR_2400SPS = 0x00A0 # 2400 samples per second
- __ADS1015_REG_CONFIG_DR_3300SPS = 0x00C0 # 3300 samples per second (also 0x00E0)
-
- __ADS1115_REG_CONFIG_DR_8SPS = 0x0000 # 8 samples per second
- __ADS1115_REG_CONFIG_DR_16SPS = 0x0020 # 16 samples per second
- __ADS1115_REG_CONFIG_DR_32SPS = 0x0040 # 32 samples per second
- __ADS1115_REG_CONFIG_DR_64SPS = 0x0060 # 64 samples per second
- __ADS1115_REG_CONFIG_DR_128SPS = 0x0080 # 128 samples per second
- __ADS1115_REG_CONFIG_DR_250SPS = 0x00A0 # 250 samples per second (default)
- __ADS1115_REG_CONFIG_DR_475SPS = 0x00C0 # 475 samples per second
- __ADS1115_REG_CONFIG_DR_860SPS = 0x00E0 # 860 samples per second
-
- __ADS1015_REG_CONFIG_CMODE_MASK = 0x0010
- __ADS1015_REG_CONFIG_CMODE_TRAD = 0x0000 # Traditional comparator with hysteresis (default)
- __ADS1015_REG_CONFIG_CMODE_WINDOW = 0x0010 # Window comparator
-
- __ADS1015_REG_CONFIG_CPOL_MASK = 0x0008
- __ADS1015_REG_CONFIG_CPOL_ACTVLOW = 0x0000 # ALERT/RDY pin is low when active (default)
- __ADS1015_REG_CONFIG_CPOL_ACTVHI = 0x0008 # ALERT/RDY pin is high when active
-
- __ADS1015_REG_CONFIG_CLAT_MASK = 0x0004 # Determines if ALERT/RDY pin latches once asserted
- __ADS1015_REG_CONFIG_CLAT_NONLAT = 0x0000 # Non-latching comparator (default)
- __ADS1015_REG_CONFIG_CLAT_LATCH = 0x0004 # Latching comparator
-
- __ADS1015_REG_CONFIG_CQUE_MASK = 0x0003
- __ADS1015_REG_CONFIG_CQUE_1CONV = 0x0000 # Assert ALERT/RDY after one conversions
- __ADS1015_REG_CONFIG_CQUE_2CONV = 0x0001 # Assert ALERT/RDY after two conversions
- __ADS1015_REG_CONFIG_CQUE_4CONV = 0x0002 # Assert ALERT/RDY after four conversions
- __ADS1015_REG_CONFIG_CQUE_NONE = 0x0003 # Disable the comparator and put ALERT/RDY in high state (default)
-
-
- # Dictionaries with the sampling speed values
- # These simplify and clean the code (avoid the abuse of if/elif/else clauses)
- spsADS1115 = {
- 8:__ADS1115_REG_CONFIG_DR_8SPS,
- 16:__ADS1115_REG_CONFIG_DR_16SPS,
- 32:__ADS1115_REG_CONFIG_DR_32SPS,
- 64:__ADS1115_REG_CONFIG_DR_64SPS,
- 128:__ADS1115_REG_CONFIG_DR_128SPS,
- 250:__ADS1115_REG_CONFIG_DR_250SPS,
- 475:__ADS1115_REG_CONFIG_DR_475SPS,
- 860:__ADS1115_REG_CONFIG_DR_860SPS
- }
- spsADS1015 = {
- 128:__ADS1015_REG_CONFIG_DR_128SPS,
- 250:__ADS1015_REG_CONFIG_DR_250SPS,
- 490:__ADS1015_REG_CONFIG_DR_490SPS,
- 920:__ADS1015_REG_CONFIG_DR_920SPS,
- 1600:__ADS1015_REG_CONFIG_DR_1600SPS,
- 2400:__ADS1015_REG_CONFIG_DR_2400SPS,
- 3300:__ADS1015_REG_CONFIG_DR_3300SPS
- }
- # Dictionariy with the programable gains
- pgaADS1x15 = {
- 6144:__ADS1015_REG_CONFIG_PGA_6_144V,
- 4096:__ADS1015_REG_CONFIG_PGA_4_096V,
- 2048:__ADS1015_REG_CONFIG_PGA_2_048V,
- 1024:__ADS1015_REG_CONFIG_PGA_1_024V,
- 512:__ADS1015_REG_CONFIG_PGA_0_512V,
- 256:__ADS1015_REG_CONFIG_PGA_0_256V
- }
-
-
- # Constructor
- def __init__(self, address=0x48, ic=__IC_ADS1015, debug=False):
- # Depending on if you have an old or a new Raspberry Pi, you
- # may need to change the I2C bus. Older Pis use SMBus 0,
- # whereas new Pis use SMBus 1. If you see an error like:
- # 'Error accessing 0x48: Check your I2C address '
- # change the SMBus number in the initializer below!
- self.i2c = Adafruit_I2C(address)
- self.address = address
- self.debug = debug
-
- # Make sure the IC specified is valid
- if ((ic < self.__IC_ADS1015) | (ic > self.__IC_ADS1115)):
- if (self.debug):
- print "ADS1x15: Invalid IC specfied: %h" % ic
- return -1
- else:
- self.ic = ic
-
- # Set pga value, so that getLastConversionResult() can use it,
- # any function that accepts a pga value must update this.
- self.pga = 6144
-
-
- def readADCSingleEnded(self, channel=0, pga=6144, sps=250):
- "Gets a single-ended ADC reading from the specified channel in mV. \
- The sample rate for this mode (single-shot) can be used to lower the noise \
- (low sps) or to lower the power consumption (high sps) by duty cycling, \
- see datasheet page 14 for more info. \
- The pga must be given in mV, see page 13 for the supported values."
-
- # With invalid channel return -1
- if (channel > 3):
- if (self.debug):
- print "ADS1x15: Invalid channel specified: %d" % channel
- return -1
-
- # Disable comparator, Non-latching, Alert/Rdy active low
- # traditional comparator, single-shot mode
- config = self.__ADS1015_REG_CONFIG_CQUE_NONE | \
- self.__ADS1015_REG_CONFIG_CLAT_NONLAT | \
- self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW | \
- self.__ADS1015_REG_CONFIG_CMODE_TRAD | \
- self.__ADS1015_REG_CONFIG_MODE_SINGLE
-
- # Set sample per seconds, defaults to 250sps
- # If sps is in the dictionary (defined in init) it returns the value of the constant
- # othewise it returns the value for 250sps. This saves a lot of if/elif/else code!
- if (self.ic == self.__IC_ADS1015):
- config |= self.spsADS1015.setdefault(sps, self.__ADS1015_REG_CONFIG_DR_1600SPS)
- else:
- if ( (sps not in self.spsADS1115) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.spsADS1115.setdefault(sps, self.__ADS1115_REG_CONFIG_DR_250SPS)
-
- # Set PGA/voltage range, defaults to +-6.144V
- if ( (pga not in self.pgaADS1x15) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V)
- self.pga = pga
-
- # Set the channel to be converted
- if channel == 3:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_3
- elif channel == 2:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_2
- elif channel == 1:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_1
- else:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_0
-
- # Set 'start single-conversion' bit
- config |= self.__ADS1015_REG_CONFIG_OS_SINGLE
-
- # Write config register to the ADC
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
-
- # Wait for the ADC conversion to complete
- # The minimum delay depends on the sps: delay >= 1/sps
- # We add 0.1ms to be sure
- delay = 1.0/sps+0.0001
- time.sleep(delay)
-
- # Read the conversion results
- result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2)
- if (self.ic == self.__IC_ADS1015):
- # Shift right 4 bits for the 12-bit ADS1015 and convert to mV
- return ( ((result[0] << 8) | (result[1] & 0xFF)) >> 4 )*pga/2048.0
- else:
- # Return a mV value for the ADS1115
- # (Take signed values into account as well)
- val = (result[0] << 8) | (result[1])
- if val > 0x7FFF:
- return (val - 0xFFFF)*pga/32768.0
- else:
- return ( (result[0] << 8) | (result[1]) )*pga/32768.0
-
-
- def readADCDifferential(self, chP=0, chN=1, pga=6144, sps=250):
- "Gets a differential ADC reading from channels chP and chN in mV. \
- The sample rate for this mode (single-shot) can be used to lower the noise \
- (low sps) or to lower the power consumption (high sps) by duty cycling, \
- see data sheet page 14 for more info. \
- The pga must be given in mV, see page 13 for the supported values."
-
- # Disable comparator, Non-latching, Alert/Rdy active low
- # traditional comparator, single-shot mode
- config = self.__ADS1015_REG_CONFIG_CQUE_NONE | \
- self.__ADS1015_REG_CONFIG_CLAT_NONLAT | \
- self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW | \
- self.__ADS1015_REG_CONFIG_CMODE_TRAD | \
- self.__ADS1015_REG_CONFIG_MODE_SINGLE
-
- # Set channels
- if ( (chP == 0) & (chN == 1) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_1
- elif ( (chP == 0) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_3
- elif ( (chP == 2) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_2_3
- elif ( (chP == 1) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_1_3
- else:
- if (self.debug):
- print "ADS1x15: Invalid channels specified: %d, %d" % (chP, chN)
- return -1
-
- # Set sample per seconds, defaults to 250sps
- # If sps is in the dictionary (defined in init()) it returns the value of the constant
- # othewise it returns the value for 250sps. This saves a lot of if/elif/else code!
- if (self.ic == self.__IC_ADS1015):
- config |= self.spsADS1015.setdefault(sps, self.__ADS1015_REG_CONFIG_DR_1600SPS)
- else:
- if ( (sps not in self.spsADS1115) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.spsADS1115.setdefault(sps, self.__ADS1115_REG_CONFIG_DR_250SPS)
-
- # Set PGA/voltage range, defaults to +-6.144V
- if ( (pga not in self.pgaADS1x15) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V)
- self.pga = pga
-
- # Set 'start single-conversion' bit
- config |= self.__ADS1015_REG_CONFIG_OS_SINGLE
-
- # Write config register to the ADC
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
-
- # Wait for the ADC conversion to complete
- # The minimum delay depends on the sps: delay >= 1/sps
- # We add 0.1ms to be sure
- delay = 1.0/sps+0.0001
- time.sleep(delay)
-
- # Read the conversion results
- result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2)
- if (self.ic == self.__IC_ADS1015):
- # Shift right 4 bits for the 12-bit ADS1015 and convert to mV
- val = ((result[0] << 8) | (result[1] & 0xFF)) >> 4
- # (Take signed values into account as well)
- if val >> 11:
- val = val - 0xfff
- return val*pga/2048.0
- else:
- # Return a mV value for the ADS1115
- # (Take signed values into account as well)
- val = (result[0] << 8) | (result[1])
- if val > 0x7FFF:
- return (val - 0xFFFF)*pga/32768.0
- else:
- return ( (result[0] << 8) | (result[1]) )*pga/32768.0
-
-
- def readADCDifferential01(self, pga=6144, sps=250):
- "Gets a differential ADC reading from channels 0 and 1 in mV\
- The sample rate for this mode (single-shot) can be used to lower the noise \
- (low sps) or to lower the power consumption (high sps) by duty cycling, \
- see data sheet page 14 for more info. \
- The pga must be given in mV, see page 13 for the supported values."
- return self.readADCDifferential(0, 1, pga, sps)
-
-
- def readADCDifferential03(self, pga=6144, sps=250):
- "Gets a differential ADC reading from channels 0 and 3 in mV \
- The sample rate for this mode (single-shot) can be used to lower the noise \
- (low sps) or to lower the power consumption (high sps) by duty cycling, \
- see data sheet page 14 for more info. \
- The pga must be given in mV, see page 13 for the supported values."
- return self.readADCDifferential(0, 3, pga, sps)
-
-
- def readADCDifferential13(self, pga=6144, sps=250):
- "Gets a differential ADC reading from channels 1 and 3 in mV \
- The sample rate for this mode (single-shot) can be used to lower the noise \
- (low sps) or to lower the power consumption (high sps) by duty cycling, \
- see data sheet page 14 for more info. \
- The pga must be given in mV, see page 13 for the supported values."
- return self.__readADCDifferential(1, 3, pga, sps)
-
-
- def readADCDifferential23(self, pga=6144, sps=250):
- "Gets a differential ADC reading from channels 2 and 3 in mV \
- The sample rate for this mode (single-shot) can be used to lower the noise \
- (low sps) or to lower the power consumption (high sps) by duty cycling, \
- see data sheet page 14 for more info. \
- The pga must be given in mV, see page 13 for the supported values."
- return self.readADCDifferential(2, 3, pga, sps)
-
-
- def startContinuousConversion(self, channel=0, pga=6144, sps=250):
- "Starts the continuous conversion mode and returns the first ADC reading \
- in mV from the specified channel. \
- The sps controls the sample rate. \
- The pga must be given in mV, see datasheet page 13 for the supported values. \
- Use getLastConversionResults() to read the next values and \
- stopContinuousConversion() to stop converting."
-
- # Default to channel 0 with invalid channel, or return -1?
- if (channel > 3):
- if (self.debug):
- print "ADS1x15: Invalid channel specified: %d" % channel
- return -1
-
- # Disable comparator, Non-latching, Alert/Rdy active low
- # traditional comparator, continuous mode
- # The last flag is the only change we need, page 11 datasheet
- config = self.__ADS1015_REG_CONFIG_CQUE_NONE | \
- self.__ADS1015_REG_CONFIG_CLAT_NONLAT | \
- self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW | \
- self.__ADS1015_REG_CONFIG_CMODE_TRAD | \
- self.__ADS1015_REG_CONFIG_MODE_CONTIN
-
- # Set sample per seconds, defaults to 250sps
- # If sps is in the dictionary (defined in init()) it returns the value of the constant
- # othewise it returns the value for 250sps. This saves a lot of if/elif/else code!
- if (self.ic == self.__IC_ADS1015):
- config |= self.spsADS1015.setdefault(sps, self.__ADS1015_REG_CONFIG_DR_1600SPS)
- else:
- if ( (sps not in self.spsADS1115) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.spsADS1115.setdefault(sps, self.__ADS1115_REG_CONFIG_DR_250SPS)
-
- # Set PGA/voltage range, defaults to +-6.144V
- if ( (pga not in self.pgaADS1x15) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V)
- self.pga = pga
-
- # Set the channel to be converted
- if channel == 3:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_3
- elif channel == 2:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_2
- elif channel == 1:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_1
- else:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_0
-
- # Set 'start single-conversion' bit to begin conversions
- # No need to change this for continuous mode!
- config |= self.__ADS1015_REG_CONFIG_OS_SINGLE
-
- # Write config register to the ADC
- # Once we write the ADC will convert continously
- # we can read the next values using getLastConversionResult
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
-
- # Wait for the ADC conversion to complete
- # The minimum delay depends on the sps: delay >= 1/sps
- # We add 0.5ms to be sure
- delay = 1.0/sps+0.0005
- time.sleep(delay)
-
- # Read the conversion results
- result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2)
- if (self.ic == self.__IC_ADS1015):
- # Shift right 4 bits for the 12-bit ADS1015 and convert to mV
- return ( ((result[0] << 8) | (result[1] & 0xFF)) >> 4 )*pga/2048.0
- else:
- # Return a mV value for the ADS1115
- # (Take signed values into account as well)
- val = (result[0] << 8) | (result[1])
- if val > 0x7FFF:
- return (val - 0xFFFF)*pga/32768.0
- else:
- return ( (result[0] << 8) | (result[1]) )*pga/32768.0
-
- def startContinuousDifferentialConversion(self, chP=0, chN=1, pga=6144, sps=250):
- "Starts the continuous differential conversion mode and returns the first ADC reading \
- in mV as the difference from the specified channels. \
- The sps controls the sample rate. \
- The pga must be given in mV, see datasheet page 13 for the supported values. \
- Use getLastConversionResults() to read the next values and \
- stopContinuousConversion() to stop converting."
-
- # Disable comparator, Non-latching, Alert/Rdy active low
- # traditional comparator, continuous mode
- # The last flag is the only change we need, page 11 datasheet
- config = self.__ADS1015_REG_CONFIG_CQUE_NONE | \
- self.__ADS1015_REG_CONFIG_CLAT_NONLAT | \
- self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW | \
- self.__ADS1015_REG_CONFIG_CMODE_TRAD | \
- self.__ADS1015_REG_CONFIG_MODE_CONTIN
-
- # Set sample per seconds, defaults to 250sps
- # If sps is in the dictionary (defined in init()) it returns the value of the constant
- # othewise it returns the value for 250sps. This saves a lot of if/elif/else code!
- if (self.ic == self.__IC_ADS1015):
- config |= self.spsADS1015.setdefault(sps, self.__ADS1015_REG_CONFIG_DR_1600SPS)
- else:
- if ( (sps not in self.spsADS1115) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.spsADS1115.setdefault(sps, self.__ADS1115_REG_CONFIG_DR_250SPS)
-
- # Set PGA/voltage range, defaults to +-6.144V
- if ( (pga not in self.pgaADS1x15) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % sps
- config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V)
- self.pga = pga
-
- # Set channels
- if ( (chP == 0) & (chN == 1) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_1
- elif ( (chP == 0) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_3
- elif ( (chP == 2) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_2_3
- elif ( (chP == 1) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_1_3
- else:
- if (self.debug):
- print "ADS1x15: Invalid channels specified: %d, %d" % (chP, chN)
- return -1
-
- # Set 'start single-conversion' bit to begin conversions
- # No need to change this for continuous mode!
- config |= self.__ADS1015_REG_CONFIG_OS_SINGLE
-
- # Write config register to the ADC
- # Once we write the ADC will convert continously
- # we can read the next values using getLastConversionResult
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
-
- # Wait for the ADC conversion to complete
- # The minimum delay depends on the sps: delay >= 1/sps
- # We add 0.5ms to be sure
- delay = 1.0/sps+0.0005
- time.sleep(delay)
-
- # Read the conversion results
- result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2)
- if (self.ic == self.__IC_ADS1015):
- # Shift right 4 bits for the 12-bit ADS1015 and convert to mV
- return ( ((result[0] << 8) | (result[1] & 0xFF)) >> 4 )*pga/2048.0
- else:
- # Return a mV value for the ADS1115
- # (Take signed values into account as well)
- val = (result[0] << 8) | (result[1])
- if val > 0x7FFF:
- return (val - 0xFFFF)*pga/32768.0
- else:
- return ( (result[0] << 8) | (result[1]) )*pga/32768.0
-
-
- def stopContinuousConversion(self):
- "Stops the ADC's conversions when in continuous mode \
- and resets the configuration to its default value."
- # Write the default config register to the ADC
- # Once we write, the ADC will do a single conversion and
- # enter power-off mode.
- config = 0x8583 # Page 18 datasheet.
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
- return True
-
- def getLastConversionResults(self):
- "Returns the last ADC conversion result in mV"
- # Read the conversion results
- result = self.i2c.readList(self.__ADS1015_REG_POINTER_CONVERT, 2)
- if (self.ic == self.__IC_ADS1015):
- # Shift right 4 bits for the 12-bit ADS1015 and convert to mV
- return ( ((result[0] << 8) | (result[1] & 0xFF)) >> 4 )*self.pga/2048.0
- else:
- # Return a mV value for the ADS1115
- # (Take signed values into account as well)
- val = (result[0] << 8) | (result[1])
- if val > 0x7FFF:
- return (val - 0xFFFF)*self.pga/32768.0
- else:
- return ( (result[0] << 8) | (result[1]) )*self.pga/32768.0
-
-
- def startSingleEndedComparator(self, channel, thresholdHigh, thresholdLow, \
- pga=6144, sps=250, \
- activeLow=True, traditionalMode=True, latching=False, \
- numReadings=1):
- "Starts the comparator mode on the specified channel, see datasheet pg. 15. \
- In traditional mode it alerts (ALERT pin will go low) when voltage exceeds \
- thresholdHigh until it falls below thresholdLow (both given in mV). \
- In window mode (traditionalMode=False) it alerts when voltage doesn't lie\
- between both thresholds.\
- In latching mode the alert will continue until the conversion value is read. \
- numReadings controls how many readings are necessary to trigger an alert: 1, 2 or 4.\
- Use getLastConversionResults() to read the current value (which may differ \
- from the one that triggered the alert) and clear the alert pin in latching mode. \
- This function starts the continuous conversion mode. The sps controls \
- the sample rate and the pga the gain, see datasheet page 13. "
-
- # With invalid channel return -1
- if (channel > 3):
- if (self.debug):
- print "ADS1x15: Invalid channel specified: %d" % channel
- return -1
-
- # Continuous mode
- config = self.__ADS1015_REG_CONFIG_MODE_CONTIN
-
- if (activeLow==False):
- config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVHI
- else:
- config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW
-
- if (traditionalMode==False):
- config |= self.__ADS1015_REG_CONFIG_CMODE_WINDOW
- else:
- config |= self.__ADS1015_REG_CONFIG_CMODE_TRAD
-
- if (latching==True):
- config |= self.__ADS1015_REG_CONFIG_CLAT_LATCH
- else:
- config |= self.__ADS1015_REG_CONFIG_CLAT_NONLAT
-
- if (numReadings==4):
- config |= self.__ADS1015_REG_CONFIG_CQUE_4CONV
- elif (numReadings==2):
- config |= self.__ADS1015_REG_CONFIG_CQUE_2CONV
- else:
- config |= self.__ADS1015_REG_CONFIG_CQUE_1CONV
-
- # Set sample per seconds, defaults to 250sps
- # If sps is in the dictionary (defined in init()) it returns the value of the constant
- # othewise it returns the value for 250sps. This saves a lot of if/elif/else code!
- if (self.ic == self.__IC_ADS1015):
- if ( (sps not in self.spsADS1015) & self.debug):
- print "ADS1x15: Invalid sps specified: %d, using 1600sps" % sps
- config |= self.spsADS1015.setdefault(sps, self.__ADS1015_REG_CONFIG_DR_1600SPS)
- else:
- if ( (sps not in self.spsADS1115) & self.debug):
- print "ADS1x15: Invalid sps specified: %d, using 250sps" % sps
- config |= self.spsADS1115.setdefault(sps, self.__ADS1115_REG_CONFIG_DR_250SPS)
-
- # Set PGA/voltage range, defaults to +-6.144V
- if ( (pga not in self.pgaADS1x15) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % pga
- config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V)
- self.pga = pga
-
- # Set the channel to be converted
- if channel == 3:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_3
- elif channel == 2:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_2
- elif channel == 1:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_1
- else:
- config |= self.__ADS1015_REG_CONFIG_MUX_SINGLE_0
-
- # Set 'start single-conversion' bit to begin conversions
- config |= self.__ADS1015_REG_CONFIG_OS_SINGLE
-
- # Write threshold high and low registers to the ADC
- # V_digital = (2^(n-1)-1)/pga*V_analog
- if (self.ic == self.__IC_ADS1015):
- thresholdHighWORD = int(thresholdHigh*(2048.0/pga))
- else:
- thresholdHighWORD = int(thresholdHigh*(32767.0/pga))
- bytes = [(thresholdHighWORD >> 8) & 0xFF, thresholdHighWORD & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_HITHRESH, bytes)
-
- if (self.ic == self.__IC_ADS1015):
- thresholdLowWORD = int(thresholdLow*(2048.0/pga))
- else:
- thresholdLowWORD = int(thresholdLow*(32767.0/pga))
- bytes = [(thresholdLowWORD >> 8) & 0xFF, thresholdLowWORD & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_LOWTHRESH, bytes)
-
- # Write config register to the ADC
- # Once we write the ADC will convert continously and alert when things happen,
- # we can read the converted values using getLastConversionResult
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
-
-
- def startDifferentialComparator(self, chP, chN, thresholdHigh, thresholdLow, \
- pga=6144, sps=250, \
- activeLow=True, traditionalMode=True, latching=False, \
- numReadings=1):
- "Starts the comparator mode on the specified channel, see datasheet pg. 15. \
- In traditional mode it alerts (ALERT pin will go low) when voltage exceeds \
- thresholdHigh until it falls below thresholdLow (both given in mV). \
- In window mode (traditionalMode=False) it alerts when voltage doesn't lie\
- between both thresholds.\
- In latching mode the alert will continue until the conversion value is read. \
- numReadings controls how many readings are necessary to trigger an alert: 1, 2 or 4.\
- Use getLastConversionResults() to read the current value (which may differ \
- from the one that triggered the alert) and clear the alert pin in latching mode. \
- This function starts the continuous conversion mode. The sps controls \
- the sample rate and the pga the gain, see datasheet page 13. "
-
- # Continuous mode
- config = self.__ADS1015_REG_CONFIG_MODE_CONTIN
-
- if (activeLow==False):
- config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVHI
- else:
- config |= self.__ADS1015_REG_CONFIG_CPOL_ACTVLOW
-
- if (traditionalMode==False):
- config |= self.__ADS1015_REG_CONFIG_CMODE_WINDOW
- else:
- config |= self.__ADS1015_REG_CONFIG_CMODE_TRAD
-
- if (latching==True):
- config |= self.__ADS1015_REG_CONFIG_CLAT_LATCH
- else:
- config |= self.__ADS1015_REG_CONFIG_CLAT_NONLAT
-
- if (numReadings==4):
- config |= self.__ADS1015_REG_CONFIG_CQUE_4CONV
- elif (numReadings==2):
- config |= self.__ADS1015_REG_CONFIG_CQUE_2CONV
- else:
- config |= self.__ADS1015_REG_CONFIG_CQUE_1CONV
-
- # Set sample per seconds, defaults to 250sps
- # If sps is in the dictionary (defined in init()) it returns the value of the constant
- # othewise it returns the value for 250sps. This saves a lot of if/elif/else code!
- if (self.ic == self.__IC_ADS1015):
- if ( (sps not in self.spsADS1015) & self.debug):
- print "ADS1x15: Invalid sps specified: %d, using 1600sps" % sps
- config |= self.spsADS1015.setdefault(sps, self.__ADS1015_REG_CONFIG_DR_1600SPS)
- else:
- if ( (sps not in self.spsADS1115) & self.debug):
- print "ADS1x15: Invalid sps specified: %d, using 250sps" % sps
- config |= self.spsADS1115.setdefault(sps, self.__ADS1115_REG_CONFIG_DR_250SPS)
-
- # Set PGA/voltage range, defaults to +-6.144V
- if ( (pga not in self.pgaADS1x15) & self.debug):
- print "ADS1x15: Invalid pga specified: %d, using 6144mV" % pga
- config |= self.pgaADS1x15.setdefault(pga, self.__ADS1015_REG_CONFIG_PGA_6_144V)
- self.pga = pga
-
- # Set channels
- if ( (chP == 0) & (chN == 1) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_1
- elif ( (chP == 0) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_0_3
- elif ( (chP == 2) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_2_3
- elif ( (chP == 1) & (chN == 3) ):
- config |= self.__ADS1015_REG_CONFIG_MUX_DIFF_1_3
- else:
- if (self.debug):
- print "ADS1x15: Invalid channels specified: %d, %d" % (chP, chN)
- return -1
-
- # Set 'start single-conversion' bit to begin conversions
- config |= self.__ADS1015_REG_CONFIG_OS_SINGLE
-
- # Write threshold high and low registers to the ADC
- # V_digital = (2^(n-1)-1)/pga*V_analog
- if (self.ic == self.__IC_ADS1015):
- thresholdHighWORD = int(thresholdHigh*(2048.0/pga))
- else:
- thresholdHighWORD = int(thresholdHigh*(32767.0/pga))
- bytes = [(thresholdHighWORD >> 8) & 0xFF, thresholdHighWORD & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_HITHRESH, bytes)
-
- if (self.ic == self.__IC_ADS1015):
- thresholdLowWORD = int(thresholdLow*(2048.0/pga))
- else:
- thresholdLowWORD = int(thresholdLow*(32767.0/pga))
- bytes = [(thresholdLowWORD >> 8) & 0xFF, thresholdLowWORD & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_LOWTHRESH, bytes)
-
- # Write config register to the ADC
- # Once we write the ADC will convert continously and alert when things happen,
- # we can read the converted values using getLastConversionResult
- bytes = [(config >> 8) & 0xFF, config & 0xFF]
- self.i2c.writeList(self.__ADS1015_REG_POINTER_CONFIG, bytes)
-
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/FT232H.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/FT232H.py
deleted file mode 100644
index 3453db0258..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/FT232H.py
+++ /dev/null
@@ -1,817 +0,0 @@
-# Copyright (c) 2014 Adafruit Industries
-# Author: Tony DiCola
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-
-import atexit
-import logging
-import math
-import os
-import subprocess
-import sys
-import time
-
-import ftdi1 as ftdi
-
-import GPIO
-
-
-logger = logging.getLogger(__name__)
-
-FT232H_VID = 0x0403 # Default FTDI FT232H vendor ID
-FT232H_PID = 0x6014 # Default FTDI FT232H product ID
-
-MSBFIRST = 0
-LSBFIRST = 1
-
-_REPEAT_DELAY = 4
-
-
-def _check_running_as_root():
- # NOTE: Checking for root with user ID 0 isn't very portable, perhaps
- # there's a better alternative?
- if os.geteuid() != 0:
- raise RuntimeError('Expected to be run by root user! Try running with sudo.')
-
-def disable_FTDI_driver():
- """Disable the FTDI drivers for the current platform. This is necessary
- because they will conflict with libftdi and accessing the FT232H. Note you
- can enable the FTDI drivers again by calling enable_FTDI_driver.
- """
- logger.debug('Disabling FTDI driver.')
- if sys.platform == 'darwin':
- logger.debug('Detected Mac OSX')
- # Mac OS commands to disable FTDI driver.
- _check_running_as_root()
- subprocess.call('kextunload -b com.apple.driver.AppleUSBFTDI', shell=True)
- subprocess.call('kextunload /System/Library/Extensions/FTDIUSBSerialDriver.kext', shell=True)
- elif sys.platform.startswith('linux'):
- logger.debug('Detected Linux')
- # Linux commands to disable FTDI driver.
- _check_running_as_root()
- subprocess.call('modprobe -r -q ftdi_sio', shell=True)
- subprocess.call('modprobe -r -q usbserial', shell=True)
- # Note there is no need to disable FTDI drivers on Windows!
-
-def enable_FTDI_driver():
- """Re-enable the FTDI drivers for the current platform."""
- logger.debug('Enabling FTDI driver.')
- if sys.platform == 'darwin':
- logger.debug('Detected Mac OSX')
- # Mac OS commands to enable FTDI driver.
- _check_running_as_root()
- subprocess.check_call('kextload -b com.apple.driver.AppleUSBFTDI', shell=True)
- subprocess.check_call('kextload /System/Library/Extensions/FTDIUSBSerialDriver.kext', shell=True)
- elif sys.platform.startswith('linux'):
- logger.debug('Detected Linux')
- # Linux commands to enable FTDI driver.
- _check_running_as_root()
- subprocess.check_call('modprobe -q ftdi_sio', shell=True)
- subprocess.check_call('modprobe -q usbserial', shell=True)
-
-def use_FT232H():
- """Disable any built in FTDI drivers which will conflict and cause problems
- with libftdi (which is used to communicate with the FT232H). Will register
- an exit function so the drivers are re-enabled on program exit.
- """
- disable_FTDI_driver()
- atexit.register(enable_FTDI_driver)
-
-def enumerate_device_serials(vid=FT232H_VID, pid=FT232H_PID):
- """Return a list of all FT232H device serial numbers connected to the
- machine. You can use these serial numbers to open a specific FT232H device
- by passing it to the FT232H initializer's serial parameter.
- """
- try:
- # Create a libftdi context.
- ctx = None
- ctx = ftdi.new()
- # Enumerate FTDI devices.
- device_list = None
- count, device_list = ftdi.usb_find_all(ctx, vid, pid)
- if count < 0:
- raise RuntimeError('ftdi_usb_find_all returned error {0}: {1}'.format(count, ftdi.get_error_string(self._ctx)))
- # Walk through list of devices and assemble list of serial numbers.
- devices = []
- while device_list is not None:
- # Get USB device strings and add serial to list of devices.
- ret, manufacturer, description, serial = ftdi.usb_get_strings(ctx, device_list.dev, 256, 256, 256)
- if ret < 0:
- raise RuntimeError('ftdi_usb_get_strings returned error {0}: {1}'.format(ret, ftdi.get_error_string(self._ctx)))
- devices.append(serial)
- device_list = device_list.next
- return devices
- finally:
- # Make sure to clean up list and context when done.
- if device_list is not None:
- ftdi.list_free(device_list)
- if ctx is not None:
- ftdi.free(ctx)
-
-
-class FT232H(GPIO.BaseGPIO):
- # Make GPIO constants that match main GPIO class for compatibility.
- HIGH = GPIO.HIGH
- LOW = GPIO.LOW
- IN = GPIO.IN
- OUT = GPIO.OUT
-
- def __init__(self, vid=FT232H_VID, pid=FT232H_PID, serial=None):
- """Create a FT232H object. Will search for the first available FT232H
- device with the specified USB vendor ID and product ID (defaults to
- FT232H default VID & PID). Can also specify an optional serial number
- string to open an explicit FT232H device given its serial number. See
- the FT232H.enumerate_device_serials() function to see how to list all
- connected device serial numbers.
- """
- # Initialize FTDI device connection.
- self._ctx = ftdi.new()
- if self._ctx == 0:
- raise RuntimeError('ftdi_new failed! Is libftdi1 installed?')
- # Register handler to close and cleanup FTDI context on program exit.
- atexit.register(self.close)
- if serial is None:
- # Open USB connection for specified VID and PID if no serial is specified.
- self._check(ftdi.usb_open, vid, pid)
- else:
- # Open USB connection for VID, PID, serial.
- self._check(ftdi.usb_open_string, 's:{0}:{1}:{2}'.format(vid, pid, serial))
- # Reset device.
- self._check(ftdi.usb_reset)
- # Disable flow control. Commented out because it is unclear if this is necessary.
- #self._check(ftdi.setflowctrl, ftdi.SIO_DISABLE_FLOW_CTRL)
- # Change read & write buffers to maximum size, 65535 bytes.
- self._check(ftdi.read_data_set_chunksize, 65535)
- self._check(ftdi.write_data_set_chunksize, 65535)
- # Clear pending read data & write buffers.
- self._check(ftdi.usb_purge_buffers)
- # Enable MPSSE and syncronize communication with device.
- self._mpsse_enable()
- self._mpsse_sync()
- # Initialize all GPIO as inputs.
- self._write('\x80\x00\x00\x82\x00\x00')
- self._direction = 0x0000
- self._level = 0x0000
-
- def close(self):
- """Close the FTDI device. Will be automatically called when the program ends."""
- if self._ctx is not None:
- ftdi.free(self._ctx)
- self._ctx = None
-
- def _write(self, string):
- """Helper function to call write_data on the provided FTDI device and
- verify it succeeds.
- """
- # Get modem status. Useful to enable for debugging.
- #ret, status = ftdi.poll_modem_status(self._ctx)
- #if ret == 0:
- # logger.debug('Modem status {0:02X}'.format(status))
- #else:
- # logger.debug('Modem status error {0}'.format(ret))
- length = len(string)
- ret = ftdi.write_data(self._ctx, string, length)
- # Log the string that was written in a python hex string format using a very
- # ugly one-liner list comprehension for brevity.
- #logger.debug('Wrote {0}'.format(''.join(['\\x{0:02X}'.format(ord(x)) for x in string])))
- if ret < 0:
- raise RuntimeError('ftdi_write_data failed with error {0}: {1}'.format(ret, ftdi.get_error_string(self._ctx)))
- if ret != length:
- raise RuntimeError('ftdi_write_data expected to write {0} bytes but actually wrote {1}!'.format(length, ret))
-
- def _check(self, command, *args):
- """Helper function to call the provided command on the FTDI device and
- verify the response matches the expected value.
- """
- ret = command(self._ctx, *args)
- logger.debug('Called ftdi_{0} and got response {1}.'.format(command.__name__, ret))
- if ret != 0:
- raise RuntimeError('ftdi_{0} failed with error {1}: {2}'.format(command.__name__, ret, ftdi.get_error_string(self._ctx)))
-
- def _poll_read(self, expected, timeout_s=5.0):
- """Helper function to continuously poll reads on the FTDI device until an
- expected number of bytes are returned. Will throw a timeout error if no
- data is received within the specified number of timeout seconds. Returns
- the read data as a string if successful, otherwise raises an execption.
- """
- start = time.time()
- # Start with an empty response buffer.
- response = bytearray(expected)
- index = 0
- # Loop calling read until the response buffer is full or a timeout occurs.
- while time.time() - start <= timeout_s:
- ret, data = ftdi.read_data(self._ctx, expected - index)
- # Fail if there was an error reading data.
- if ret < 0:
- raise RuntimeError('ftdi_read_data failed with error code {0}.'.format(ret))
- # Add returned data to the buffer.
- response[index:index+ret] = data[:ret]
- index += ret
- # Buffer is full, return the result data.
- if index >= expected:
- return str(response)
- time.sleep(0.01)
- raise RuntimeError('Timeout while polling ftdi_read_data for {0} bytes!'.format(expected))
-
- def _mpsse_enable(self):
- """Enable MPSSE mode on the FTDI device."""
- # Reset MPSSE by sending mask = 0 and mode = 0
- self._check(ftdi.set_bitmode, 0, 0)
- # Enable MPSSE by sending mask = 0 and mode = 2
- self._check(ftdi.set_bitmode, 0, 2)
-
- def _mpsse_sync(self, max_retries=10):
- """Synchronize buffers with MPSSE by sending bad opcode and reading expected
- error response. Should be called once after enabling MPSSE."""
- # Send a bad/unknown command (0xAB), then read buffer until bad command
- # response is found.
- self._write('\xAB')
- # Keep reading until bad command response (0xFA 0xAB) is returned.
- # Fail if too many read attempts are made to prevent sticking in a loop.
- tries = 0
- sync = False
- while not sync:
- data = self._poll_read(2)
- if data == '\xFA\xAB':
- sync = True
- tries += 1
- if tries >= max_retries:
- raise RuntimeError('Could not synchronize with FT232H!')
-
- def mpsse_set_clock(self, clock_hz, adaptive=False, three_phase=False):
- """Set the clock speed of the MPSSE engine. Can be any value from 450hz
- to 30mhz and will pick that speed or the closest speed below it.
- """
- # Disable clock divisor by 5 to enable faster speeds on FT232H.
- self._write('\x8A')
- # Turn on/off adaptive clocking.
- if adaptive:
- self._write('\x96')
- else:
- self._write('\x97')
- # Turn on/off three phase clock (needed for I2C).
- # Also adjust the frequency for three-phase clocking as specified in section 2.2.4
- # of this document:
- # http://www.ftdichip.com/Support/Documents/AppNotes/AN_255_USB%20to%20I2C%20Example%20using%20the%20FT232H%20and%20FT201X%20devices.pdf
- if three_phase:
- self._write('\x8C')
- else:
- self._write('\x8D')
- # Compute divisor for requested clock.
- # Use equation from section 3.8.1 of:
- # http://www.ftdichip.com/Support/Documents/AppNotes/AN_108_Command_Processor_for_MPSSE_and_MCU_Host_Bus_Emulation_Modes.pdf
- # Note equation is using 60mhz master clock instead of 12mhz.
- divisor = int(math.ceil((30000000.0-float(clock_hz))/float(clock_hz))) & 0xFFFF
- if three_phase:
- divisor = int(divisor*(2.0/3.0))
- logger.debug('Setting clockspeed with divisor value {0}'.format(divisor))
- # Send command to set divisor from low and high byte values.
- self._write(str(bytearray((0x86, divisor & 0xFF, (divisor >> 8) & 0xFF))))
-
- def mpsse_read_gpio(self):
- """Read both GPIO bus states and return a 16 bit value with their state.
- D0-D7 are the lower 8 bits and C0-C7 are the upper 8 bits.
- """
- # Send command to read low byte and high byte.
- self._write('\x81\x83')
- # Wait for 2 byte response.
- data = self._poll_read(2)
- # Assemble response into 16 bit value.
- low_byte = ord(data[0])
- high_byte = ord(data[1])
- logger.debug('Read MPSSE GPIO low byte = {0:02X} and high byte = {1:02X}'.format(low_byte, high_byte))
- return (high_byte << 8) | low_byte
-
- def mpsse_gpio(self):
- """Return command to update the MPSSE GPIO state to the current direction
- and level.
- """
- level_low = chr(self._level & 0xFF)
- level_high = chr((self._level >> 8) & 0xFF)
- dir_low = chr(self._direction & 0xFF)
- dir_high = chr((self._direction >> 8) & 0xFF)
- return str(bytearray((0x80, level_low, dir_low, 0x82, level_high, dir_high)))
-
- def mpsse_write_gpio(self):
- """Write the current MPSSE GPIO state to the FT232H chip."""
- self._write(self.mpsse_gpio())
-
- def get_i2c_device(self, address, **kwargs):
- """Return an I2CDevice instance using this FT232H object and the provided
- I2C address. Meant to be passed as the i2c_provider parameter to objects
- which use the Adafruit_Python_GPIO library for I2C.
- """
- return I2CDevice(self, address, **kwargs)
-
- # GPIO functions below:
-
- def _setup_pin(self, pin, mode):
- if pin < 0 or pin > 15:
- raise ValueError('Pin must be between 0 and 15 (inclusive).')
- if mode not in (GPIO.IN, GPIO.OUT):
- raise ValueError('Mode must be GPIO.IN or GPIO.OUT.')
- if mode == GPIO.IN:
- # Set the direction and level of the pin to 0.
- self._direction &= ~(1 << pin) & 0xFFFF
- self._level &= ~(1 << pin) & 0xFFFF
- else:
- # Set the direction of the pin to 1.
- self._direction |= (1 << pin) & 0xFFFF
-
- def setup(self, pin, mode):
- """Set the input or output mode for a specified pin. Mode should be
- either OUT or IN."""
- self._setup_pin(pin, mode)
- self.mpsse_write_gpio()
-
- def setup_pins(self, pins, values={}, write=True):
- """Setup multiple pins as inputs or outputs at once. Pins should be a
- dict of pin name to pin mode (IN or OUT). Optional starting values of
- pins can be provided in the values dict (with pin name to pin value).
- """
- # General implementation that can be improved by subclasses.
- for pin, mode in iter(pins.items()):
- self._setup_pin(pin, mode)
- for pin, value in iter(values.items()):
- self._output_pin(pin, value)
- if write:
- self.mpsse_write_gpio()
-
- def _output_pin(self, pin, value):
- if value:
- self._level |= (1 << pin) & 0xFFFF
- else:
- self._level &= ~(1 << pin) & 0xFFFF
-
- def output(self, pin, value):
- """Set the specified pin the provided high/low value. Value should be
- either HIGH/LOW or a boolean (true = high)."""
- if pin < 0 or pin > 15:
- raise ValueError('Pin must be between 0 and 15 (inclusive).')
- self._output_pin(pin, value)
- self.mpsse_write_gpio()
-
- def output_pins(self, pins, write=True):
- """Set multiple pins high or low at once. Pins should be a dict of pin
- name to pin value (HIGH/True for 1, LOW/False for 0). All provided pins
- will be set to the given values.
- """
- for pin, value in iter(pins.items()):
- self._output_pin(pin, value)
- if write:
- self.mpsse_write_gpio()
-
- def input(self, pin):
- """Read the specified pin and return HIGH/true if the pin is pulled high,
- or LOW/false if pulled low."""
- return self.input_pins([pin])[0]
-
- def input_pins(self, pins):
- """Read multiple pins specified in the given list and return list of pin values
- GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low."""
- if [pin for pin in pins if pin < 0 or pin > 15]:
- raise ValueError('Pin must be between 0 and 15 (inclusive).')
- _pins = self.mpsse_read_gpio()
- return [((_pins >> pin) & 0x0001) == 1 for pin in pins]
-
-
-class SPI(object):
- def __init__(self, ft232h, cs=None, max_speed_hz=1000000, mode=0, bitorder=MSBFIRST):
- self._ft232h = ft232h
- # Initialize chip select pin if provided to output high.
- if cs is not None:
- ft232h.setup(cs, GPIO.OUT)
- ft232h.set_high(cs)
- self._cs = cs
- # Initialize clock, mode, and bit order.
- self.set_clock_hz(max_speed_hz)
- self.set_mode(mode)
- self.set_bit_order(bitorder)
-
- def _assert_cs(self):
- if self._cs is not None:
- self._ft232h.set_low(self._cs)
-
- def _deassert_cs(self):
- if self._cs is not None:
- self._ft232h.set_high(self._cs)
-
- def set_clock_hz(self, hz):
- """Set the speed of the SPI clock in hertz. Note that not all speeds
- are supported and a lower speed might be chosen by the hardware.
- """
- self._ft232h.mpsse_set_clock(hz)
-
- def set_mode(self, mode):
- """Set SPI mode which controls clock polarity and phase. Should be a
- numeric value 0, 1, 2, or 3. See wikipedia page for details on meaning:
- http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
- """
- if mode < 0 or mode > 3:
- raise ValueError('Mode must be a value 0, 1, 2, or 3.')
- if mode == 0:
- # Mode 0 captures on rising clock, propagates on falling clock
- self.write_clock_ve = 1
- self.read_clock_ve = 0
- # Clock base is low.
- clock_base = GPIO.LOW
- elif mode == 1:
- # Mode 1 capture of falling edge, propagate on rising clock
- self.write_clock_ve = 0
- self.read_clock_ve = 1
- # Clock base is low.
- clock_base = GPIO.LOW
- elif mode == 2:
- # Mode 2 capture on rising clock, propagate on falling clock
- self.write_clock_ve = 1
- self.read_clock_ve = 0
- # Clock base is high.
- clock_base = GPIO.HIGH
- elif mode == 3:
- # Mode 3 capture on falling edge, propagage on rising clock
- self.write_clock_ve = 0
- self.read_clock_ve = 1
- # Clock base is high.
- clock_base = GPIO.HIGH
- # Set clock and DO as output, DI as input. Also start clock at its base value.
- self._ft232h.setup_pins({0: GPIO.OUT, 1: GPIO.OUT, 2: GPIO.IN}, {0: clock_base})
-
- def set_bit_order(self, order):
- """Set order of bits to be read/written over serial lines. Should be
- either MSBFIRST for most-significant first, or LSBFIRST for
- least-signifcant first.
- """
- if order == MSBFIRST:
- self.lsbfirst = 0
- elif order == LSBFIRST:
- self.lsbfirst = 1
- else:
- raise ValueError('Order must be MSBFIRST or LSBFIRST.')
-
- def write(self, data):
- """Half-duplex SPI write. The specified array of bytes will be clocked
- out the MOSI line.
- """
- # Build command to write SPI data.
- command = 0x10 | (self.lsbfirst << 3) | self.write_clock_ve
- logger.debug('SPI write with command {0:2X}.'.format(command))
- # Compute length low and high bytes.
- # NOTE: Must actually send length minus one because the MPSSE engine
- # considers 0 a length of 1 and FFFF a length of 65536
- length = len(data)-1
- len_low = length & 0xFF
- len_high = (length >> 8) & 0xFF
- self._assert_cs()
- # Send command and length.
- self._ft232h._write(str(bytearray((command, len_low, len_high))))
- # Send data.
- self._ft232h._write(str(bytearray(data)))
- self._deassert_cs()
-
- def read(self, length):
- """Half-duplex SPI read. The specified length of bytes will be clocked
- in the MISO line and returned as a bytearray object.
- """
- # Build command to read SPI data.
- command = 0x20 | (self.lsbfirst << 3) | (self.read_clock_ve << 2)
- logger.debug('SPI read with command {0:2X}.'.format(command))
- # Compute length low and high bytes.
- # NOTE: Must actually send length minus one because the MPSSE engine
- # considers 0 a length of 1 and FFFF a length of 65536
- len_low = (length-1) & 0xFF
- len_high = ((length-1) >> 8) & 0xFF
- self._assert_cs()
- # Send command and length.
- self._ft232h._write(str(bytearray((command, len_low, len_high, 0x87))))
- self._deassert_cs()
- # Read response bytes.
- return bytearray(self._ft232h._poll_read(length))
-
- def transfer(self, data):
- """Full-duplex SPI read and write. The specified array of bytes will be
- clocked out the MOSI line, while simultaneously bytes will be read from
- the MISO line. Read bytes will be returned as a bytearray object.
- """
- # Build command to read and write SPI data.
- command = 0x30 | (self.lsbfirst << 3) | (self.read_clock_ve << 2) | self.write_clock_ve
- logger.debug('SPI transfer with command {0:2X}.'.format(command))
- # Compute length low and high bytes.
- # NOTE: Must actually send length minus one because the MPSSE engine
- # considers 0 a length of 1 and FFFF a length of 65536
- length = len(data)
- len_low = (length-1) & 0xFF
- len_high = ((length-1) >> 8) & 0xFF
- # Send command and length.
- self._assert_cs()
- self._ft232h._write(str(bytearray((command, len_low, len_high))))
- self._ft232h._write(str(bytearray(data)))
- self._ft232h._write('\x87')
- self._deassert_cs()
- # Read response bytes.
- return bytearray(self._ft232h._poll_read(length))
-
-
-class I2CDevice(object):
- """Class for communicating with an I2C device using the smbus library.
- Allows reading and writing 8-bit, 16-bit, and byte array values to registers
- on the device."""
- # Note that most of the functions in this code are adapted from this app note:
- # http://www.ftdichip.com/Support/Documents/AppNotes/AN_255_USB%20to%20I2C%20Example%20using%20the%20FT232H%20and%20FT201X%20devices.pdf
- def __init__(self, ft232h, address, clock_hz=100000):
- """Create an instance of the I2C device at the specified address on the
- specified I2C bus number."""
- self._address = address
- self._ft232h = ft232h
- # Enable clock with three phases for I2C.
- self._ft232h.mpsse_set_clock(clock_hz, three_phase=True)
- # Enable drive-zero mode to drive outputs low on 0 and tri-state on 1.
- # This matches the protocol for I2C communication so multiple devices can
- # share the I2C bus.
- self._ft232h._write('\x9E\x07\x00')
- self._idle()
-
- def _idle(self):
- """Put I2C lines into idle state."""
- # Put the I2C lines into an idle state with SCL and SDA high.
- self._ft232h.setup_pins({0: GPIO.OUT, 1: GPIO.OUT, 2: GPIO.IN},
- {0: GPIO.HIGH, 1: GPIO.HIGH})
-
- def _transaction_start(self):
- """Start I2C transaction."""
- # Clear command buffer and expected response bytes.
- self._command = []
- self._expected = 0
-
- def _transaction_end(self):
- """End I2C transaction and get response bytes, including ACKs."""
- # Ask to return response bytes immediately.
- self._command.append('\x87')
- # Send the entire command to the MPSSE.
- self._ft232h._write(''.join(self._command))
- # Read response bytes and return them.
- return bytearray(self._ft232h._poll_read(self._expected))
-
- def _i2c_start(self):
- """Send I2C start signal. Must be called within a transaction start/end.
- """
- # Set SCL high and SDA low, repeat 4 times to stay in this state for a
- # short period of time.
- self._ft232h.output_pins({0: GPIO.HIGH, 1: GPIO.LOW}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
- # Now drop SCL to low (again repeat 4 times for short delay).
- self._ft232h.output_pins({0: GPIO.LOW, 1: GPIO.LOW}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
-
- def _i2c_idle(self):
- """Set I2C signals to idle state with SCL and SDA at a high value. Must
- be called within a transaction start/end.
- """
- self._ft232h.output_pins({0: GPIO.HIGH, 1: GPIO.HIGH}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
-
- def _i2c_stop(self):
- """Send I2C stop signal. Must be called within a transaction start/end.
- """
- # Set SCL low and SDA low for a short period.
- self._ft232h.output_pins({0: GPIO.LOW, 1: GPIO.LOW}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
- # Set SCL high and SDA low for a short period.
- self._ft232h.output_pins({0: GPIO.HIGH, 1: GPIO.LOW}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
- # Finally set SCL high and SDA high for a short period.
- self._ft232h.output_pins({0: GPIO.HIGH, 1: GPIO.HIGH}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
-
- def _i2c_read_bytes(self, length=1):
- """Read the specified number of bytes from the I2C bus. Length is the
- number of bytes to read (must be 1 or more).
- """
- for i in range(length-1):
- # Read a byte and send ACK.
- self._command.append('\x20\x00\x00\x13\x00\x00')
- # Make sure pins are back in idle state with clock low and data high.
- self._ft232h.output_pins({0: GPIO.LOW, 1: GPIO.HIGH}, write=False)
- self._command.append(self._ft232h.mpsse_gpio())
- # Read last byte and send NAK.
- self._command.append('\x20\x00\x00\x13\x00\xFF')
- # Make sure pins are back in idle state with clock low and data high.
- self._ft232h.output_pins({0: GPIO.LOW, 1: GPIO.HIGH}, write=False)
- self._command.append(self._ft232h.mpsse_gpio())
- # Increase expected number of bytes.
- self._expected += length
-
- def _i2c_write_bytes(self, data):
- """Write the specified number of bytes to the chip."""
- for byte in data:
- # Write byte.
- self._command.append(str(bytearray((0x11, 0x00, 0x00, byte))))
- # Make sure pins are back in idle state with clock low and data high.
- self._ft232h.output_pins({0: GPIO.LOW, 1: GPIO.HIGH}, write=False)
- self._command.append(self._ft232h.mpsse_gpio() * _REPEAT_DELAY)
- # Read bit for ACK/NAK.
- self._command.append('\x22\x00')
- # Increase expected response bytes.
- self._expected += len(data)
-
- def _address_byte(self, read=True):
- """Return the address byte with the specified R/W bit set. If read is
- True the R/W bit will be 1, otherwise the R/W bit will be 0.
- """
- if read:
- return (self._address << 1) | 0x01
- else:
- return self._address << 1
-
- def _verify_acks(self, response):
- """Check all the specified bytes have the ACK bit set. Throws a
- RuntimeError exception if not all the ACKs are set.
- """
- for byte in response:
- if byte & 0x01 != 0x00:
- raise RuntimeError('Failed to find expected I2C ACK!')
-
- def ping(self):
- """Attempt to detect if a device at this address is present on the I2C
- bus. Will send out the device's address for writing and verify an ACK
- is received. Returns true if the ACK is received, and false if not.
- """
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False)])
- self._i2c_stop()
- response = self._transaction_end()
- if len(response) != 1:
- raise RuntimeError('Expected 1 response byte but received {0} byte(s).'.format(len(response)))
- return ((response[0] & 0x01) == 0x00)
-
- def writeRaw8(self, value):
- """Write an 8-bit value on the bus (without register)."""
- value = value & 0xFF
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False), value])
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response)
-
- def write8(self, register, value):
- """Write an 8-bit value to the specified register."""
- value = value & 0xFF
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False), register, value])
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response)
-
- def write16(self, register, value, little_endian=True):
- """Write a 16-bit value to the specified register."""
- value = value & 0xFFFF
- value_low = value & 0xFF
- value_high = (value >> 8) & 0xFF
- if not little_endian:
- value_low, value_high = value_high, value_low
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False), register, value_low,
- value_high])
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response)
-
- def writeList(self, register, data):
- """Write bytes to the specified register."""
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False), register] + data)
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response)
-
- def readList(self, register, length):
- """Read a length number of bytes from the specified register. Results
- will be returned as a bytearray."""
- if length <= 0:
- raise ValueError("Length must be at least 1 byte.")
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(True), register])
- self._i2c_stop()
- self._i2c_idle()
- self._i2c_start()
- self._i2c_read_bytes(length)
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response[:-length])
- return response[-length:]
-
- def readRaw8(self):
- """Read an 8-bit value on the bus (without register)."""
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False)])
- self._i2c_stop()
- self._i2c_idle()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(True)])
- self._i2c_read_bytes(1)
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response[:-1])
- return response[-1]
-
- def readU8(self, register):
- """Read an unsigned byte from the specified register."""
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False), register])
- self._i2c_stop()
- self._i2c_idle()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(True)])
- self._i2c_read_bytes(1)
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response[:-1])
- return response[-1]
-
- def readS8(self, register):
- """Read a signed byte from the specified register."""
- result = self.readU8(register)
- if result > 127:
- result -= 256
- return result
-
- def readU16(self, register, little_endian=True):
- """Read an unsigned 16-bit value from the specified register, with the
- specified endianness (default little endian, or least significant byte
- first)."""
- self._idle()
- self._transaction_start()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(False), register])
- self._i2c_stop()
- self._i2c_idle()
- self._i2c_start()
- self._i2c_write_bytes([self._address_byte(True)])
- self._i2c_read_bytes(2)
- self._i2c_stop()
- response = self._transaction_end()
- self._verify_acks(response[:-2])
- if little_endian:
- return (response[-1] << 8) | response[-2]
- else:
- return (response[-2] << 8) | response[-1]
-
- def readS16(self, register, little_endian=True):
- """Read a signed 16-bit value from the specified register, with the
- specified endianness (default little endian, or least significant byte
- first)."""
- result = self.readU16(register, little_endian)
- if result > 32767:
- result -= 65536
- return result
-
- def readU16LE(self, register):
- """Read an unsigned 16-bit value from the specified register, in little
- endian byte order."""
- return self.readU16(register, little_endian=True)
-
- def readU16BE(self, register):
- """Read an unsigned 16-bit value from the specified register, in big
- endian byte order."""
- return self.readU16(register, little_endian=False)
-
- def readS16LE(self, register):
- """Read a signed 16-bit value from the specified register, in little
- endian byte order."""
- return self.readS16(register, little_endian=True)
-
- def readS16BE(self, register):
- """Read a signed 16-bit value from the specified register, in big
- endian byte order."""
- return self.readS16(register, little_endian=False)
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/GPIO.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/GPIO.py
deleted file mode 100644
index 08e99c6de4..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/GPIO.py
+++ /dev/null
@@ -1,426 +0,0 @@
-# Copyright (c) 2014 Adafruit Industries
-# Author: Tony DiCola
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-
-import Adafruit_GPIO.Platform as Platform
-
-
-OUT = 0
-IN = 1
-HIGH = True
-LOW = False
-
-RISING = 1
-FALLING = 2
-BOTH = 3
-
-PUD_OFF = 0
-PUD_DOWN = 1
-PUD_UP = 2
-
-class BaseGPIO(object):
- """Base class for implementing simple digital IO for a platform.
- Implementors are expected to subclass from this and provide an implementation
- of the setup, output, and input functions."""
-
- def setup(self, pin, mode, pull_up_down=PUD_OFF):
- """Set the input or output mode for a specified pin. Mode should be
- either OUT or IN."""
- raise NotImplementedError
-
- def output(self, pin, value):
- """Set the specified pin the provided high/low value. Value should be
- either HIGH/LOW or a boolean (true = high)."""
- raise NotImplementedError
-
- def input(self, pin):
- """Read the specified pin and return HIGH/true if the pin is pulled high,
- or LOW/false if pulled low."""
- raise NotImplementedError
-
- def set_high(self, pin):
- """Set the specified pin HIGH."""
- self.output(pin, HIGH)
-
- def set_low(self, pin):
- """Set the specified pin LOW."""
- self.output(pin, LOW)
-
- def is_high(self, pin):
- """Return true if the specified pin is pulled high."""
- return self.input(pin) == HIGH
-
- def is_low(self, pin):
- """Return true if the specified pin is pulled low."""
- return self.input(pin) == LOW
-
-
-# Basic implementation of multiple pin methods just loops through pins and
-# processes each one individually. This is not optimal, but derived classes can
-# provide a more optimal implementation that deals with groups of pins
-# simultaneously.
-# See MCP230xx or PCF8574 classes for examples of optimized implementations.
-
- def output_pins(self, pins):
- """Set multiple pins high or low at once. Pins should be a dict of pin
- name to pin value (HIGH/True for 1, LOW/False for 0). All provided pins
- will be set to the given values.
- """
- # General implementation just loops through pins and writes them out
- # manually. This is not optimized, but subclasses can choose to implement
- # a more optimal batch output implementation. See the MCP230xx class for
- # example of optimized implementation.
- for pin, value in iter(pins.items()):
- self.output(pin, value)
-
- def setup_pins(self, pins):
- """Setup multiple pins as inputs or outputs at once. Pins should be a
- dict of pin name to pin type (IN or OUT).
- """
- # General implementation that can be optimized by derived classes.
- for pin, value in iter(pins.items()):
- self.setup(pin, value)
-
- def input_pins(self, pins):
- """Read multiple pins specified in the given list and return list of pin values
- GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
- """
- # General implementation that can be optimized by derived classes.
- return [self.input(pin) for pin in pins]
-
-
- def add_event_detect(self, pin, edge):
- """Enable edge detection events for a particular GPIO channel. Pin
- should be type IN. Edge must be RISING, FALLING or BOTH.
- """
- raise NotImplementedError
-
- def remove_event_detect(self, pin):
- """Remove edge detection for a particular GPIO channel. Pin should be
- type IN.
- """
- raise NotImplementedError
-
- def add_event_callback(self, pin, callback):
- """Add a callback for an event already defined using add_event_detect().
- Pin should be type IN.
- """
- raise NotImplementedError
-
- def event_detected(self, pin):
- """Returns True if an edge has occured on a given GPIO. You need to
- enable edge detection using add_event_detect() first. Pin should be
- type IN.
- """
- raise NotImplementedError
-
- def wait_for_edge(self, pin, edge):
- """Wait for an edge. Pin should be type IN. Edge must be RISING,
- FALLING or BOTH."""
- raise NotImplementedError
-
- def cleanup(self, pin=None):
- """Clean up GPIO event detection for specific pin, or all pins if none
- is specified.
- """
- raise NotImplementedError
-
-
-# helper functions useful to derived classes
-
- def _validate_pin(self, pin):
- # Raise an exception if pin is outside the range of allowed values.
- if pin < 0 or pin >= self.NUM_GPIO:
- raise ValueError('Invalid GPIO value, must be between 0 and {0}.'.format(self.NUM_GPIO))
-
- def _bit2(self, src, bit, val):
- bit = 1 << bit
- return (src | bit) if val else (src & ~bit)
-
-
-class RPiGPIOAdapter(BaseGPIO):
- """GPIO implementation for the Raspberry Pi using the RPi.GPIO library."""
-
- def __init__(self, rpi_gpio, mode=None):
- self.rpi_gpio = rpi_gpio
- # Suppress warnings about GPIO in use.
- rpi_gpio.setwarnings(False)
- # Setup board pin mode.
- if mode == rpi_gpio.BOARD or mode == rpi_gpio.BCM:
- rpi_gpio.setmode(mode)
- elif mode is not None:
- raise ValueError('Unexpected value for mode. Must be BOARD or BCM.')
- else:
- # Default to BCM numbering if not told otherwise.
- rpi_gpio.setmode(rpi_gpio.BCM)
- # Define mapping of Adafruit GPIO library constants to RPi.GPIO constants.
- self._dir_mapping = { OUT: rpi_gpio.OUT,
- IN: rpi_gpio.IN }
- self._pud_mapping = { PUD_OFF: rpi_gpio.PUD_OFF,
- PUD_DOWN: rpi_gpio.PUD_DOWN,
- PUD_UP: rpi_gpio.PUD_UP }
- self._edge_mapping = { RISING: rpi_gpio.RISING,
- FALLING: rpi_gpio.FALLING,
- BOTH: rpi_gpio.BOTH }
-
- def setup(self, pin, mode, pull_up_down=PUD_OFF):
- """Set the input or output mode for a specified pin. Mode should be
- either OUTPUT or INPUT.
- """
- self.rpi_gpio.setup(pin, self._dir_mapping[mode],
- pull_up_down=self._pud_mapping[pull_up_down])
-
- def output(self, pin, value):
- """Set the specified pin the provided high/low value. Value should be
- either HIGH/LOW or a boolean (true = high).
- """
- self.rpi_gpio.output(pin, value)
-
- def input(self, pin):
- """Read the specified pin and return HIGH/true if the pin is pulled high,
- or LOW/false if pulled low.
- """
- return self.rpi_gpio.input(pin)
-
- def input_pins(self, pins):
- """Read multiple pins specified in the given list and return list of pin values
- GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
- """
- # maybe rpi has a mass read... it would be more efficient to use it if it exists
- return [self.rpi_gpio.input(pin) for pin in pins]
-
- def add_event_detect(self, pin, edge, callback=None, bouncetime=-1):
- """Enable edge detection events for a particular GPIO channel. Pin
- should be type IN. Edge must be RISING, FALLING or BOTH. Callback is a
- function for the event. Bouncetime is switch bounce timeout in ms for
- callback
- """
- kwargs = {}
- if callback:
- kwargs['callback']=callback
- if bouncetime > 0:
- kwargs['bouncetime']=bouncetime
- self.rpi_gpio.add_event_detect(pin, self._edge_mapping[edge], **kwargs)
-
- def remove_event_detect(self, pin):
- """Remove edge detection for a particular GPIO channel. Pin should be
- type IN.
- """
- self.rpi_gpio.remove_event_detect(pin)
-
- def add_event_callback(self, pin, callback):
- """Add a callback for an event already defined using add_event_detect().
- Pin should be type IN.
- """
- self.rpi_gpio.add_event_callback(pin, callback)
-
- def event_detected(self, pin):
- """Returns True if an edge has occured on a given GPIO. You need to
- enable edge detection using add_event_detect() first. Pin should be
- type IN.
- """
- return self.rpi_gpio.event_detected(pin)
-
- def wait_for_edge(self, pin, edge):
- """Wait for an edge. Pin should be type IN. Edge must be RISING,
- FALLING or BOTH.
- """
- self.rpi_gpio.wait_for_edge(pin, self._edge_mapping[edge])
-
- def cleanup(self, pin=None):
- """Clean up GPIO event detection for specific pin, or all pins if none
- is specified.
- """
- if pin is None:
- self.rpi_gpio.cleanup()
- else:
- self.rpi_gpio.cleanup(pin)
-
-class AdafruitBBIOAdapter(BaseGPIO):
- """GPIO implementation for the Beaglebone Black using the Adafruit_BBIO
- library.
- """
-
- def __init__(self, bbio_gpio):
- self.bbio_gpio = bbio_gpio
- # Define mapping of Adafruit GPIO library constants to RPi.GPIO constants.
- self._dir_mapping = { OUT: bbio_gpio.OUT,
- IN: bbio_gpio.IN }
- self._pud_mapping = { PUD_OFF: bbio_gpio.PUD_OFF,
- PUD_DOWN: bbio_gpio.PUD_DOWN,
- PUD_UP: bbio_gpio.PUD_UP }
- self._edge_mapping = { RISING: bbio_gpio.RISING,
- FALLING: bbio_gpio.FALLING,
- BOTH: bbio_gpio.BOTH }
-
- def setup(self, pin, mode, pull_up_down=PUD_OFF):
- """Set the input or output mode for a specified pin. Mode should be
- either OUTPUT or INPUT.
- """
- self.bbio_gpio.setup(pin, self._dir_mapping[mode],
- pull_up_down=self._pud_mapping[pull_up_down])
-
- def output(self, pin, value):
- """Set the specified pin the provided high/low value. Value should be
- either HIGH/LOW or a boolean (true = high).
- """
- self.bbio_gpio.output(pin, value)
-
- def input(self, pin):
- """Read the specified pin and return HIGH/true if the pin is pulled high,
- or LOW/false if pulled low.
- """
- return self.bbio_gpio.input(pin)
-
- def input_pins(self, pins):
- """Read multiple pins specified in the given list and return list of pin values
- GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
- """
- # maybe bbb has a mass read... it would be more efficient to use it if it exists
- return [self.bbio_gpio.input(pin) for pin in pins]
-
- def add_event_detect(self, pin, edge, callback=None, bouncetime=-1):
- """Enable edge detection events for a particular GPIO channel. Pin
- should be type IN. Edge must be RISING, FALLING or BOTH. Callback is a
- function for the event. Bouncetime is switch bounce timeout in ms for
- callback
- """
- kwargs = {}
- if callback:
- kwargs['callback']=callback
- if bouncetime > 0:
- kwargs['bouncetime']=bouncetime
- self.bbio_gpio.add_event_detect(pin, self._edge_mapping[edge], **kwargs)
-
- def remove_event_detect(self, pin):
- """Remove edge detection for a particular GPIO channel. Pin should be
- type IN.
- """
- self.bbio_gpio.remove_event_detect(pin)
-
- def add_event_callback(self, pin, callback, bouncetime=-1):
- """Add a callback for an event already defined using add_event_detect().
- Pin should be type IN. Bouncetime is switch bounce timeout in ms for
- callback
- """
- kwargs = {}
- if bouncetime > 0:
- kwargs['bouncetime']=bouncetime
- self.bbio_gpio.add_event_callback(pin, callback, **kwargs)
-
- def event_detected(self, pin):
- """Returns True if an edge has occured on a given GPIO. You need to
- enable edge detection using add_event_detect() first. Pin should be
- type IN.
- """
- return self.bbio_gpio.event_detected(pin)
-
- def wait_for_edge(self, pin, edge):
- """Wait for an edge. Pin should be type IN. Edge must be RISING,
- FALLING or BOTH.
- """
- self.bbio_gpio.wait_for_edge(pin, self._edge_mapping[edge])
-
- def cleanup(self, pin=None):
- """Clean up GPIO event detection for specific pin, or all pins if none
- is specified.
- """
- if pin is None:
- self.bbio_gpio.cleanup()
- else:
- self.bbio_gpio.cleanup(pin)
-
-class AdafruitMinnowAdapter(BaseGPIO):
- """GPIO implementation for the Minnowboard + MAX using the mraa library"""
-
- def __init__(self,mraa_gpio):
- self.mraa_gpio = mraa_gpio
- # Define mapping of Adafruit GPIO library constants to mraa constants
- self._dir_mapping = { OUT: self.mraa_gpio.DIR_OUT,
- IN: self.mraa_gpio.DIR_IN }
- self._pud_mapping = { PUD_OFF: self.mraa_gpio.MODE_STRONG,
- PUD_UP: self.mraa_gpio.MODE_HIZ,
- PUD_DOWN: self.mraa_gpio.MODE_PULLDOWN }
- self._edge_mapping = { RISING: self.mraa_gpio.EDGE_RISING,
- FALLING: self.mraa_gpio.EDGE_FALLING,
- BOTH: self.mraa_gpio.EDGE_BOTH }
-
- def setup(self,pin,mode):
- """Set the input or output mode for a specified pin. Mode should be
- either DIR_IN or DIR_OUT.
- """
- self.mraa_gpio.Gpio.dir(self.mraa_gpio.Gpio(pin),self._dir_mapping[mode])
-
- def output(self,pin,value):
- """Set the specified pin the provided high/low value. Value should be
- either 1 (ON or HIGH), or 0 (OFF or LOW) or a boolean.
- """
- self.mraa_gpio.Gpio.write(self.mraa_gpio.Gpio(pin), value)
-
- def input(self,pin):
- """Read the specified pin and return HIGH/true if the pin is pulled high,
- or LOW/false if pulled low.
- """
- return self.mraa_gpio.Gpio.read(self.mraa_gpio.Gpio(pin))
-
- def add_event_detect(self, pin, edge, callback=None, bouncetime=-1):
- """Enable edge detection events for a particular GPIO channel. Pin
- should be type IN. Edge must be RISING, FALLING or BOTH. Callback is a
- function for the event. Bouncetime is switch bounce timeout in ms for
- callback
- """
- kwargs = {}
- if callback:
- kwargs['callback']=callback
- if bouncetime > 0:
- kwargs['bouncetime']=bouncetime
- self.mraa_gpio.Gpio.isr(self.mraa_gpio.Gpio(pin), self._edge_mapping[edge], **kwargs)
-
- def remove_event_detect(self, pin):
- """Remove edge detection for a particular GPIO channel. Pin should be
- type IN.
- """
- self.mraa_gpio.Gpio.isrExit(self.mraa_gpio.Gpio(pin))
-
- def wait_for_edge(self, pin, edge):
- """Wait for an edge. Pin should be type IN. Edge must be RISING,
- FALLING or BOTH.
- """
- self.bbio_gpio.wait_for_edge(self.mraa_gpio.Gpio(pin), self._edge_mapping[edge])
-
-def get_platform_gpio(**keywords):
- """Attempt to return a GPIO instance for the platform which the code is being
- executed on. Currently supports only the Raspberry Pi using the RPi.GPIO
- library and Beaglebone Black using the Adafruit_BBIO library. Will throw an
- exception if a GPIO instance can't be created for the current platform. The
- returned GPIO object is an instance of BaseGPIO.
- """
- plat = Platform.platform_detect()
- if plat == Platform.RASPBERRY_PI:
- import RPi.GPIO
- return RPiGPIOAdapter(RPi.GPIO, **keywords)
- elif plat == Platform.BEAGLEBONE_BLACK:
- import Adafruit_BBIO.GPIO
- return AdafruitBBIOAdapter(Adafruit_BBIO.GPIO, **keywords)
- elif plat == Platform.MINNOWBOARD:
- import mraa
- return AdafruitMinnowAdapter(mraa, **keywords)
- elif plat == Platform.UNKNOWN:
- raise RuntimeError('Could not determine platform.')
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/I2C.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/I2C.py
deleted file mode 100644
index 49a6e57632..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/I2C.py
+++ /dev/null
@@ -1,195 +0,0 @@
-# Copyright (c) 2014 Adafruit Industries
-# Author: Tony DiCola
-# Based on Adafruit_I2C.py created by Kevin Townsend.
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-import logging
-import subprocess
-
-import smbus
-
-import Adafruit_GPIO.Platform as Platform
-
-
-def reverseByteOrder(data):
- """Reverses the byte order of an int (16-bit) or long (32-bit) value."""
- # Courtesy Vishal Sapre
- byteCount = len(hex(data)[2:].replace('L','')[::2])
- val = 0
- for i in range(byteCount):
- val = (val << 8) | (data & 0xff)
- data >>= 8
- return val
-
-def get_default_bus():
- """Return the default bus number based on the device platform. For a
- Raspberry Pi either bus 0 or 1 (based on the Pi revision) will be returned.
- For a Beaglebone Black the first user accessible bus, 1, will be returned.
- """
- plat = Platform.platform_detect()
- if plat == Platform.RASPBERRY_PI:
- if Platform.pi_revision() == 1:
- # Revision 1 Pi uses I2C bus 0.
- return 0
- else:
- # Revision 2 Pi uses I2C bus 1.
- return 1
- elif plat == Platform.BEAGLEBONE_BLACK:
- # Beaglebone Black has multiple I2C buses, default to 1 (P9_19 and P9_20).
- return 1
- else:
- raise RuntimeError('Could not determine default I2C bus for platform.')
-
-def get_i2c_device(address, busnum=None, **kwargs):
- """Return an I2C device for the specified address and on the specified bus.
- If busnum isn't specified, the default I2C bus for the platform will attempt
- to be detected.
- """
- if busnum is None:
- busnum = get_default_bus()
- return Device(address, busnum, **kwargs)
-
-def require_repeated_start():
- """Enable repeated start conditions for I2C register reads. This is the
- normal behavior for I2C, however on some platforms like the Raspberry Pi
- there are bugs which disable repeated starts unless explicitly enabled with
- this function. See this thread for more details:
- http://www.raspberrypi.org/forums/viewtopic.php?f=44&t=15840
- """
- plat = Platform.platform_detect()
- if plat == Platform.RASPBERRY_PI:
- # On the Raspberry Pi there is a bug where register reads don't send a
- # repeated start condition like the kernel smbus I2C driver functions
- # define. As a workaround this bit in the BCM2708 driver sysfs tree can
- # be changed to enable I2C repeated starts.
- subprocess.check_call('chmod 666 /sys/module/i2c_bcm2708/parameters/combined', shell=True)
- subprocess.check_call('echo -n 1 > /sys/module/i2c_bcm2708/parameters/combined', shell=True)
- # Other platforms are a no-op because they (presumably) have the correct
- # behavior and send repeated starts.
-
-
-class Device(object):
- """Class for communicating with an I2C device using the smbus library.
- Allows reading and writing 8-bit, 16-bit, and byte array values to registers
- on the device."""
- def __init__(self, address, busnum):
- """Create an instance of the I2C device at the specified address on the
- specified I2C bus number."""
- self._address = address
- self._bus = smbus.SMBus(busnum)
- self._logger = logging.getLogger('Adafruit_I2C.Device.Bus.{0}.Address.{1:#0X}' \
- .format(busnum, address))
-
- def writeRaw8(self, value):
- """Write an 8-bit value on the bus (without register)."""
- value = value & 0xFF
- self._bus.write_byte(self._address, value)
- self._logger.debug("Wrote 0x%02X",
- value)
-
- def write8(self, register, value):
- """Write an 8-bit value to the specified register."""
- value = value & 0xFF
- self._bus.write_byte_data(self._address, register, value)
- self._logger.debug("Wrote 0x%02X to register 0x%02X",
- value, register)
-
- def write16(self, register, value):
- """Write a 16-bit value to the specified register."""
- value = value & 0xFFFF
- self._bus.write_word_data(self._address, register, value)
- self._logger.debug("Wrote 0x%04X to register pair 0x%02X, 0x%02X",
- value, register, register+1)
-
- def writeList(self, register, data):
- """Write bytes to the specified register."""
- self._bus.write_i2c_block_data(self._address, register, data)
- self._logger.debug("Wrote to register 0x%02X: %s",
- register, data)
-
- def readList(self, register, length):
- """Read a length number of bytes from the specified register. Results
- will be returned as a bytearray."""
- results = self._bus.read_i2c_block_data(self._address, register, length)
- self._logger.debug("Read the following from register 0x%02X: %s",
- register, results)
- return results
-
- def readRaw8(self):
- """Read an 8-bit value on the bus (without register)."""
- result = self._bus.read_byte(self._address) & 0xFF
- self._logger.debug("Read 0x%02X",
- result)
- return result
-
- def readU8(self, register):
- """Read an unsigned byte from the specified register."""
- result = self._bus.read_byte_data(self._address, register) & 0xFF
- self._logger.debug("Read 0x%02X from register 0x%02X",
- result, register)
- return result
-
- def readS8(self, register):
- """Read a signed byte from the specified register."""
- result = self.readU8(register)
- if result > 127:
- result -= 256
- return result
-
- def readU16(self, register, little_endian=True):
- """Read an unsigned 16-bit value from the specified register, with the
- specified endianness (default little endian, or least significant byte
- first)."""
- result = self._bus.read_word_data(self._address,register) & 0xFFFF
- self._logger.debug("Read 0x%04X from register pair 0x%02X, 0x%02X",
- result, register, register+1)
- # Swap bytes if using big endian because read_word_data assumes little
- # endian on ARM (little endian) systems.
- if not little_endian:
- result = ((result << 8) & 0xFF00) + (result >> 8)
- return result
-
- def readS16(self, register, little_endian=True):
- """Read a signed 16-bit value from the specified register, with the
- specified endianness (default little endian, or least significant byte
- first)."""
- result = self.readU16(register, little_endian)
- if result > 32767:
- result -= 65536
- return result
-
- def readU16LE(self, register):
- """Read an unsigned 16-bit value from the specified register, in little
- endian byte order."""
- return self.readU16(register, little_endian=True)
-
- def readU16BE(self, register):
- """Read an unsigned 16-bit value from the specified register, in big
- endian byte order."""
- return self.readU16(register, little_endian=False)
-
- def readS16LE(self, register):
- """Read a signed 16-bit value from the specified register, in little
- endian byte order."""
- return self.readS16(register, little_endian=True)
-
- def readS16BE(self, register):
- """Read a signed 16-bit value from the specified register, in big
- endian byte order."""
- return self.readS16(register, little_endian=False)
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/MCP230xx.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/MCP230xx.py
deleted file mode 100644
index 8496ccd193..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/MCP230xx.py
+++ /dev/null
@@ -1,165 +0,0 @@
-# Copyright (c) 2014 Adafruit Industries
-# Author: Tony DiCola
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-import math
-
-import Adafruit_GPIO as GPIO
-import Adafruit_GPIO.I2C as I2C
-
-
-class MCP230xxBase(GPIO.BaseGPIO):
- """Base class to represent an MCP230xx series GPIO extender. Is compatible
- with the Adafruit_GPIO BaseGPIO class so it can be used as a custom GPIO
- class for interacting with device.
- """
-
- def __init__(self, address, i2c=None, **kwargs):
- """Initialize MCP230xx at specified I2C address and bus number. If bus
- is not specified it will default to the appropriate platform detected bus.
- """
- # Create I2C device.
- if i2c is None:
- import Adafruit_GPIO.I2C as I2C
- i2c = I2C
- self._device = i2c.get_i2c_device(address, **kwargs)
- # Assume starting in ICON.BANK = 0 mode (sequential access).
- # Compute how many bytes are needed to store count of GPIO.
- self.gpio_bytes = int(math.ceil(self.NUM_GPIO/8.0))
- # Buffer register values so they can be changed without reading.
- self.iodir = [0x00]*self.gpio_bytes # Default direction to all inputs.
- self.gppu = [0x00]*self.gpio_bytes # Default to pullups disabled.
- self.gpio = [0x00]*self.gpio_bytes
- # Write current direction and pullup buffer state.
- self.write_iodir()
- self.write_gppu()
-
-
- def setup(self, pin, value):
- """Set the input or output mode for a specified pin. Mode should be
- either GPIO.OUT or GPIO.IN.
- """
- self._validate_pin(pin)
- # Set bit to 1 for input or 0 for output.
- if value == GPIO.IN:
- self.iodir[int(pin/8)] |= 1 << (int(pin%8))
- elif value == GPIO.OUT:
- self.iodir[int(pin/8)] &= ~(1 << (int(pin%8)))
- else:
- raise ValueError('Unexpected value. Must be GPIO.IN or GPIO.OUT.')
- self.write_iodir()
-
-
- def output(self, pin, value):
- """Set the specified pin the provided high/low value. Value should be
- either GPIO.HIGH/GPIO.LOW or a boolean (True = HIGH).
- """
- self.output_pins({pin: value})
-
- def output_pins(self, pins):
- """Set multiple pins high or low at once. Pins should be a dict of pin
- name to pin value (HIGH/True for 1, LOW/False for 0). All provided pins
- will be set to the given values.
- """
- [self._validate_pin(pin) for pin in pins.keys()]
- # Set each changed pin's bit.
- for pin, value in iter(pins.items()):
- if value:
- self.gpio[int(pin/8)] |= 1 << (int(pin%8))
- else:
- self.gpio[int(pin/8)] &= ~(1 << (int(pin%8)))
- # Write GPIO state.
- self.write_gpio()
-
-
- def input(self, pin):
- """Read the specified pin and return GPIO.HIGH/True if the pin is pulled
- high, or GPIO.LOW/False if pulled low.
- """
- return self.input_pins([pin])[0]
-
- def input_pins(self, pins):
- """Read multiple pins specified in the given list and return list of pin values
- GPIO.HIGH/True if the pin is pulled high, or GPIO.LOW/False if pulled low.
- """
- [self._validate_pin(pin) for pin in pins]
- # Get GPIO state.
- gpio = self._device.readList(self.GPIO, self.gpio_bytes)
- # Return True if pin's bit is set.
- return [(gpio[int(pin/8)] & 1 << (int(pin%8))) > 0 for pin in pins]
-
-
- def pullup(self, pin, enabled):
- """Turn on the pull-up resistor for the specified pin if enabled is True,
- otherwise turn off the pull-up resistor.
- """
- self._validate_pin(pin)
- if enabled:
- self.gppu[int(pin/8)] |= 1 << (int(pin%8))
- else:
- self.gppu[int(pin/8)] &= ~(1 << (int(pin%8)))
- self.write_gppu()
-
- def write_gpio(self, gpio=None):
- """Write the specified byte value to the GPIO registor. If no value
- specified the current buffered value will be written.
- """
- if gpio is not None:
- self.gpio = gpio
- self._device.writeList(self.GPIO, self.gpio)
-
- def write_iodir(self, iodir=None):
- """Write the specified byte value to the IODIR registor. If no value
- specified the current buffered value will be written.
- """
- if iodir is not None:
- self.iodir = iodir
- self._device.writeList(self.IODIR, self.iodir)
-
- def write_gppu(self, gppu=None):
- """Write the specified byte value to the GPPU registor. If no value
- specified the current buffered value will be written.
- """
- if gppu is not None:
- self.gppu = gppu
- self._device.writeList(self.GPPU, self.gppu)
-
-
-class MCP23017(MCP230xxBase):
- """MCP23017-based GPIO class with 16 GPIO pins."""
- # Define number of pins and registor addresses.
- NUM_GPIO = 16
- IODIR = 0x00
- GPIO = 0x12
- GPPU = 0x0C
-
- def __init__(self, address=0x20, **kwargs):
- super(MCP23017, self).__init__(address, **kwargs)
-
-
-class MCP23008(MCP230xxBase):
- """MCP23008-based GPIO class with 8 GPIO pins."""
- # Define number of pins and registor addresses.
- NUM_GPIO = 8
- IODIR = 0x00
- GPIO = 0x09
- GPPU = 0x06
-
- def __init__(self, address=0x20, **kwargs):
- super(MCP23008, self).__init__(address, **kwargs)
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/PCF8574.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/PCF8574.py
deleted file mode 100644
index fe58e52435..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/PCF8574.py
+++ /dev/null
@@ -1,94 +0,0 @@
-'''
-Adafruit compatible using BaseGPIO class to represent a PCF8574/A IO expander
-Copyright (C) 2015 Sylvan Butler
-
-Permission is hereby granted, free of charge, to any person obtaining a copy of
-this software and associated documentation files (the "Software"), to deal in
-the Software without restriction, including without limitation the rights to
-use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
-of the Software, and to permit persons to whom the Software is furnished to do
-so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.'''
-
-import Adafruit_GPIO as GPIO
-import Adafruit_GPIO.I2C as I2C
-
-
-
-IN = GPIO.IN
-OUT = GPIO.OUT
-HIGH = GPIO.HIGH
-LOW = GPIO.LOW
-
-
-class PCF8574(GPIO.BaseGPIO):
- """Class to represent a PCF8574 or PCF8574A GPIO extender. Compatible
- with the Adafruit_GPIO BaseGPIO class so it can be used as a custom GPIO
- class for interacting with device.
- """
-
- NUM_GPIO = 8
-
- def __init__(self, address=0x27, busnum=None, i2c=None, **kwargs):
- address = int(address)
- self.__name__ = \
- "PCF8574" if address in range(0x20, 0x28) else \
- "PCF8574A" if address in range(0x38, 0x40) else \
- "Bad address for PCF8574(A): 0x%02X not in range [0x20..0x27, 0x38..0x3F]" % address
- if self.__name__[0] != 'P':
- raise ValueError(self.__name__)
- # Create I2C device.
- i2c = i2c or I2C
- busnum = busnum or i2c.get_default_bus()
- self._device = i2c.get_i2c_device(address, busnum, **kwargs)
- # Buffer register values so they can be changed without reading.
- self.iodir = 0xFF # Default direction to all inputs is in
- self.gpio = 0x00
- self._write_pins()
-
-
- def _write_pins(self):
- self._device.writeRaw8(self.gpio | self.iodir)
-
- def _read_pins(self):
- return self._device.readRaw8() & self.iodir
-
-
- def setup(self, pin, mode):
- self.setup_pins({pin: mode})
-
- def setup_pins(self, pins):
- if False in [y for x,y in [(self._validate_pin(pin),mode in (IN,OUT)) for pin,mode in pins.iteritems()]]:
- raise ValueError('Invalid MODE, IN or OUT')
- for pin,mode in pins.iteritems():
- self.iodir = self._bit2(self.iodir, pin, mode)
- self._write_pins()
-
-
- def output(self, pin, value):
- self.output_pins({pin: value})
-
- def output_pins(self, pins):
- [self._validate_pin(pin) for pin in pins.keys()]
- for pin,value in pins.iteritems():
- self.gpio = self._bit2(self.gpio, pin, bool(value))
- self._write_pins()
-
-
- def input(self, pin):
- return self.input_pins([pin])[0]
-
- def input_pins(self, pins):
- [self._validate_pin(pin) for pin in pins]
- inp = self._read_pins()
- return [bool(inp & (1< 100.0:
- raise ValueError('Invalid duty cycle value, must be between 0.0 to 100.0 (inclusive).')
- # Make pin an output.
- self.rpi_gpio.setup(pin, self.rpi_gpio.OUT)
- # Create PWM instance and save a reference for later access.
- self.pwm[pin] = self.rpi_gpio.PWM(pin, frequency_hz)
- # Start the PWM at the specified duty cycle.
- self.pwm[pin].start(dutycycle)
-
- def set_duty_cycle(self, pin, dutycycle):
- """Set percent duty cycle of PWM output on specified pin. Duty cycle must
- be a value 0.0 to 100.0 (inclusive).
- """
- if dutycycle < 0.0 or dutycycle > 100.0:
- raise ValueError('Invalid duty cycle value, must be between 0.0 to 100.0 (inclusive).')
- if pin not in self.pwm:
- raise ValueError('Pin {0} is not configured as a PWM. Make sure to first call start for the pin.'.format(pin))
- self.pwm[pin].ChangeDutyCycle(dutycycle)
-
- def set_frequency(self, pin, frequency_hz):
- """Set frequency (in Hz) of PWM output on specified pin."""
- if pin not in self.pwm:
- raise ValueError('Pin {0} is not configured as a PWM. Make sure to first call start for the pin.'.format(pin))
- self.pwm[pin].ChangeFrequency(frequency_hz)
-
- def stop(self, pin):
- """Stop PWM output on specified pin."""
- if pin not in self.pwm:
- raise ValueError('Pin {0} is not configured as a PWM. Make sure to first call start for the pin.'.format(pin))
- self.pwm[pin].stop()
- del self.pwm[pin]
-
-
-class BBIO_PWM_Adapter(object):
- """PWM implementation for the BeagleBone Black using the Adafruit_BBIO.PWM
- library.
- """
-
- def __init__(self, bbio_pwm):
- self.bbio_pwm = bbio_pwm
-
- def start(self, pin, dutycycle, frequency_hz=2000):
- """Enable PWM output on specified pin. Set to intiial percent duty cycle
- value (0.0 to 100.0) and frequency (in Hz).
- """
- if dutycycle < 0.0 or dutycycle > 100.0:
- raise ValueError('Invalid duty cycle value, must be between 0.0 to 100.0 (inclusive).')
- self.bbio_pwm.start(pin, dutycycle, frequency_hz)
-
- def set_duty_cycle(self, pin, dutycycle):
- """Set percent duty cycle of PWM output on specified pin. Duty cycle must
- be a value 0.0 to 100.0 (inclusive).
- """
- if dutycycle < 0.0 or dutycycle > 100.0:
- raise ValueError('Invalid duty cycle value, must be between 0.0 to 100.0 (inclusive).')
- self.bbio_pwm.set_duty_cycle(pin, dutycycle)
-
- def set_frequency(self, pin, frequency_hz):
- """Set frequency (in Hz) of PWM output on specified pin."""
- self.bbio_pwm.set_frequency(pin, frequency_hz)
-
- def stop(self, pin):
- """Stop PWM output on specified pin."""
- self.bbio_pwm.stop(pin)
-
-
-def get_platform_pwm(**keywords):
- """Attempt to return a PWM instance for the platform which the code is being
- executed on. Currently supports only the Raspberry Pi using the RPi.GPIO
- library and Beaglebone Black using the Adafruit_BBIO library. Will throw an
- exception if a PWM instance can't be created for the current platform. The
- returned PWM object has the same interface as the RPi_PWM_Adapter and
- BBIO_PWM_Adapter classes.
- """
- plat = Platform.platform_detect()
- if plat == Platform.RASPBERRY_PI:
- import RPi.GPIO
- return RPi_PWM_Adapter(RPi.GPIO, **keywords)
- elif plat == Platform.BEAGLEBONE_BLACK:
- import Adafruit_BBIO.PWM
- return BBIO_PWM_Adapter(Adafruit_BBIO.PWM, **keywords)
- elif plat == Platform.UNKNOWN:
- raise RuntimeError('Could not determine platform.')
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/Platform.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/Platform.py
deleted file mode 100644
index 8ba4ca9744..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/Platform.py
+++ /dev/null
@@ -1,106 +0,0 @@
-# Copyright (c) 2014 Adafruit Industries
-# Author: Tony DiCola
-
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-# SOFTWARE.
-import platform
-import re
-
-# Platform identification constants.
-UNKNOWN = 0
-RASPBERRY_PI = 1
-BEAGLEBONE_BLACK = 2
-MINNOWBOARD = 3
-
-def platform_detect():
- """Detect if running on the Raspberry Pi or Beaglebone Black and return the
- platform type. Will return RASPBERRY_PI, BEAGLEBONE_BLACK, or UNKNOWN."""
- # Handle Raspberry Pi
- pi = pi_version()
- if pi is not None:
- return RASPBERRY_PI
-
- # Handle Beaglebone Black
- # TODO: Check the Beaglebone Black /proc/cpuinfo value instead of reading
- # the platform.
- plat = platform.platform()
- if plat.lower().find('armv7l-with-debian') > -1:
- return BEAGLEBONE_BLACK
- elif plat.lower().find('armv7l-with-ubuntu') > -1:
- return BEAGLEBONE_BLACK
- elif plat.lower().find('armv7l-with-glibc2.4') > -1:
- return BEAGLEBONE_BLACK
-
- # Handle Minnowboard
- # Assumption is that mraa is installed
- try:
- import mraa
- if mraa.getPlatformName()=='MinnowBoard MAX':
- return MINNOWBOARD
- except ImportError:
- pass
-
- # Couldn't figure out the platform, just return unknown.
- return UNKNOWN
-
-
-def pi_revision():
- """Detect the revision number of a Raspberry Pi, useful for changing
- functionality like default I2C bus based on revision."""
- # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
- with open('/proc/cpuinfo', 'r') as infile:
- for line in infile:
- # Match a line of the form "Revision : 0002" while ignoring extra
- # info in front of the revsion (like 1000 when the Pi was over-volted).
- match = re.match('Revision\s+:\s+.*(\w{4})$', line, flags=re.IGNORECASE)
- if match and match.group(1) in ['0000', '0002', '0003']:
- # Return revision 1 if revision ends with 0000, 0002 or 0003.
- return 1
- elif match:
- # Assume revision 2 if revision ends with any other 4 chars.
- return 2
- # Couldn't find the revision, throw an exception.
- raise RuntimeError('Could not determine Raspberry Pi revision.')
-
-
-def pi_version():
- """Detect the version of the Raspberry Pi. Returns either 1, 2 or
- None depending on if it's a Raspberry Pi 1 (model A, B, A+, B+),
- Raspberry Pi 2 (model B+), or not a Raspberry Pi.
- """
- # Check /proc/cpuinfo for the Hardware field value.
- # 2708 is pi 1
- # 2709 is pi 2
- # Anything else is not a pi.
- with open('/proc/cpuinfo', 'r') as infile:
- cpuinfo = infile.read()
- # Match a line like 'Hardware : BCM2709'
- match = re.search('^Hardware\s+:\s+(\w+)$', cpuinfo,
- flags=re.MULTILINE | re.IGNORECASE)
- if not match:
- # Couldn't find the hardware, assume it isn't a pi.
- return None
- if match.group(1) == 'BCM2708':
- # Pi 1
- return 1
- elif match.group(1) == 'BCM2709':
- # Pi 2
- return 2
- else:
- # Something else, not a pi.
- return None
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/SPI.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/SPI.py
deleted file mode 100644
index cfad79bd08..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/SPI.py
+++ /dev/null
@@ -1,336 +0,0 @@
-# Copyright (c) 2014 Adafruit Industries
-# Author: Tony DiCola
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-
-import operator
-import time
-
-import Adafruit_GPIO as GPIO
-
-
-MSBFIRST = 0
-LSBFIRST = 1
-
-
-class SpiDev(object):
- """Hardware-based SPI implementation using the spidev interface."""
-
- def __init__(self, port, device, max_speed_hz=500000):
- """Initialize an SPI device using the SPIdev interface. Port and device
- identify the device, for example the device /dev/spidev1.0 would be port
- 1 and device 0.
- """
- import spidev
- self._device = spidev.SpiDev()
- self._device.open(port, device)
- self._device.max_speed_hz=max_speed_hz
- # Default to mode 0.
- self._device.mode = 0
-
- def set_clock_hz(self, hz):
- """Set the speed of the SPI clock in hertz. Note that not all speeds
- are supported and a lower speed might be chosen by the hardware.
- """
- self._device.max_speed_hz=hz
-
- def set_mode(self, mode):
- """Set SPI mode which controls clock polarity and phase. Should be a
- numeric value 0, 1, 2, or 3. See wikipedia page for details on meaning:
- http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
- """
- if mode < 0 or mode > 3:
- raise ValueError('Mode must be a value 0, 1, 2, or 3.')
- self._device.mode = mode
-
- def set_bit_order(self, order):
- """Set order of bits to be read/written over serial lines. Should be
- either MSBFIRST for most-significant first, or LSBFIRST for
- least-signifcant first.
- """
- if order == MSBFIRST:
- self._device.lsbfirst = False
- elif order == LSBFIRST:
- self._device.lsbfirst = True
- else:
- raise ValueError('Order must be MSBFIRST or LSBFIRST.')
-
- def close(self):
- """Close communication with the SPI device."""
- self._device.close()
-
- def write(self, data):
- """Half-duplex SPI write. The specified array of bytes will be clocked
- out the MOSI line.
- """
- self._device.writebytes(data)
-
- def read(self, length):
- """Half-duplex SPI read. The specified length of bytes will be clocked
- in the MISO line and returned as a bytearray object.
- """
- return bytearray(self._device.readbytes(length))
-
- def transfer(self, data):
- """Full-duplex SPI read and write. The specified array of bytes will be
- clocked out the MOSI line, while simultaneously bytes will be read from
- the MISO line. Read bytes will be returned as a bytearray object.
- """
- return bytearray(self._device.xfer2(data))
-
-class SpiDevMraa(object):
- """Hardware SPI implementation with the mraa library on Minnowboard"""
- def __init__(self, port, device, max_speed_hz=500000):
- import mraa
- self._device = mraa.Spi(0)
- self._device.mode(0)
-
- def set_clock_hz(self, hz):
- """Set the speed of the SPI clock in hertz. Note that not all speeds
- are supported and a lower speed might be chosen by the hardware.
- """
- self._device.frequency(hz)
-
- def set_mode(self,mode):
- """Set SPI mode which controls clock polarity and phase. Should be a
- numeric value 0, 1, 2, or 3. See wikipedia page for details on meaning:
- http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
- """
- if mode < 0 or mode > 3:
- raise ValueError('Mode must be a value 0, 1, 2, or 3.')
- self._device.mode(mode)
-
- def set_mode(self,mode):
- """Set SPI mode which controls clock polarity and phase. Should be a
- numeric value 0, 1, 2, or 3. See wikipedia page for details on meaning:
- http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
- """
- if mode < 0 or mode > 3:
- raise ValueError('Mode must be a value 0, 1, 2, or 3.')
- self._device.mode(mode)
-
- def set_bit_order(self, order):
- """Set order of bits to be read/written over serial lines. Should be
- either MSBFIRST for most-significant first, or LSBFIRST for
- least-signifcant first.
- """
- if order == MSBFIRST:
- self._device.lsbmode(False)
- elif order == LSBFIRST:
- self._device.lsbmode(True)
- else:
- raise ValueError('Order must be MSBFIRST or LSBFIRST.')
-
- def close(self):
- """Close communication with the SPI device."""
- self._device.Spi()
-
- def write(self, data):
- """Half-duplex SPI write. The specified array of bytes will be clocked
- out the MOSI line.
- """
- self._device.write(bytearray(data))
-
-class BitBang(object):
- """Software-based implementation of the SPI protocol over GPIO pins."""
-
- def __init__(self, gpio, sclk, mosi=None, miso=None, ss=None):
- """Initialize bit bang (or software) based SPI. Must provide a BaseGPIO
- class, the SPI clock, and optionally MOSI, MISO, and SS (slave select)
- pin numbers. If MOSI is set to None then writes will be disabled and fail
- with an error, likewise for MISO reads will be disabled. If SS is set to
- None then SS will not be asserted high/low by the library when
- transfering data.
- """
- self._gpio = gpio
- self._sclk = sclk
- self._mosi = mosi
- self._miso = miso
- self._ss = ss
- # Set pins as outputs/inputs.
- gpio.setup(sclk, GPIO.OUT)
- if mosi is not None:
- gpio.setup(mosi, GPIO.OUT)
- if miso is not None:
- gpio.setup(miso, GPIO.IN)
- if ss is not None:
- gpio.setup(ss, GPIO.OUT)
- # Assert SS high to start with device communication off.
- gpio.set_high(ss)
- # Assume mode 0.
- self.set_mode(0)
- # Assume most significant bit first order.
- self.set_bit_order(MSBFIRST)
-
- def set_clock_hz(self, hz):
- """Set the speed of the SPI clock. This is unsupported with the bit
- bang SPI class and will be ignored.
- """
- pass
-
- def set_mode(self, mode):
- """Set SPI mode which controls clock polarity and phase. Should be a
- numeric value 0, 1, 2, or 3. See wikipedia page for details on meaning:
- http://en.wikipedia.org/wiki/Serial_Peripheral_Interface_Bus
- """
- if mode < 0 or mode > 3:
- raise ValueError('Mode must be a value 0, 1, 2, or 3.')
- if mode & 0x02:
- # Clock is normally high in mode 2 and 3.
- self._clock_base = GPIO.HIGH
- else:
- # Clock is normally low in mode 0 and 1.
- self._clock_base = GPIO.LOW
- if mode & 0x01:
- # Read on trailing edge in mode 1 and 3.
- self._read_leading = False
- else:
- # Read on leading edge in mode 0 and 2.
- self._read_leading = True
- # Put clock into its base state.
- self._gpio.output(self._sclk, self._clock_base)
-
- def set_bit_order(self, order):
- """Set order of bits to be read/written over serial lines. Should be
- either MSBFIRST for most-significant first, or LSBFIRST for
- least-signifcant first.
- """
- # Set self._mask to the bitmask which points at the appropriate bit to
- # read or write, and appropriate left/right shift operator function for
- # reading/writing.
- if order == MSBFIRST:
- self._mask = 0x80
- self._write_shift = operator.lshift
- self._read_shift = operator.rshift
- elif order == LSBFIRST:
- self._mask = 0x01
- self._write_shift = operator.rshift
- self._read_shift = operator.lshift
- else:
- raise ValueError('Order must be MSBFIRST or LSBFIRST.')
-
- def close(self):
- """Close the SPI connection. Unused in the bit bang implementation."""
- pass
-
- def write(self, data, assert_ss=True, deassert_ss=True):
- """Half-duplex SPI write. If assert_ss is True, the SS line will be
- asserted low, the specified bytes will be clocked out the MOSI line, and
- if deassert_ss is True the SS line be put back high.
- """
- # Fail MOSI is not specified.
- if self._mosi is None:
- raise RuntimeError('Write attempted with no MOSI pin specified.')
- if assert_ss and self._ss is not None:
- self._gpio.set_low(self._ss)
- for byte in data:
- for i in range(8):
- # Write bit to MOSI.
- if self._write_shift(byte, i) & self._mask:
- self._gpio.set_high(self._mosi)
- else:
- self._gpio.set_low(self._mosi)
- # Flip clock off base.
- self._gpio.output(self._sclk, not self._clock_base)
- # Return clock to base.
- self._gpio.output(self._sclk, self._clock_base)
- if deassert_ss and self._ss is not None:
- self._gpio.set_high(self._ss)
-
- def read(self, length, assert_ss=True, deassert_ss=True):
- """Half-duplex SPI read. If assert_ss is true, the SS line will be
- asserted low, the specified length of bytes will be clocked in the MISO
- line, and if deassert_ss is true the SS line will be put back high.
- Bytes which are read will be returned as a bytearray object.
- """
- if self._miso is None:
- raise RuntimeError('Read attempted with no MISO pin specified.')
- if assert_ss and self._ss is not None:
- self._gpio.set_low(self._ss)
- result = bytearray(length)
- for i in range(length):
- for j in range(8):
- # Flip clock off base.
- self._gpio.output(self._sclk, not self._clock_base)
- # Handle read on leading edge of clock.
- if self._read_leading:
- if self._gpio.is_high(self._miso):
- # Set bit to 1 at appropriate location.
- result[i] |= self._read_shift(self._mask, j)
- else:
- # Set bit to 0 at appropriate location.
- result[i] &= ~self._read_shift(self._mask, j)
- # Return clock to base.
- self._gpio.output(self._sclk, self._clock_base)
- # Handle read on trailing edge of clock.
- if not self._read_leading:
- if self._gpio.is_high(self._miso):
- # Set bit to 1 at appropriate location.
- result[i] |= self._read_shift(self._mask, j)
- else:
- # Set bit to 0 at appropriate location.
- result[i] &= ~self._read_shift(self._mask, j)
- if deassert_ss and self._ss is not None:
- self._gpio.set_high(self._ss)
- return result
-
- def transfer(self, data, assert_ss=True, deassert_ss=True):
- """Full-duplex SPI read and write. If assert_ss is true, the SS line
- will be asserted low, the specified bytes will be clocked out the MOSI
- line while bytes will also be read from the MISO line, and if
- deassert_ss is true the SS line will be put back high. Bytes which are
- read will be returned as a bytearray object.
- """
- if self._mosi is None:
- raise RuntimeError('Write attempted with no MOSI pin specified.')
- if self._mosi is None:
- raise RuntimeError('Read attempted with no MISO pin specified.')
- if assert_ss and self._ss is not None:
- self._gpio.set_low(self._ss)
- result = bytearray(len(data))
- for i in range(len(data)):
- for j in range(8):
- # Write bit to MOSI.
- if self._write_shift(data[i], j) & self._mask:
- self._gpio.set_high(self._mosi)
- else:
- self._gpio.set_low(self._mosi)
- # Flip clock off base.
- self._gpio.output(self._sclk, not self._clock_base)
- # Handle read on leading edge of clock.
- if self._read_leading:
- if self._gpio.is_high(self._miso):
- # Set bit to 1 at appropriate location.
- result[i] |= self._read_shift(self._mask, j)
- else:
- # Set bit to 0 at appropriate location.
- result[i] &= ~self._read_shift(self._mask, j)
- # Return clock to base.
- self._gpio.output(self._sclk, self._clock_base)
- # Handle read on trailing edge of clock.
- if not self._read_leading:
- if self._gpio.is_high(self._miso):
- # Set bit to 1 at appropriate location.
- result[i] |= self._read_shift(self._mask, j)
- else:
- # Set bit to 0 at appropriate location.
- result[i] &= ~self._read_shift(self._mask, j)
- if deassert_ss and self._ss is not None:
- self._gpio.set_high(self._ss)
- return result
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/__init__.py
deleted file mode 100644
index 7e99604838..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_GPIO/__init__.py
+++ /dev/null
@@ -1,3 +0,0 @@
-from __future__ import absolute_import
-
-from Adafruit_GPIO.GPIO import *
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_I2C/Adafruit_I2C.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_I2C/Adafruit_I2C.py
deleted file mode 100755
index 9b9a292ec4..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_I2C/Adafruit_I2C.py
+++ /dev/null
@@ -1,161 +0,0 @@
-#!/usr/bin/python
-import re
-import smbus
-
-# ===========================================================================
-# Adafruit_I2C Class
-# ===========================================================================
-
-class Adafruit_I2C(object):
-
- @staticmethod
- def getPiRevision():
- "Gets the version number of the Raspberry Pi board"
- # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
- try:
- with open('/proc/cpuinfo', 'r') as infile:
- for line in infile:
- # Match a line of the form "Revision : 0002" while ignoring extra
- # info in front of the revsion (like 1000 when the Pi was over-volted).
- match = re.match('Revision\s+:\s+.*(\w{4})$', line)
- if match and match.group(1) in ['0000', '0002', '0003']:
- # Return revision 1 if revision ends with 0000, 0002 or 0003.
- return 1
- elif match:
- # Assume revision 2 if revision ends with any other 4 chars.
- return 2
- # Couldn't find the revision, assume revision 0 like older code for compatibility.
- return 0
- except:
- return 0
-
- @staticmethod
- def getPiI2CBusNumber():
- # Gets the I2C bus number /dev/i2c#
- return 1 if Adafruit_I2C.getPiRevision() > 1 else 0
-
- def __init__(self, address, busnum=-1, debug=False):
- self.address = address
- # By default, the correct I2C bus is auto-detected using /proc/cpuinfo
- # Alternatively, you can hard-code the bus version below:
- # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's)
- #self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's)
- self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber())
- self.debug = debug
-
- def reverseByteOrder(self, data):
- "Reverses the byte order of an int (16-bit) or long (32-bit) value"
- # Courtesy Vishal Sapre
- byteCount = len(hex(data)[2:].replace('L','')[::2])
- val = 0
- for i in range(byteCount):
- val = (val << 8) | (data & 0xff)
- data >>= 8
- return val
-
- def errMsg(self):
- #OMFG stop this error print "Error accessing 0x%02X: Check your I2C address" % self.address
- return -1
-
- def write8(self, reg, value):
- "Writes an 8-bit value to the specified register/address"
- try:
- self.bus.write_byte_data(self.address, reg, value)
- if self.debug:
- print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg)
- except IOError, err:
- return self.errMsg()
-
- def write16(self, reg, value):
- "Writes a 16-bit value to the specified register/address pair"
- try:
- self.bus.write_word_data(self.address, reg, value)
- if self.debug:
- print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" %
- (value, reg, reg+1))
- except IOError, err:
- return self.errMsg()
-
- def writeRaw8(self, value):
- "Writes an 8-bit value on the bus"
- try:
- self.bus.write_byte(self.address, value)
- if self.debug:
- print "I2C: Wrote 0x%02X" % value
- except IOError, err:
- return self.errMsg()
-
- def writeList(self, reg, list):
- "Writes an array of bytes using I2C format"
- try:
- if self.debug:
- print "I2C: Writing list to register 0x%02X:" % reg
- print list
- self.bus.write_i2c_block_data(self.address, reg, list)
- except IOError, err:
- return self.errMsg()
-
- def readList(self, reg, length):
- "Read a list of bytes from the I2C device"
- try:
- results = self.bus.read_i2c_block_data(self.address, reg, length)
- if self.debug:
- print ("I2C: Device 0x%02X returned the following from reg 0x%02X" %
- (self.address, reg))
- print results
- return results
- except IOError, err:
- return self.errMsg()
-
- def readU8(self, reg):
- "Read an unsigned byte from the I2C device"
- try:
- result = self.bus.read_byte_data(self.address, reg)
- if self.debug:
- print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
- (self.address, result & 0xFF, reg))
- return result
- except IOError, err:
- return self.errMsg()
-
- def readS8(self, reg):
- "Reads a signed byte from the I2C device"
- try:
- result = self.bus.read_byte_data(self.address, reg)
- if result > 127: result -= 256
- if self.debug:
- print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
- (self.address, result & 0xFF, reg))
- return result
- except IOError, err:
- return self.errMsg()
-
- def readU16(self, reg, little_endian=True):
- "Reads an unsigned 16-bit value from the I2C device"
- try:
- result = self.bus.read_word_data(self.address,reg)
- # Swap bytes if using big endian because read_word_data assumes little
- # endian on ARM (little endian) systems.
- if not little_endian:
- result = ((result << 8) & 0xFF00) + (result >> 8)
- if (self.debug):
- print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg)
- return result
- except IOError, err:
- return self.errMsg()
-
- def readS16(self, reg, little_endian=True):
- "Reads a signed 16-bit value from the I2C device"
- try:
- result = self.readU16(reg,little_endian)
- if result > 32767: result -= 65536
- return result
- except IOError, err:
- return self.errMsg()
-
-if __name__ == '__main__':
- try:
- bus = Adafruit_I2C(address=0)
- print "Default I2C bus is accessible"
- except:
- print "Error accessing default I2C bus"
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_I2C/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_I2C/__init__.py
deleted file mode 100644
index 8f0781ee7e..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_I2C/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from .Adafruit_I2C import Adafruit_I2C
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_LSM303/Adafruit_LSM303.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_LSM303/Adafruit_LSM303.py
deleted file mode 100755
index 1734530e59..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_LSM303/Adafruit_LSM303.py
+++ /dev/null
@@ -1,207 +0,0 @@
-#!/usr/bin/python
-
-# Python library for Adafruit Flora Accelerometer/Compass Sensor (LSM303).
-# This is pretty much a direct port of the current Arduino library and is
-# similarly incomplete (e.g. no orientation value returned from read()
-# method). This does add optional high resolution mode to accelerometer
-# though.
-
-# Copyright 2013 Adafruit Industries
-
-# Permission is hereby granted, free of charge, to any person obtaining a
-# copy of this software and associated documentation files (the "Software"),
-# to deal in the Software without restriction, including without limitation
-# the rights to use, copy, modify, merge, publish, distribute, sublicense,
-# and/or sell copies of the Software, and to permit persons to whom the
-# Software is furnished to do so, subject to the following conditions:
-
-# The above copyright notice and this permission notice shall be included
-# in all copies or substantial portions of the Software.
-
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
-# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
-# DEALINGS IN THE SOFTWARE.
-
-from Adafruit_I2C import Adafruit_I2C
-
-
-class Adafruit_LSM303(Adafruit_I2C):
-
- # Minimal constants carried over from Arduino library
- LSM303_ADDRESS_ACCEL = (0x32 >> 1) # 0011001x
- LSM303_ADDRESS_MAG = (0x3C >> 1) # 0011110x
- # Default Type
- LSM303_REGISTER_ACCEL_CTRL_REG1_A = 0x20 # 00000111 rw
- LSM303_REGISTER_ACCEL_CTRL_REG4_A = 0x23 # 00000000 rw
- LSM303_REGISTER_ACCEL_OUT_X_L_A = 0x28
- LSM303_REGISTER_MAG_CRB_REG_M = 0x01
- LSM303_REGISTER_MAG_MR_REG_M = 0x02
- LSM303_REGISTER_MAG_OUT_X_H_M = 0x03
-
- # Gain settings for setMagGain()
- LSM303_MAG_GAIN_1_3 = 0x20 # +/- 1.3
- LSM303_MAG_GAIN_1_9 = 0x40 # +/- 1.9
- LSM303_MAG_GAIN_2_5 = 0x60 # +/- 2.5
- LSM303_MAG_GAIN_4_0 = 0x80 # +/- 4.0
- LSM303_MAG_GAIN_4_7 = 0xA0 # +/- 4.7
- LSM303_MAG_GAIN_5_6 = 0xC0 # +/- 5.6
- LSM303_MAG_GAIN_8_1 = 0xE0 # +/- 8.1
-
- # Sensitivity settings for getAccelSens()
- LSM303_MAG_XY_SENSITIVITY_1_3 = 0.000909091 # 1100 LSB/Gauss
- LSM303_MAG_XY_SENSITIVITY_1_9 = 0.001169591 # 855 LSB/Gauss
- LSM303_MAG_XY_SENSITIVITY_2_5 = 0.001492537 # 670 LSB/Gauss
- LSM303_MAG_XY_SENSITIVITY_4_0 = 0.002222222 # 450 LSB/Gauss
- LSM303_MAG_XY_SENSITIVITY_4_7 = 0.002500000 # 400 LSB/Gauss
- LSM303_MAG_XY_SENSITIVITY_5_6 = 0.003030303 # 330 LSB/Gauss
- LSM303_MAG_XY_SENSITIVITY_8_1 = 0.004347826 # 230 LSB/Gauss
- # Z gain is different from X and Y gain
- LSM303_MAG_Z_SENSITIVITY_1_3 = 0.001020408 # 980 LSB/Gauss
- LSM303_MAG_Z_SENSITIVITY_1_9 = 0.001315789 # 760 LSB/Gauss
- LSM303_MAG_Z_SENSITIVITY_2_5 = 0.001666667 # 600 LSB/Gauss
- LSM303_MAG_Z_SENSITIVITY_4_0 = 0.002500000 # 400 LSB/Gauss
- LSM303_MAG_Z_SENSITIVITY_4_7 = 0.002816901 # 355 LSB/Gauss
- LSM303_MAG_Z_SENSITIVITY_5_6 = 0.003389831 # 295 LSB/Gauss
- LSM303_MAG_Z_SENSITIVITY_8_1 = 0.004878049 # 205 LSB/Gauss
-
- # Gain settings setAccelGain()
- LSM303_ACCEL_GAIN_2 = 0x00 # +/- 2 g
- LSM303_ACCEL_GAIN_4 = 0x10 # +/- 4 g
- LSM303_ACCEL_GAIN_8 = 0x20 # +/- 8 g
- LSM303_ACCEL_GAIN_16 = 0x30 # +/- 16 g
-
- # Sensitivity settings for getAccelSens()
- LSM303_ACCEL_SENSITIVITY_2 = 0.001 # 1 mg/LSB
- LSM303_ACCEL_SENSITIVITY_4 = 0.002 # 2 mg/LSB
- LSM303_ACCEL_SENSITIVITY_8 = 0.004 # 4 mg/LSB
- LSM303_ACCEL_SENSITIVITY_16 = 0.012 # 12 mg/LSB As in the datasheet, but why?
-
- def __init__(self, accel_gain=LSM303_ACCEL_GAIN_2,
- mag_gain=LSM303_MAG_GAIN_1_3,
- busnum=-1, debug=False, hires=False):
-
- # Accelerometer and magnetometer are at different I2C
- # addresses, so invoke a separate I2C instance for each
- self.accel = Adafruit_I2C(self.LSM303_ADDRESS_ACCEL, busnum, debug)
- self.mag = Adafruit_I2C(self.LSM303_ADDRESS_MAG , busnum, debug)
-
- # Enable the accelerometer
- self.accel.write8(self.LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x27)
- # Select hi-res (12-bit) or low-res (10-bit) output mode.
- # Low-res mode uses less power and sustains a higher update rate,
- # output is padded to compatible 12-bit units.
- self.hires = hires
-
- # Enable the magnetometer
- self.mag.write8(self.LSM303_REGISTER_MAG_MR_REG_M, 0x00)
-
- self.setAccelGain(accel_gain)
-
- self.setMagGain(mag_gain)
-
-
- # Interpret signed 12-bit acceleration component from list
- def accel12(self, list, idx):
- n = list[idx] | (list[idx+1] << 8) # Low, high bytes
- if n > 32767: n -= 65536 # 2's complement signed
- return n >> 4 # 12-bit resolution
-
-
- # Interpret signed 16-bit magnetometer component from list
- def mag16(self, list, idx):
- n = (list[idx] << 8) | list[idx+1] # High, low bytes
- return n if n < 32768 else n - 65536 # 2's complement signed
-
- def getAccelSens(self, value_int):
- value_f = 0.0
- if self.accel_gain == self.LSM303_ACCEL_GAIN_2:
- value_f = value_int * self.LSM303_ACCEL_SENSITIVITY_2
- elif self.accel_gain == self.LSM303_ACCEL_GAIN_4:
- value_f = value_int * self.LSM303_ACCEL_SENSITIVITY_4
- elif self.accel_gain == self.LSM303_ACCEL_GAIN_8:
- value_f = value_int * self.LSM303_ACCEL_SENSITIVITY_8
- elif self.accel_gain == self.LSM303_ACCEL_GAIN_16:
- value_f = value_int * self.LSM303_ACCEL_SENSITIVITY_16
- return value_f
-
- def getMagXYSens(self, value_int):
- value_f = 0.0
- if self.mag_gain == self.LSM303_MAG_GAIN_1_3:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_1_3
- elif self.mag_gain == self.LSM303_MAG_GAIN_1_9:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_1_9
- elif self.mag_gain == self.LSM303_MAG_GAIN_2_5:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_2_5
- elif self.mag_gain == self.LSM303_MAG_GAIN_4_0:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_4_0
- elif self.mag_gain == self.LSM303_MAG_GAIN_4_7:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_4_7
- elif self.mag_gain == self.LSM303_MAG_GAIN_5_6:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_5_6
- elif self.mag_gain == self.LSM303_MAG_GAIN_8_1:
- value_f = value_int * self.LSM303_MAG_XY_SENSITIVITY_8_1
- return value_f
-
- def getMagZSens(self, value_int):
- value_f = 0.0
- if self.mag_gain == self.LSM303_MAG_GAIN_1_3:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_1_3
- elif self.mag_gain == self.LSM303_MAG_GAIN_1_9:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_1_9
- elif self.mag_gain == self.LSM303_MAG_GAIN_2_5:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_2_5
- elif self.mag_gain == self.LSM303_MAG_GAIN_4_0:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_4_0
- elif self.mag_gain == self.LSM303_MAG_GAIN_4_7:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_4_7
- elif self.mag_gain == self.LSM303_MAG_GAIN_5_6:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_5_6
- elif self.mag_gain == self.LSM303_MAG_GAIN_8_1:
- value_f = value_int * self.LSM303_MAG_Z_SENSITIVITY_8_1
- return value_f
-
- def read(self):
- # Read the accelerometer
- list = self.accel.readList(
- self.LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80, 6)
- res = [( self.getAccelSens(self.accel12(list, 0)),
- self.getAccelSens(self.accel12(list, 2)),
- self.getAccelSens(self.accel12(list, 4)) )]
-
- # Read the magnetometer
- list = self.mag.readList(self.LSM303_REGISTER_MAG_OUT_X_H_M, 6)
- res.append((self.getMagXYSens(self.mag16(list, 0)),
- self.getMagXYSens(self.mag16(list, 2)),
- self.getMagZSens(self.mag16(list, 4)) ))
- # ToDo: Calculate orientation
-
- return res
-
-
- def setAccelGain(self, accel_gain):
- reg = accel_gain
- if self.hires:
- reg |= 0b00001000
- self.accel.write8(self.LSM303_REGISTER_ACCEL_CTRL_REG4_A, reg)
- self.accel_gain = accel_gain
-
- def setMagGain(self, mag_gain):
- self.mag.write8(self.LSM303_REGISTER_MAG_CRB_REG_M, mag_gain)
- self.mag_gain = mag_gain
-
-
-# Simple example prints accel/mag data once per second:
-if __name__ == '__main__':
-
- from time import sleep
-
- lsm = Adafruit_LSM303()
-
- print '[(Accelerometer X, Y, Z), (Magnetometer X, Y, Z, orientation)]'
- while True:
- print lsm.read()
- sleep(1) # Output is fun to watch if this is commented out
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_LSM303/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_LSM303/__init__.py
deleted file mode 100644
index 9c75e8fc4b..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_LSM303/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from .Adafruit_LSM303 import Adafruit_LSM303
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_MotorHAT/Adafruit_MotorHAT.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_MotorHAT/Adafruit_MotorHAT.py
deleted file mode 100755
index 2fd603bc90..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_MotorHAT/Adafruit_MotorHAT.py
+++ /dev/null
@@ -1,252 +0,0 @@
-#!/usr/bin/python
-
-from Adafruit_PWM_Servo_Driver import PWM
-import time
-
-class Adafruit_StepperMotor:
- MICROSTEPS = 8
- MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255]
-
- #MICROSTEPS = 16
- # a sinusoidal curve NOT LINEAR!
- #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255]
-
- def __init__(self, controller, num, steps=200):
- self.MC = controller
- self.revsteps = steps
- self.motornum = num
- self.sec_per_step = 0.1
- self.steppingcounter = 0
- self.currentstep = 0
-
- num -= 1
-
- if (num == 0):
- self.PWMA = 8
- self.AIN2 = 9
- self.AIN1 = 10
- self.PWMB = 13
- self.BIN2 = 12
- self.BIN1 = 11
- elif (num == 1):
- self.PWMA = 2
- self.AIN2 = 3
- self.AIN1 = 4
- self.PWMB = 7
- self.BIN2 = 6
- self.BIN1 = 5
- else:
- raise NameError('MotorHAT Stepper must be between 1 and 2 inclusive')
-
- def setSpeed(self, rpm):
- self.sec_per_step = 60.0 / (self.revsteps * rpm)
- self.steppingcounter = 0
-
- def oneStep(self, dir, style):
- pwm_a = pwm_b = 255
-
- # first determine what sort of stepping procedure we're up to
- if (style == Adafruit_MotorHAT.SINGLE):
- if ((self.currentstep/(self.MICROSTEPS/2)) % 2):
- # we're at an odd step, weird
- if (dir == Adafruit_MotorHAT.FORWARD):
- self.currentstep += self.MICROSTEPS/2
- else:
- self.currentstep -= self.MICROSTEPS/2
- else:
- # go to next even step
- if (dir == Adafruit_MotorHAT.FORWARD):
- self.currentstep += self.MICROSTEPS
- else:
- self.currentstep -= self.MICROSTEPS
- if (style == Adafruit_MotorHAT.DOUBLE):
- if not (self.currentstep/(self.MICROSTEPS/2) % 2):
- # we're at an even step, weird
- if (dir == Adafruit_MotorHAT.FORWARD):
- self.currentstep += self.MICROSTEPS/2
- else:
- self.currentstep -= self.MICROSTEPS/2
- else:
- # go to next odd step
- if (dir == Adafruit_MotorHAT.FORWARD):
- self.currentstep += self.MICROSTEPS
- else:
- self.currentstep -= self.MICROSTEPS
- if (style == Adafruit_MotorHAT.INTERLEAVE):
- if (dir == Adafruit_MotorHAT.FORWARD):
- self.currentstep += self.MICROSTEPS/2
- else:
- self.currentstep -= self.MICROSTEPS/2
-
- if (style == Adafruit_MotorHAT.MICROSTEP):
- if (dir == Adafruit_MotorHAT.FORWARD):
- self.currentstep += 1
- else:
- self.currentstep -= 1
-
- # go to next 'step' and wrap around
- self.currentstep += self.MICROSTEPS * 4
- self.currentstep %= self.MICROSTEPS * 4
-
- pwm_a = pwm_b = 0
- if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
- pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS - self.currentstep]
- pwm_b = self.MICROSTEP_CURVE[self.currentstep]
- elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
- pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS]
- pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*2 - self.currentstep]
- elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
- pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS*3 - self.currentstep]
- pwm_b = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*2]
- elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
- pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*3]
- pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*4 - self.currentstep]
-
-
- # go to next 'step' and wrap around
- self.currentstep += self.MICROSTEPS * 4
- self.currentstep %= self.MICROSTEPS * 4
-
- # only really used for microstepping, otherwise always on!
- self.MC._pwm.setPWM(self.PWMA, 0, pwm_a*16)
- self.MC._pwm.setPWM(self.PWMB, 0, pwm_b*16)
-
- # set up coil energizing!
- coils = [0, 0, 0, 0]
-
- if (style == Adafruit_MotorHAT.MICROSTEP):
- if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
- coils = [1, 1, 0, 0]
- elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
- coils = [0, 1, 1, 0]
- elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
- coils = [0, 0, 1, 1]
- elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
- coils = [1, 0, 0, 1]
- else:
- step2coils = [ [1, 0, 0, 0],
- [1, 1, 0, 0],
- [0, 1, 0, 0],
- [0, 1, 1, 0],
- [0, 0, 1, 0],
- [0, 0, 1, 1],
- [0, 0, 0, 1],
- [1, 0, 0, 1] ]
- coils = step2coils[self.currentstep/(self.MICROSTEPS/2)]
-
- #print "coils state = " + str(coils)
- self.MC.setPin(self.AIN2, coils[0])
- self.MC.setPin(self.BIN1, coils[1])
- self.MC.setPin(self.AIN1, coils[2])
- self.MC.setPin(self.BIN2, coils[3])
-
- return self.currentstep
-
- def step(self, steps, direction, stepstyle):
- s_per_s = self.sec_per_step
- lateststep = 0
-
- if (stepstyle == Adafruit_MotorHAT.INTERLEAVE):
- s_per_s = s_per_s / 2.0
- if (stepstyle == Adafruit_MotorHAT.MICROSTEP):
- s_per_s /= self.MICROSTEPS
- steps *= self.MICROSTEPS
-
- print s_per_s, " sec per step"
-
- for s in range(steps):
- lateststep = self.oneStep(direction, stepstyle)
- time.sleep(s_per_s)
-
- if (stepstyle == Adafruit_MotorHAT.MICROSTEP):
- # this is an edge case, if we are in between full steps, lets just keep going
- # so we end on a full step
- while (lateststep != 0) and (lateststep != self.MICROSTEPS):
- lateststep = self.oneStep(dir, stepstyle)
- time.sleep(s_per_s)
-
-class Adafruit_DCMotor:
- def __init__(self, controller, num):
- self.MC = controller
- self.motornum = num
- pwm = in1 = in2 = 0
-
- if (num == 0):
- pwm = 8
- in2 = 9
- in1 = 10
- elif (num == 1):
- pwm = 13
- in2 = 12
- in1 = 11
- elif (num == 2):
- pwm = 2
- in2 = 3
- in1 = 4
- elif (num == 3):
- pwm = 7
- in2 = 6
- in1 = 5
- else:
- raise NameError('MotorHAT Motor must be between 1 and 4 inclusive')
- self.PWMpin = pwm
- self.IN1pin = in1
- self.IN2pin = in2
-
- def run(self, command):
- if not self.MC:
- return
- if (command == Adafruit_MotorHAT.FORWARD):
- self.MC.setPin(self.IN2pin, 0)
- self.MC.setPin(self.IN1pin, 1)
- if (command == Adafruit_MotorHAT.BACKWARD):
- self.MC.setPin(self.IN1pin, 0)
- self.MC.setPin(self.IN2pin, 1)
- if (command == Adafruit_MotorHAT.RELEASE):
- self.MC.setPin(self.IN1pin, 0)
- self.MC.setPin(self.IN2pin, 0)
- def setSpeed(self, speed):
- if (speed < 0):
- speed = 0
- if (speed > 255):
- speed = 255
- self.MC._pwm.setPWM(self.PWMpin, 0, speed*16)
-
-class Adafruit_MotorHAT:
- FORWARD = 1
- BACKWARD = 2
- BRAKE = 3
- RELEASE = 4
-
- SINGLE = 1
- DOUBLE = 2
- INTERLEAVE = 3
- MICROSTEP = 4
-
- def __init__(self, addr = 0x60, freq = 1600):
- self._i2caddr = addr # default addr on HAT
- self._frequency = freq # default @1600Hz PWM freq
- self.motors = [ Adafruit_DCMotor(self, m) for m in range(4) ]
- self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
- self._pwm = PWM(addr, debug=False)
- self._pwm.setPWMFreq(self._frequency)
-
- def setPin(self, pin, value):
- if (pin < 0) or (pin > 15):
- raise NameError('PWM pin must be between 0 and 15 inclusive')
- if (value != 0) and (value != 1):
- raise NameError('Pin value must be 0 or 1!')
- if (value == 0):
- self._pwm.setPWM(pin, 0, 4096)
- if (value == 1):
- self._pwm.setPWM(pin, 4096, 0)
-
- def getStepper(self, steps, num):
- if (num < 1) or (num > 2):
- raise NameError('MotorHAT Stepper must be between 1 and 2 inclusive')
- return self.steppers[num-1]
-
- def getMotor(self, num):
- if (num < 1) or (num > 4):
- raise NameError('MotorHAT Motor must be between 1 and 4 inclusive')
- return self.motors[num-1]
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_MotorHAT/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_MotorHAT/__init__.py
deleted file mode 100644
index 131ab55442..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_MotorHAT/__init__.py
+++ /dev/null
@@ -1,2 +0,0 @@
-from .Adafruit_MotorHAT import Adafruit_StepperMotor, Adafruit_DCMotor, Adafruit_MotorHAT
-
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_PWM_Servo_Driver/Adafruit_PWM_Servo_Driver.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_PWM_Servo_Driver/Adafruit_PWM_Servo_Driver.py
deleted file mode 100644
index 35c993ca41..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_PWM_Servo_Driver/Adafruit_PWM_Servo_Driver.py
+++ /dev/null
@@ -1,92 +0,0 @@
-#!/usr/bin/python
-
-import time
-import math
-from Adafruit_I2C import Adafruit_I2C
-
-# ============================================================================
-# Adafruit PCA9685 16-Channel PWM Servo Driver
-# ============================================================================
-
-class PWM :
- # Registers/etc.
- __MODE1 = 0x00
- __MODE2 = 0x01
- __SUBADR1 = 0x02
- __SUBADR2 = 0x03
- __SUBADR3 = 0x04
- __PRESCALE = 0xFE
- __LED0_ON_L = 0x06
- __LED0_ON_H = 0x07
- __LED0_OFF_L = 0x08
- __LED0_OFF_H = 0x09
- __ALL_LED_ON_L = 0xFA
- __ALL_LED_ON_H = 0xFB
- __ALL_LED_OFF_L = 0xFC
- __ALL_LED_OFF_H = 0xFD
-
- # Bits
- __RESTART = 0x80
- __SLEEP = 0x10
- __ALLCALL = 0x01
- __INVRT = 0x10
- __OUTDRV = 0x04
-
- general_call_i2c = Adafruit_I2C(0x00)
-
- @classmethod
- def softwareReset(cls):
- "Sends a software reset (SWRST) command to all the servo drivers on the bus"
- cls.general_call_i2c.writeRaw8(0x06) # SWRST
-
- def __init__(self, address=0x40, debug=False):
- self.i2c = Adafruit_I2C(address)
- self.i2c.debug = debug
- self.address = address
- self.debug = debug
- if (self.debug):
- print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2"
- self.setAllPWM(0, 0)
- self.i2c.write8(self.__MODE2, self.__OUTDRV)
- self.i2c.write8(self.__MODE1, self.__ALLCALL)
- time.sleep(0.005) # wait for oscillator
-
- mode1 = self.i2c.readU8(self.__MODE1)
- mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep)
- self.i2c.write8(self.__MODE1, mode1)
- time.sleep(0.005) # wait for oscillator
-
- def setPWMFreq(self, freq):
- "Sets the PWM frequency"
- prescaleval = 25000000.0 # 25MHz
- prescaleval /= 4096.0 # 12-bit
- prescaleval /= float(freq)
- prescaleval -= 1.0
- if (self.debug):
- print "Setting PWM frequency to %d Hz" % freq
- print "Estimated pre-scale: %d" % prescaleval
- prescale = math.floor(prescaleval + 0.5)
- if (self.debug):
- print "Final pre-scale: %d" % prescale
-
- oldmode = self.i2c.readU8(self.__MODE1);
- newmode = (oldmode & 0x7F) | 0x10 # sleep
- self.i2c.write8(self.__MODE1, newmode) # go to sleep
- self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
- self.i2c.write8(self.__MODE1, oldmode)
- time.sleep(0.005)
- self.i2c.write8(self.__MODE1, oldmode | 0x80)
-
- def setPWM(self, channel, on, off):
- "Sets a single PWM channel"
- self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF)
- self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8)
- self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF)
- self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8)
-
- def setAllPWM(self, on, off):
- "Sets a all PWM channels"
- self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF)
- self.i2c.write8(self.__ALL_LED_ON_H, on >> 8)
- self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF)
- self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8)
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_PWM_Servo_Driver/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_PWM_Servo_Driver/__init__.py
deleted file mode 100644
index 55c3f5efc9..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Adafruit_PWM_Servo_Driver/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from .Adafruit_PWM_Servo_Driver import PWM
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Gyro_L3GD20/Gyro_L3GD20.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Gyro_L3GD20/Gyro_L3GD20.py
deleted file mode 100755
index 943abee344..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Gyro_L3GD20/Gyro_L3GD20.py
+++ /dev/null
@@ -1,132 +0,0 @@
-#!/usr/bin/python
-
-# Python interface driver for L3GD20 gyroscope, inspired by
-# Adafruit_LSM303 drivers.
-#
-# date: 12/11/2015
-#
-# author: Dmitry Yershov
-#
-
-from Adafruit_I2C import Adafruit_I2C
-
-
-class Gyro_L3GD20(Adafruit_I2C):
-
- # Minimal constants carried over from Arduino library
- L3GD20_ADDRESS = (0xD6 >> 1) # 1101011x
-
- # L3GD20 registers
- # Default Type
- L3GD20_REGISTER_WHO_AM_I = 0x0F # 11010100 r
- L3GD20_REGISTER_CTRL_REG1 = 0x20 # 00000111 rw
- L3GD20_REGISTER_CTRL_REG2 = 0x21 # 00000000 rw
- L3GD20_REGISTER_CTRL_REG3 = 0x22 # 00000000 rw
- L3GD20_REGISTER_CTRL_REG4 = 0x23 # 00000000 rw
- L3GD20_REGISTER_CTRL_REG5 = 0x24 # 00000000 rw
- L3GD20_REGISTER_REFERENCE = 0x25 # 00000000 r
- L3GD20_REGISTER_OUT_TEMP = 0x26 # r
- L3GD20_REGISTER_STATUS_REG = 0x27 # r
- L3GD20_REGISTER_OUT_X_L = 0x28 # r
- L3GD20_REGISTER_OUT_X_H = 0x29 # r
- L3GD20_REGISTER_OUT_Y_L = 0x2A # r
- L3GD20_REGISTER_OUT_Y_H = 0x2B # r
- L3GD20_REGISTER_OUT_Z_L = 0x2C # r
- L3GD20_REGISTER_OUT_Z_H = 0x2D # r
- L3GD20_REGISTER_FIFO_CTRL_REG = 0x2E # 00000000 rw
- L3GD20_REGISTER_FIFO_SRC_REG = 0x2F # r
- L3GD20_REGISTER_INT1_CFG = 0x30 # 00000000 rw
- L3GD20_REGISTER_INT1_SRC = 0x31 # r
- L3GD20_REGISTER_TSH_XH = 0x32 # 00000000 rw
- L3GD20_REGISTER_TSH_XL = 0x33 # 00000000 rw
- L3GD20_REGISTER_TSH_YH = 0x34 # 00000000 rw
- L3GD20_REGISTER_TSH_YL = 0x35 # 00000000 rw
- L3GD20_REGISTER_TSH_ZH = 0x36 # 00000000 rw
- L3GD20_REGISTER_TSH_ZL = 0x37 # 00000000 rw
- L3GD20_REGISTER_INT1_DURATION = 0x38 # 00000000 rw
-
-
- # Chipsed IDs
- L3GD20_ID = 0xD4
- L3GD20H_ID = 0xD7
-
- # Range settings setRange()
- L3GD20_RANGE_250DPS = 250
- L3GD20_RANGE_500DPS = 500
- L3GD20_RANGE_2000DPS = 2000
-
- # Range sensitivity getSens()
- L3GD20_SENSITIVITY_250DPS = 0.00875
- L3GD20_SENSITIVITY_500DPS = 0.0175
- L3GD20_SENSITIVITY_2000DPS = 0.070
-
- def __init__(self, gyro_range=250, busnum=-1, debug=False, hires=False):
-
- # Accelerometer and magnetometer are at different I2C
- # addresses, so invoke a separate I2C instance for each
- self.gyro = Adafruit_I2C(self.L3GD20_ADDRESS, busnum, debug)
-
- chipset_id = self.gyro.readU8(self.L3GD20_REGISTER_WHO_AM_I)
-
- if chipset_id != self.L3GD20_ID and chipset_id != self.L3GD20H_ID:
- raise Warning("L3GD20 error: wrong id %4x received at address %4x" % (chipset_id, self.L3GD20_ADDRESS))
-
- # Enable all three axis on the gyro
- self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG1, 0x00)
- self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG1, 0x0F)
-
- self.gyro_range = self.L3GD20_SENSITIVITY_250DPS
- self.setRange(gyro_range)
-
- # Interpret signed 16-bit angular rotation from list
- def gyro16(self, values_uint8, idx):
- value_int16 = values_uint8[idx] | (values_uint8[idx+1] << 8)
- if value_int16 > 32767: value_int16 -= 65536
- return value_int16
-
- def getSens(self, value_int16):
- value_f = 0.0
- if self.gyro_range == self.L3GD20_RANGE_250DPS:
- value_f = value_int16 * self.L3GD20_SENSITIVITY_250DPS
- elif self.gyro_range == self.L3GD20_RANGE_500DPS:
- value_f = value_int16 * self.L3GD20_SENSITIVITY_500DPS
- elif self.gyro_range == self.L3GD20_RANGE_2000DPS:
- value_f = value_int16 * self.L3GD20_SENSITIVITY_2000DPS
- return value_f
-
- def read(self):
- # Read the gyro
- values_uint8 = self.gyro.readList(self.L3GD20_REGISTER_OUT_X_L | 0x80, 6)
- # Convert readings
- result = [( self.getSens(self.gyro16(values_uint8, 0)),
- self.getSens(self.gyro16(values_uint8, 2)),
- self.getSens(self.gyro16(values_uint8, 4)) )]
- return result
-
-
- def setRange(self, gyro_range):
- if gyro_range == self.gyro_range:
- return;
- elif gyro_range == self.L3GD20_RANGE_250DPS:
- self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG4, 0x00)
- elif gyro_range == self.L3GD20_RANGE_500DPS:
- self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG4, 0x10)
- elif gyro_range == self.L3GD20_RANGE_2000DPS:
- self.gyro.write8(self.L3GD20_REGISTER_CTRL_REG4, 0x20)
- else:
- raise ValueError("L3GD20 range value can only be 250, 500, or 2000")
-
- self.gyro_range = gyro_range;
-
-
-# Simple example prints accel/mag data once per second:
-if __name__ == '__main__':
-
- from time import sleep
-
- gyro = Gyro_L3GD20()
-
- print '[(Gyro w_X, w_Y, w_Z)]'
- while True:
- print gyro.read()
- sleep(1) # Output is fun to watch if this is commented out
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Gyro_L3GD20/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Gyro_L3GD20/__init__.py
deleted file mode 100644
index 158a24054f..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Gyro_L3GD20/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from .Gyro_L3GD20 import Gyro_L3GD20
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Sonar_MB1242/Sonar_MB1242.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Sonar_MB1242/Sonar_MB1242.py
deleted file mode 100755
index e3718ba584..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Sonar_MB1242/Sonar_MB1242.py
+++ /dev/null
@@ -1,36 +0,0 @@
-#!/usr/bin/python
-
-# Python library for MB1242 sonar
-
-from Adafruit_I2C import Adafruit_I2C
-
-
-class Sonar_MB1242(Adafruit_I2C):
-
- # Minimal constants carried over from Arduino library
- MB1242_ADDRESS = 0x70 # 1110000x
- # Default Type
- MB1242_REGISTER_RANGE_OUT = 0x51 # 01010001 rw
-
-
- def __init__(self, busnum=-1, debug=False):
- # initiate sonar i2c interface
- self.sonar = Adafruit_I2C(self.MB1242_ADDRESS, busnum, debug)
-
- def read(self):
- # Read the accelerometer
- sonar_range = self.sonar.readList(self.MB1242_REGISTER_RANGE_OUT, 2)
- return sonar_range
-
-
-# Simple example prints accel/mag data once per second:
-if __name__ == '__main__':
-
- from time import sleep
-
- sonar = Sonar_MB1242()
-
- print 'Range'
- while True:
- print sonar.read()
- sleep(1) # Output is fun to watch if this is commented out
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/include/Sonar_MB1242/__init__.py b/catkin_ws/src/05-teleop/adafruit_drivers/include/Sonar_MB1242/__init__.py
deleted file mode 100644
index 92893a4cab..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/include/Sonar_MB1242/__init__.py
+++ /dev/null
@@ -1 +0,0 @@
-from .Sonar_MB1242 import Sonar_MB1242
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/package.xml b/catkin_ws/src/05-teleop/adafruit_drivers/package.xml
deleted file mode 100644
index 205e07fea5..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/package.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
- adafruit_drivers
- 1.0.0
- TODO: Add a description of package `adafruit_drivers` in `package.xml`.
-
- Dmitry Yershov
- Mack
-
- GPLv3
-
-
-
- catkin
- rospy
- rospy
-
-
diff --git a/catkin_ws/src/05-teleop/adafruit_drivers/setup.py b/catkin_ws/src/05-teleop/adafruit_drivers/setup.py
deleted file mode 100644
index 43ae8b0e77..0000000000
--- a/catkin_ws/src/05-teleop/adafruit_drivers/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
- packages=['Adafruit_ADS1x15', 'Adafruit_GPIO','Adafruit_I2C','Adafruit_LSM303','Adafruit_MotorHAT','Adafruit_PWM_Servo_Driver','Gyro_L3GD20'],
- package_dir={'': 'include'},
-)
-
-setup(**setup_args)
diff --git a/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/dagu_wheels_driver.py b/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/dagu_wheels_driver.py
deleted file mode 100644
index 692eabb3b7..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/dagu_wheels_driver.py
+++ /dev/null
@@ -1,131 +0,0 @@
-#!/usr/bin/python
-
-# Wrapping the Adafruit API to talk to DC motors with a simpler interface
-#
-# date: 11/17/2015
-#
-# authors: Valerio Varricchio
-# Luca Carlone
-# Dmitry Yershov
-# Shih-Yuan Liu
-
-from Adafruit_MotorHAT import Adafruit_MotorHAT
-from math import fabs, floor
-
-class DaguWheelsDriver:
- LEFT_MOTOR_MIN_PWM = 60 # Minimum speed for left motor
- LEFT_MOTOR_MAX_PWM = 255 # Maximum speed for left motor
- RIGHT_MOTOR_MIN_PWM = 60 # Minimum speed for right motor
- RIGHT_MOTOR_MAX_PWM = 255 # Maximum speed for right motor
- # AXEL_TO_RADIUS_RATIO = 1.0 # The axel length and turning radius ratio
- SPEED_TOLERANCE = 1.e-2 # speed tolerance level
-
- def __init__(self, verbose=False, debug=False, left_flip=False, right_flip=False):
- self.motorhat = Adafruit_MotorHAT(addr=0x60)
- self.leftMotor = self.motorhat.getMotor(1)
- self.rightMotor = self.motorhat.getMotor(2)
- self.verbose = verbose or debug
- self.debug = debug
-
- self.left_sgn = 1.0
- if left_flip:
- self.left_sgn = -1.0
-
- self.right_sgn = 1.0
- if right_flip:
- self.right_sgn = -1.0
-
- self.leftSpeed = 0.0
- self.rightSpeed = 0.0
- self.updatePWM()
-
- def PWMvalue(self, v, minPWM, maxPWM):
- pwm = 0
- if fabs(v) > self.SPEED_TOLERANCE:
- pwm = int(floor(fabs(v) * (maxPWM - minPWM) + minPWM))
- return min(pwm, maxPWM)
-
- def updatePWM(self):
- vl = self.leftSpeed*self.left_sgn
- vr = self.rightSpeed*self.right_sgn
-
- pwml = self.PWMvalue(vl, self.LEFT_MOTOR_MIN_PWM, self.LEFT_MOTOR_MAX_PWM)
- pwmr = self.PWMvalue(vr, self.RIGHT_MOTOR_MIN_PWM, self.RIGHT_MOTOR_MAX_PWM)
-
- if self.debug:
- print "v = %5.3f, u = %5.3f, vl = %5.3f, vr = %5.3f, pwml = %3d, pwmr = %3d" % (v, u, vl, vr, pwml, pwmr)
-
- if fabs(vl) < self.SPEED_TOLERANCE:
- leftMotorMode = Adafruit_MotorHAT.RELEASE
- pwml = 0
- elif vl > 0:
- leftMotorMode = Adafruit_MotorHAT.FORWARD
- elif vl < 0:
- leftMotorMode = Adafruit_MotorHAT.BACKWARD
-
- if fabs(vr) < self.SPEED_TOLERANCE:
- rightMotorMode = Adafruit_MotorHAT.RELEASE
- pwmr = 0
- elif vr > 0:
- rightMotorMode = Adafruit_MotorHAT.FORWARD
- elif vr < 0:
- rightMotorMode = Adafruit_MotorHAT.BACKWARD
-
- self.leftMotor.setSpeed(pwml)
- self.leftMotor.run(leftMotorMode)
- self.rightMotor.setSpeed(pwmr)
- self.rightMotor.run(rightMotorMode)
-
- def setWheelsSpeed(self, left, right):
- self.leftSpeed = left
- self.rightSpeed = right
- self.updatePWM()
-
- def __del__(self):
- self.leftMotor.run(Adafruit_MotorHAT.RELEASE)
- self.rightMotor.run(Adafruit_MotorHAT.RELEASE)
- del self.motorhat
-
-# Simple example to test motors
-if __name__ == '__main__':
- from time import sleep
-
- N = 10
- delay = 100. / 1000.
-
- dagu = DAGU_Differential_Drive()
-
- # turn left
- dagu.setSteerAngle(1.0)
- # accelerate forward
- for i in range(N):
- dagu.setSpeed((1.0 + i) / N)
- sleep(delay)
- # decelerate forward
- for i in range(N):
- dagu.setSpeed((-1.0 - i + N) / N)
- sleep(delay)
-
- # turn right
- dagu.setSteerAngle(-1.0)
- # accelerate backward
- for i in range(N):
- dagu.setSpeed(-(1.0 + i) / N)
- sleep(delay)
- # decelerate backward
- for i in range(N):
- dagu.setSpeed(-(-1.0 - i + N) / N)
- sleep(delay)
-
- # turn left
- dagu.setSteerAngle(1.0)
- # accelerate forward
- for i in range(N):
- dagu.setSpeed((1.0 + i) / N)
- sleep(delay)
- # decelerate forward
- for i in range(N):
- dagu.setSpeed((-1.0 - i + N) / N)
- sleep(delay)
-
- del dagu
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/forward_kinematics_node.launch b/catkin_ws/src/05-teleop/dagu_car/launch/forward_kinematics_node.launch
deleted file mode 100644
index 06d2d90470..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/launch/forward_kinematics_node.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/inverse_kinematics_node.launch b/catkin_ws/src/05-teleop/dagu_car/launch/inverse_kinematics_node.launch
deleted file mode 100644
index 562f5d7f01..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/launch/inverse_kinematics_node.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/velocity_to_pose_node.launch b/catkin_ws/src/05-teleop/dagu_car/launch/velocity_to_pose_node.launch
deleted file mode 100644
index a3c9ed32af..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/launch/velocity_to_pose_node.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/wheels_driver_node.launch b/catkin_ws/src/05-teleop/dagu_car/launch/wheels_driver_node.launch
deleted file mode 100644
index 6a061657b6..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/launch/wheels_driver_node.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/wheels_driver_node.easy_node.yaml b/catkin_ws/src/05-teleop/dagu_car/src/wheels_driver_node.easy_node.yaml
deleted file mode 100644
index 5ef00f4124..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/src/wheels_driver_node.easy_node.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-description:
-parameters: {}
-
-subscriptions: {}
-
-publishers: {}
-
-contracts: {}
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/wheels_driver_node.py b/catkin_ws/src/05-teleop/dagu_car/src/wheels_driver_node.py
deleted file mode 100755
index 1482c0bae6..0000000000
--- a/catkin_ws/src/05-teleop/dagu_car/src/wheels_driver_node.py
+++ /dev/null
@@ -1,128 +0,0 @@
-#!/usr/bin/env python
-import rospy
-from duckietown_msgs.msg import WheelsCmdStamped, BoolStamped
-from dagu_car.dagu_wheels_driver import DaguWheelsDriver
-import numpy as np
-
-class WheelsDriverNode(object):
- def __init__(self):
- self.node_name = rospy.get_name()
- rospy.loginfo("[%s] Initializing " %(self.node_name))
- self.estop=False
-
- # Parameters for maximal turning radius
- self.use_rad_lim = self.setupParam("~use_rad_lim", False)
- self.min_rad = self.setupParam("~min_rad", 0.08)
- self.wheel_distance = self.setupParam("~wheel_distance", 0.103)
-
-
- # Setup publishers
- self.driver = DaguWheelsDriver()
- #add publisher for wheels command wih execution time
- self.msg_wheels_cmd = WheelsCmdStamped()
- self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",WheelsCmdStamped, queue_size=1)
-
- # Setup subscribers
- self.control_constant = 1.0
- self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
- self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
- self.sub_rad_lim = rospy.Subscriber("~radius_limit", BoolStamped, self.cbRadLimit, queue_size=1)
-
- self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
-
-
- def setupParam(self,param_name,default_value):
- value = rospy.get_param(param_name,default_value)
- rospy.set_param(param_name,value) #Write to parameter server for transparancy
- rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
- return value
-
- def updateParams(self,event):
- self.use_rad_lim = rospy.get_param("~use_rad_lim")
- self.min_rad = rospy.get_param("~min_rad")
- self.wheel_distance = rospy.get_param("~wheel_distance")
-
- def cbWheelsCmd(self,msg):
- if self.estop:
- self.driver.setWheelsSpeed(left=0.0,right=0.0)
- return
-
- # Check if radius limitation is enabled
- if (self.use_rad_lim and (msg.vel_left != 0 or msg.vel_right != 0)):
- self.checkAndAdjustRadius(msg)
-
-
- self.driver.setWheelsSpeed(left=msg.vel_left,right=msg.vel_right)
- # Put the wheel commands in a message and publish
- self.msg_wheels_cmd.header = msg.header
- # Record the time the command was given to the wheels_driver
- self.msg_wheels_cmd.header.stamp = rospy.get_rostime()
- self.msg_wheels_cmd.vel_left = msg.vel_left
- self.msg_wheels_cmd.vel_right = msg.vel_right
- self.pub_wheels_cmd.publish(self.msg_wheels_cmd)
-
- def cbRadLimit(self, msg):
- rospy.set_param("~use_rad_lim", msg.data)
- self.use_rad_lim = msg.data
-
- def checkAndAdjustRadius(self, msg):
- didAdjustment = False
- # if both motor cmds do not have the same sign, we're demanding for an on-point turn (not allowed)
- if (np.sign(msg.vel_left) != np.sign(msg.vel_right)):
-
- # Simply set the smaller velocity to zero
- if (abs(msg.vel_left) < abs(msg.vel_right)):
- msg.vel_left = 0.0
- else:
- msg.vel_right = 0.0
-
- didAdjustment = True
-
- # set v1, v2 from msg velocities such that v2 > v1
- if (abs(msg.vel_right) > abs(msg.vel_left)):
- v1 = msg.vel_left
- v2 = msg.vel_right
- else:
- v1 = msg.vel_right
- v2 = msg.vel_left
-
- # Check if a smaller radius than allowed is demanded
- if (v1 == 0 or abs(v2 / v1) > (self.min_rad + self.wheel_distance/2.0)/(self.min_rad - self.wheel_distance/2.0)):
-
- # adjust velocities evenly such that condition is fulfilled
- delta_v = (v2-v1)/2 - self.wheel_distance/(4*self.min_rad)*(v1+v2)
- v1 += delta_v
- v2 -= delta_v
- didAdjustment = True
-
- # set msg velocities from v1, v2 with the same mapping as when we set v1, v2
- if (abs(msg.vel_right) > abs(msg.vel_left)):
- msg.vel_left = v1
- msg.vel_right = v2
- else:
- msg.vel_left = v2
- msg.vel_right = v1
-
- return didAdjustment
-
-
- def cbEStop(self,msg):
- self.estop=not self.estop
- if self.estop:
- rospy.loginfo("[%s] Emergency Stop Activated")
- else:
- rospy.loginfo("[%s] Emergency Stop Released")
-
- def on_shutdown(self):
- self.driver.setWheelsSpeed(left=0.0,right=0.0)
- rospy.loginfo("[%s] Shutting down."%(rospy.get_name()))
-
-if __name__ == '__main__':
- # Initialize the node with rospy
- rospy.init_node('wheels_driver_node', anonymous=False)
- # Create the DaguCar object
- node = WheelsDriverNode()
- # Setup proper shutdown behavior
- rospy.on_shutdown(node.on_shutdown)
- # Keep it spinning to keep the node alive
- rospy.spin()
diff --git a/catkin_ws/src/05-teleop/joy_mapper/launch/joy_mapper_node.launch b/catkin_ws/src/05-teleop/joy_mapper/launch/joy_mapper_node.launch
deleted file mode 100644
index 6c86c9bd97..0000000000
--- a/catkin_ws/src/05-teleop/joy_mapper/launch/joy_mapper_node.launch
+++ /dev/null
@@ -1,25 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/catkin_ws/src/05-teleop/pi_camera/launch/camera_node.launch b/catkin_ws/src/05-teleop/pi_camera/launch/camera_node.launch
deleted file mode 100644
index 5e41d2247a..0000000000
--- a/catkin_ws/src/05-teleop/pi_camera/launch/camera_node.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/pi_camera/launch/image_proc_node.launch b/catkin_ws/src/05-teleop/pi_camera/launch/image_proc_node.launch
deleted file mode 100644
index f07e55cfd0..0000000000
--- a/catkin_ws/src/05-teleop/pi_camera/launch/image_proc_node.launch
+++ /dev/null
@@ -1,22 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_continuous.easy_node.yaml b/catkin_ws/src/05-teleop/pi_camera/src/camera_node_continuous.easy_node.yaml
deleted file mode 100644
index 1e071d1bc6..0000000000
--- a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_continuous.easy_node.yaml
+++ /dev/null
@@ -1,27 +0,0 @@
-description:
- Camera driver.
-
-parameters:
- framerate:
- type: float
- default: 60
- desc: Frame rate
- res_w:
- type: int
- default: 320
- desc: Resolution (width)
- res_h:
- type: int
- default: 200
- desc: Resolution (height)
-
-subscriptions: {}
-
-
-publishers:
- image_compressed:
- topic: ~image/compressed"
- type: sensor_msgs/CompressedImage
- queue_size: 1
-
-contracts: {}
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_continuous.py b/catkin_ws/src/05-teleop/pi_camera/src/camera_node_continuous.py
deleted file mode 100755
index ac936751e0..0000000000
--- a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_continuous.py
+++ /dev/null
@@ -1,105 +0,0 @@
-#!/usr/bin/env python
-import rospy
-import cv2
-import io
-import numpy as np
-from cv_bridge import CvBridge, CvBridgeError
-from sensor_msgs.msg import Image
-from sensor_msgs.msg import CompressedImage
-from picamera import PiCamera
-from picamera.array import PiRGBArray
-
-
-class CameraNode(object):
- def __init__(self):
- self.node_name = rospy.get_name()
- rospy.loginfo("[%s] Initializing......" %(self.node_name))
- # TODO: load parameters
-
- self.framerate = self.setupParam("~framerate",60.0)
- #self.res_w = self.setupParam("~res_w",320)
- #self.res_h = self.setupParam("~res_h",200)
-
- # self.img_low_framerate = self.setupParam("~img_low_framerate",30.0)
- # self.img_high_framerate = self.setupParam("~img_high_framerate",5.0)
- # self.img_low_res_w = self.setupParam("~img_low_res_w",320)
- # self.img_low_res_h = self.setupParam("~img_low_res_h",200)
- self.img_high_res_w = self.setupParam("~img_high_res_w",640)
- self.img_high_res_h = self.setupParam("~img_high_res_h",400)
- # self.uncompress = self.setupParam("~uncompress",False)
-
- # TODO: load camera info yaml file and publish CameraInfo
- self.pub_img= rospy.Publisher("~image/compressed",CompressedImage,queue_size=1)
-
- # if self.uncompress:
- # self.pub_img_low = rospy.Publisher("~img_low/raw",Image,queue_size=1)
- # self.pub_img_high= rospy.Publisher("~img_high/raw",Image,queue_size=1)
- # else:
- # self.pub_img_low = rospy.Publisher("~img_low/compressed",CompressedImage,queue_size=1)
- # self.pub_img_high= rospy.Publisher("~img_high/compressed",CompressedImage,queue_size=1)
-
- self.has_published = False
- self.bridge = CvBridge()
-
- # Setup PiCamera
- self.stream = io.BytesIO()
- self.bridge = CvBridge()
- self.camera = PiCamera()
- self.camera.framerate = self.framerate
- self.camera.resolution = (self.res_w,self.res_h)
-
- # TODO setup other parameters of the camera such as exposure and white balance etc
-
- # Setup timer
- self.camera_capture = self.camera.capture_continuous(self.stream,'jpeg',use_video_port=True)
- self.timer_img_low = rospy.Timer(rospy.Duration.from_sec(1.0/self.framerate),self.cbTimer)
- rospy.loginfo("[%s] Initialized." %(self.node_name))
-
- def setupParam(self,param_name,default_value):
- value = rospy.get_param(param_name,default_value)
- rospy.set_param(param_name,value) #Write to parameter server for transparancy
- rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
- return value
-
- def cbTimer(self,event):
- if not rospy.is_shutdown():
- self.camera_capture.next()
- self.grabAndPublish(self.stream,self.pub_img)
- # Maybe for every 5 img_low, change the setting of the camera and capture a higher res img and publish.
-
- def grabAndPublish(self,stream,publisher):
- # Grab image from stream
- stream.seek(0)
- img_data = stream.getvalue()
-
- if self.uncompress:
- # Publish raw image
- data = np.fromstring(img_data, dtype=np.uint8)
- image = cv2.imdecode(data, 1)
- image_msg = self.bridge.cv2_to_imgmsg(image)
- else:
- # Publish compressed image only
- image_msg = CompressedImage()
- image_msg.data = img_data
- image_msg.format = "jpeg"
-
- image_msg.header.stamp = rospy.Time.now()
- # Publish
- publisher.publish(image_msg)
- # Clear stream
- stream.seek(0)
- stream.truncate()
-
- if not self.has_published:
- rospy.loginfo("[%s] Published the first image." %(self.node_name))
- self.has_published = True
-
- def onShutdown(self):
- self.camera.close()
- rospy.loginfo("[%s] Shutdown." %(self.node_name))
-
-if __name__ == '__main__':
- rospy.init_node('camera_node',anonymous=False)
- camera_node = CameraNode()
- rospy.on_shutdown(camera_node.onShutdown)
- rospy.spin()
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_sequence.easy_node.yaml b/catkin_ws/src/05-teleop/pi_camera/src/camera_node_sequence.easy_node.yaml
deleted file mode 100644
index 1b691a1891..0000000000
--- a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_sequence.easy_node.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-description:
- Camera driver, second approach.
-
-parameters: {}
-subscriptions: {}
-publishers: {}
-contracts: {}
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_sequence.py b/catkin_ws/src/05-teleop/pi_camera/src/camera_node_sequence.py
deleted file mode 100755
index 7a25c70fee..0000000000
--- a/catkin_ws/src/05-teleop/pi_camera/src/camera_node_sequence.py
+++ /dev/null
@@ -1,159 +0,0 @@
-#!/usr/bin/env python
-import io
-import thread
-
-import yaml
-
-from duckietown_msgs.msg import BoolStamped
-from duckietown_utils import get_duckiefleet_root
-from picamera import PiCamera
-from picamera.array import PiRGBArray
-import rospkg
-import rospy
-from sensor_msgs.msg import CompressedImage
-from sensor_msgs.srv import SetCameraInfo, SetCameraInfoResponse
-
-
-class CameraNode(object):
-
- def __init__(self):
- self.node_name = rospy.get_name()
- rospy.loginfo("[%s] Initializing......" % (self.node_name))
-
- self.framerate_high = self.setupParam("~framerate_high", 30.0)
- self.framerate_low = self.setupParam("~framerate_low", 15.0)
- self.res_w = self.setupParam("~res_w", 640)
- self.res_h = self.setupParam("~res_h", 480)
-
- self.image_msg = CompressedImage()
-
- # Setup PiCamera
-
- self.camera = PiCamera()
- self.framerate = self.framerate_high # default to high
- self.camera.framerate = self.framerate
- self.camera.resolution = (self.res_w, self.res_h)
-
- # For intrinsic calibration
- self.cali_file_folder = get_duckiefleet_root() + "/calibrations/camera_intrinsic/"
-
- self.frame_id = rospy.get_namespace().strip('/') + "/camera_optical_frame"
-
- self.has_published = False
- self.pub_img = rospy.Publisher("~image/compressed", CompressedImage, queue_size=1)
- self.sub_switch_high = rospy.Subscriber("~framerate_high_switch", BoolStamped, self.cbSwitchHigh, queue_size=1)
-
- # Create service (for camera_calibration)
- self.srv_set_camera_info = rospy.Service("~set_camera_info", SetCameraInfo, self.cbSrvSetCameraInfo)
-
- self.stream = io.BytesIO()
-
- #self.camera.exposure_mode = 'off'
- # self.camera.awb_mode = 'off'
-
- self.is_shutdown = False
- self.update_framerate = False
- # Setup timer
- rospy.loginfo("[%s] Initialized." % (self.node_name))
-
- def cbSwitchHigh(self, switch_msg):
- print switch_msg
- if switch_msg.data and self.framerate != self.framerate_high:
- self.framerate = self.framerate_high
- self.update_framerate = True
- elif not switch_msg.data and self.framerate != self.framerate_low:
- self.framerate = self.framerate_low
- self.update_framerate = True
-
- def startCapturing(self):
- rospy.loginfo("[%s] Start capturing." % (self.node_name))
- while not self.is_shutdown and not rospy.is_shutdown():
- gen = self.grabAndPublish(self.stream, self.pub_img)
- try:
- self.camera.capture_sequence(gen, 'jpeg', use_video_port=True, splitter_port=0)
- except StopIteration:
- pass
- # print "updating framerate"
- self.camera.framerate = self.framerate
- self.update_framerate = False
-
- self.camera.close()
- rospy.loginfo("[%s] Capture Ended." % (self.node_name))
-
- def grabAndPublish(self, stream, publisher):
- while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown():
- yield stream
- # Construct image_msg
- # Grab image from stream
- stamp = rospy.Time.now()
- stream.seek(0)
- stream_data = stream.getvalue()
- # Generate compressed image
- image_msg = CompressedImage()
- image_msg.format = "jpeg"
- image_msg.data = stream_data
-
- image_msg.header.stamp = stamp
- image_msg.header.frame_id = self.frame_id
- publisher.publish(image_msg)
-
- # Clear stream
- stream.seek(0)
- stream.truncate()
-
- if not self.has_published:
- rospy.loginfo("[%s] Published the first image." % (self.node_name))
- self.has_published = True
-
- rospy.sleep(rospy.Duration.from_sec(0.001))
-
- def setupParam(self, param_name, default_value):
- value = rospy.get_param(param_name, default_value)
- rospy.set_param(param_name, value) #Write to parameter server for transparancy
- rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
- return value
-
- def onShutdown(self):
- rospy.loginfo("[%s] Closing camera." % (self.node_name))
- self.is_shutdown = True
- rospy.loginfo("[%s] Shutdown." % (self.node_name))
-
- def cbSrvSetCameraInfo(self, req):
- # TODO: save req.camera_info to yaml file
- rospy.loginfo("[cbSrvSetCameraInfo] Callback!")
- filename = self.cali_file_folder + rospy.get_namespace().strip("/") + ".yaml"
- response = SetCameraInfoResponse()
- response.success = self.saveCameraInfo(req.camera_info, filename)
- response.status_message = "Write to %s" % filename #TODO file name
- return response
-
- def saveCameraInfo(self, camera_info_msg, filename):
- # Convert camera_info_msg and save to a yaml file
- rospy.loginfo("[saveCameraInfo] filename: %s" % (filename))
-
- # Converted from camera_info_manager.py
- calib = {'image_width': camera_info_msg.width,
- 'image_height': camera_info_msg.height,
- 'camera_name': rospy.get_name().strip("/"), #TODO check this
- 'distortion_model': camera_info_msg.distortion_model,
- 'distortion_coefficients': {'data': camera_info_msg.D, 'rows':1, 'cols':5},
- 'camera_matrix': {'data': camera_info_msg.K, 'rows':3, 'cols':3},
- 'rectification_matrix': {'data': camera_info_msg.R, 'rows':3, 'cols':3},
- 'projection_matrix': {'data': camera_info_msg.P, 'rows':3, 'cols':4}}
-
- rospy.loginfo("[saveCameraInfo] calib %s" % (calib))
-
- try:
- f = open(filename, 'w')
- yaml.safe_dump(calib, f)
- return True
- except IOError:
- return False
-
-
-if __name__ == '__main__':
- rospy.init_node('camera', anonymous=False)
- camera_node = CameraNode()
- rospy.on_shutdown(camera_node.onShutdown)
- thread.start_new_thread(camera_node.startCapturing, ())
- rospy.spin()
diff --git a/catkin_ws/src/05-teleop/dagu_car/.autogenerated.md b/catkin_ws/src/06-kinematics/dagu_car/.autogenerated.md
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/.autogenerated.md
rename to catkin_ws/src/06-kinematics/dagu_car/.autogenerated.md
diff --git a/catkin_ws/src/05-teleop/dagu_car/CMakeLists.txt b/catkin_ws/src/06-kinematics/dagu_car/CMakeLists.txt
similarity index 93%
rename from catkin_ws/src/05-teleop/dagu_car/CMakeLists.txt
rename to catkin_ws/src/06-kinematics/dagu_car/CMakeLists.txt
index 61ed013d37..815b8facf7 100644
--- a/catkin_ws/src/05-teleop/dagu_car/CMakeLists.txt
+++ b/catkin_ws/src/06-kinematics/dagu_car/CMakeLists.txt
@@ -5,7 +5,6 @@ project(dagu_car)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
- adafruit_drivers
pkg_name
duckietown_msgs # Every duckietown packages should use this.
)
diff --git a/catkin_ws/src/05-teleop/dagu_car/README.md b/catkin_ws/src/06-kinematics/dagu_car/README.md
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/README.md
rename to catkin_ws/src/06-kinematics/dagu_car/README.md
diff --git a/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/__init__.py b/catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/__init__.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/include/dagu_car/__init__.py
rename to catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/__init__.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/dagucar.py b/catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/dagucar.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/include/dagu_car/dagucar.py
rename to catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/dagucar.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/daguddrive.py b/catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/daguddrive.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/include/dagu_car/daguddrive.py
rename to catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/daguddrive.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/include/dagu_car/pot.py b/catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/pot.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/include/dagu_car/pot.py
rename to catkin_ws/src/06-kinematics/dagu_car/include/dagu_car/pot.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/car_cmd_switch_node.launch b/catkin_ws/src/06-kinematics/dagu_car/launch/car_cmd_switch_node.launch
similarity index 58%
rename from catkin_ws/src/05-teleop/dagu_car/launch/car_cmd_switch_node.launch
rename to catkin_ws/src/06-kinematics/dagu_car/launch/car_cmd_switch_node.launch
index 6893c2c86e..dbcef6a4cb 100644
--- a/catkin_ws/src/05-teleop/dagu_car/launch/car_cmd_switch_node.launch
+++ b/catkin_ws/src/06-kinematics/dagu_car/launch/car_cmd_switch_node.launch
@@ -1,18 +1,12 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/06-kinematics/dagu_car/launch/forward_kinematics_node.launch b/catkin_ws/src/06-kinematics/dagu_car/launch/forward_kinematics_node.launch
new file mode 100644
index 0000000000..f66bcbfd5f
--- /dev/null
+++ b/catkin_ws/src/06-kinematics/dagu_car/launch/forward_kinematics_node.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/06-kinematics/dagu_car/launch/inverse_kinematics_node.launch b/catkin_ws/src/06-kinematics/dagu_car/launch/inverse_kinematics_node.launch
new file mode 100644
index 0000000000..9bc062d12d
--- /dev/null
+++ b/catkin_ws/src/06-kinematics/dagu_car/launch/inverse_kinematics_node.launch
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/06-kinematics/dagu_car/launch/velocity_to_pose_node.launch b/catkin_ws/src/06-kinematics/dagu_car/launch/velocity_to_pose_node.launch
new file mode 100644
index 0000000000..ca279b599d
--- /dev/null
+++ b/catkin_ws/src/06-kinematics/dagu_car/launch/velocity_to_pose_node.launch
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/wheels_cmd_switch_node.launch b/catkin_ws/src/06-kinematics/dagu_car/launch/wheels_cmd_switch_node.launch
similarity index 61%
rename from catkin_ws/src/05-teleop/dagu_car/launch/wheels_cmd_switch_node.launch
rename to catkin_ws/src/06-kinematics/dagu_car/launch/wheels_cmd_switch_node.launch
index 0a3219be4c..8ed2d42c77 100644
--- a/catkin_ws/src/05-teleop/dagu_car/launch/wheels_cmd_switch_node.launch
+++ b/catkin_ws/src/06-kinematics/dagu_car/launch/wheels_cmd_switch_node.launch
@@ -3,17 +3,10 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/05-teleop/dagu_car/launch/wheels_trimmer_node.launch b/catkin_ws/src/06-kinematics/dagu_car/launch/wheels_trimmer_node.launch
similarity index 54%
rename from catkin_ws/src/05-teleop/dagu_car/launch/wheels_trimmer_node.launch
rename to catkin_ws/src/06-kinematics/dagu_car/launch/wheels_trimmer_node.launch
index 752454e984..6fd6836b38 100644
--- a/catkin_ws/src/05-teleop/dagu_car/launch/wheels_trimmer_node.launch
+++ b/catkin_ws/src/06-kinematics/dagu_car/launch/wheels_trimmer_node.launch
@@ -1,20 +1,12 @@
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/05-teleop/dagu_car/package.xml b/catkin_ws/src/06-kinematics/dagu_car/package.xml
similarity index 89%
rename from catkin_ws/src/05-teleop/dagu_car/package.xml
rename to catkin_ws/src/06-kinematics/dagu_car/package.xml
index f9a95149ce..09411bfdc6 100644
--- a/catkin_ws/src/05-teleop/dagu_car/package.xml
+++ b/catkin_ws/src/06-kinematics/dagu_car/package.xml
@@ -15,13 +15,11 @@
duckietown_msgs
roscpp
rospy
- adafruit_drivers
pkg_name
duckietown_msgs
roscpp
rospy
- adafruit_drivers
pkg_name
diff --git a/catkin_ws/src/05-teleop/dagu_car/script/dagu_differential_drive_test.py b/catkin_ws/src/06-kinematics/dagu_car/script/dagu_differential_drive_test.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/script/dagu_differential_drive_test.py
rename to catkin_ws/src/06-kinematics/dagu_car/script/dagu_differential_drive_test.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/setup.py b/catkin_ws/src/06-kinematics/dagu_car/setup.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/setup.py
rename to catkin_ws/src/06-kinematics/dagu_car/setup.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/car_cmd_switch_node.easy_node.yaml b/catkin_ws/src/06-kinematics/dagu_car/src/car_cmd_switch_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/car_cmd_switch_node.easy_node.yaml
rename to catkin_ws/src/06-kinematics/dagu_car/src/car_cmd_switch_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/car_cmd_switch_node.py b/catkin_ws/src/06-kinematics/dagu_car/src/car_cmd_switch_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/car_cmd_switch_node.py
rename to catkin_ws/src/06-kinematics/dagu_car/src/car_cmd_switch_node.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/forward_kinematics_node.easy_node.yaml b/catkin_ws/src/06-kinematics/dagu_car/src/forward_kinematics_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/forward_kinematics_node.easy_node.yaml
rename to catkin_ws/src/06-kinematics/dagu_car/src/forward_kinematics_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/forward_kinematics_node.py b/catkin_ws/src/06-kinematics/dagu_car/src/forward_kinematics_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/forward_kinematics_node.py
rename to catkin_ws/src/06-kinematics/dagu_car/src/forward_kinematics_node.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/inverse_kinematics_node.easy_node.yaml b/catkin_ws/src/06-kinematics/dagu_car/src/inverse_kinematics_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/inverse_kinematics_node.easy_node.yaml
rename to catkin_ws/src/06-kinematics/dagu_car/src/inverse_kinematics_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/inverse_kinematics_node.py b/catkin_ws/src/06-kinematics/dagu_car/src/inverse_kinematics_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/inverse_kinematics_node.py
rename to catkin_ws/src/06-kinematics/dagu_car/src/inverse_kinematics_node.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/resources.yaml b/catkin_ws/src/06-kinematics/dagu_car/src/resources.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/resources.yaml
rename to catkin_ws/src/06-kinematics/dagu_car/src/resources.yaml
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/velocity_to_pose_node.easy_node.yaml b/catkin_ws/src/06-kinematics/dagu_car/src/velocity_to_pose_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/velocity_to_pose_node.easy_node.yaml
rename to catkin_ws/src/06-kinematics/dagu_car/src/velocity_to_pose_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/velocity_to_pose_node.py b/catkin_ws/src/06-kinematics/dagu_car/src/velocity_to_pose_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/velocity_to_pose_node.py
rename to catkin_ws/src/06-kinematics/dagu_car/src/velocity_to_pose_node.py
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/wheels_trimmer_node.easy_node.yaml b/catkin_ws/src/06-kinematics/dagu_car/src/wheels_trimmer_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/wheels_trimmer_node.easy_node.yaml
rename to catkin_ws/src/06-kinematics/dagu_car/src/wheels_trimmer_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/dagu_car/src/wheels_trimmer_node.py b/catkin_ws/src/06-kinematics/dagu_car/src/wheels_trimmer_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/dagu_car/src/wheels_trimmer_node.py
rename to catkin_ws/src/06-kinematics/dagu_car/src/wheels_trimmer_node.py
diff --git a/catkin_ws/src/05-teleop/pi_camera/.autogenerated.md b/catkin_ws/src/07-image-processing/pi_camera/.autogenerated.md
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/.autogenerated.md
rename to catkin_ws/src/07-image-processing/pi_camera/.autogenerated.md
diff --git a/catkin_ws/src/05-teleop/pi_camera/CMakeLists.txt b/catkin_ws/src/07-image-processing/pi_camera/CMakeLists.txt
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/CMakeLists.txt
rename to catkin_ws/src/07-image-processing/pi_camera/CMakeLists.txt
diff --git a/catkin_ws/src/05-teleop/pi_camera/README.md b/catkin_ws/src/07-image-processing/pi_camera/README.md
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/README.md
rename to catkin_ws/src/07-image-processing/pi_camera/README.md
diff --git a/catkin_ws/src/05-teleop/pi_camera/include/pi_camera/__init__.py b/catkin_ws/src/07-image-processing/pi_camera/include/pi_camera/__init__.py
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/include/pi_camera/__init__.py
rename to catkin_ws/src/07-image-processing/pi_camera/include/pi_camera/__init__.py
diff --git a/catkin_ws/src/05-teleop/pi_camera/include/pi_camera/camera_info.py b/catkin_ws/src/07-image-processing/pi_camera/include/pi_camera/camera_info.py
similarity index 96%
rename from catkin_ws/src/05-teleop/pi_camera/include/pi_camera/camera_info.py
rename to catkin_ws/src/07-image-processing/pi_camera/include/pi_camera/camera_info.py
index b41d5b5149..5e9625e967 100644
--- a/catkin_ws/src/05-teleop/pi_camera/include/pi_camera/camera_info.py
+++ b/catkin_ws/src/07-image-processing/pi_camera/include/pi_camera/camera_info.py
@@ -126,10 +126,13 @@ def get_camera_info_config_file(robot_name):
for df in roots:
# Load camera information
fn = os.path.join(df, 'camera_intrinsic', robot_name + '.yaml')
+ fn_default = os.path.join(df, 'camera_intrinsic', 'default.yaml')
if os.path.exists(fn):
return fn
+ elif os.path.exists(fn_default):
+ return fn_default
else:
- print('%s does not exist' % fn)
+ print('%s does not exist and neither does %s' % (fn, fn_default))
msg = 'Cannot find intrinsic file for robot %r;\n%s' % (robot_name, roots)
raise NoCameraInfoAvailable(msg)
diff --git a/catkin_ws/src/05-teleop/pi_camera/launch/cam_info_reader_node.launch b/catkin_ws/src/07-image-processing/pi_camera/launch/cam_info_reader_node.launch
similarity index 52%
rename from catkin_ws/src/05-teleop/pi_camera/launch/cam_info_reader_node.launch
rename to catkin_ws/src/07-image-processing/pi_camera/launch/cam_info_reader_node.launch
index 7bb65c73eb..6e25c835e2 100644
--- a/catkin_ws/src/05-teleop/pi_camera/launch/cam_info_reader_node.launch
+++ b/catkin_ws/src/07-image-processing/pi_camera/launch/cam_info_reader_node.launch
@@ -1,15 +1,13 @@
-
+
-
-
-
+
@@ -17,15 +15,6 @@
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/05-teleop/pi_camera/launch/decoder_node.launch b/catkin_ws/src/07-image-processing/pi_camera/launch/decoder_node.launch
similarity index 55%
rename from catkin_ws/src/05-teleop/pi_camera/launch/decoder_node.launch
rename to catkin_ws/src/07-image-processing/pi_camera/launch/decoder_node.launch
index c3d52a9c7f..75e7a34360 100644
--- a/catkin_ws/src/05-teleop/pi_camera/launch/decoder_node.launch
+++ b/catkin_ws/src/07-image-processing/pi_camera/launch/decoder_node.launch
@@ -2,17 +2,11 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/07-image-processing/pi_camera/launch/image_proc_node.launch b/catkin_ws/src/07-image-processing/pi_camera/launch/image_proc_node.launch
new file mode 100644
index 0000000000..0473131f18
--- /dev/null
+++ b/catkin_ws/src/07-image-processing/pi_camera/launch/image_proc_node.launch
@@ -0,0 +1,14 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/05-teleop/pi_camera/package.xml b/catkin_ws/src/07-image-processing/pi_camera/package.xml
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/package.xml
rename to catkin_ws/src/07-image-processing/pi_camera/package.xml
diff --git a/catkin_ws/src/05-teleop/pi_camera/setup.py b/catkin_ws/src/07-image-processing/pi_camera/setup.py
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/setup.py
rename to catkin_ws/src/07-image-processing/pi_camera/setup.py
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/cam_info_reader_node.easy_node.yaml b/catkin_ws/src/07-image-processing/pi_camera/src/cam_info_reader_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/src/cam_info_reader_node.easy_node.yaml
rename to catkin_ws/src/07-image-processing/pi_camera/src/cam_info_reader_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/cam_info_reader_node.py b/catkin_ws/src/07-image-processing/pi_camera/src/cam_info_reader_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/src/cam_info_reader_node.py
rename to catkin_ws/src/07-image-processing/pi_camera/src/cam_info_reader_node.py
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/decoder_node.easy_node.yaml b/catkin_ws/src/07-image-processing/pi_camera/src/decoder_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/src/decoder_node.easy_node.yaml
rename to catkin_ws/src/07-image-processing/pi_camera/src/decoder_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/decoder_node.py b/catkin_ws/src/07-image-processing/pi_camera/src/decoder_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/src/decoder_node.py
rename to catkin_ws/src/07-image-processing/pi_camera/src/decoder_node.py
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/img_process_node.easy_node.yaml b/catkin_ws/src/07-image-processing/pi_camera/src/img_process_node.easy_node.yaml
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/src/img_process_node.easy_node.yaml
rename to catkin_ws/src/07-image-processing/pi_camera/src/img_process_node.easy_node.yaml
diff --git a/catkin_ws/src/05-teleop/pi_camera/src/img_process_node.py b/catkin_ws/src/07-image-processing/pi_camera/src/img_process_node.py
similarity index 100%
rename from catkin_ws/src/05-teleop/pi_camera/src/img_process_node.py
rename to catkin_ws/src/07-image-processing/pi_camera/src/img_process_node.py
diff --git a/catkin_ws/src/10-lane-control/anti_instagram/src/image_transformer_node.py b/catkin_ws/src/10-lane-control/anti_instagram/src/image_transformer_node.py
index 280cc60f98..0653a7bba4 100755
--- a/catkin_ws/src/10-lane-control/anti_instagram/src/image_transformer_node.py
+++ b/catkin_ws/src/10-lane-control/anti_instagram/src/image_transformer_node.py
@@ -43,7 +43,7 @@ def __init__(self):
robot_name = rospy.get_param("~veh", "") #to read the name always reliably
# Read parameters
-
+
self.trafo_mode = self.setupParameter("~trafo_mode", 'both')
if not (self.trafo_mode == "cb" or self.trafo_mode == "lin" or self.trafo_mode == "both"):
rospy.loginfo("cannot understand argument 'trafo_mode'. set to 'both' ")
@@ -202,4 +202,4 @@ def cbNewTrafo_CB(self, th_msg):
# Setup proper shutdown behavior
#rospy.on_shutdown(node.on_shutdown)
# Keep it spinning to keep the node alive
- rospy.spin()
+rospy.spin()
diff --git a/catkin_ws/src/10-lane-control/ground_projection/include/ground_projection/configuration.py b/catkin_ws/src/10-lane-control/ground_projection/include/ground_projection/configuration.py
index 07eff656f4..f48ca2069d 100644
--- a/catkin_ws/src/10-lane-control/ground_projection/include/ground_projection/configuration.py
+++ b/catkin_ws/src/10-lane-control/ground_projection/include/ground_projection/configuration.py
@@ -67,9 +67,13 @@ def get_homography_info_config_file(robot_name):
for df in roots:
# Load camera information
fn = os.path.join(df, 'camera_extrinsic', robot_name + '.yaml')
+ fn_default = os.path.join(df, 'camera_extrinsic', 'default.yaml')
if os.path.exists(fn):
found.append(fn)
dtu.logger.info("Using filename %s" % fn)
+ elif os.path.exists(fn_default):
+ found.append(fn_default)
+ dtu.logger.info("Using filename %s" % fn_default)
if len(found) == 0:
msg = 'Cannot find homography file for robot %r;\n%s' % (robot_name, roots)
diff --git a/catkin_ws/src/10-lane-control/ground_projection/launch/ground_projection.launch b/catkin_ws/src/10-lane-control/ground_projection/launch/ground_projection.launch
index f09560a240..e4c3619496 100644
--- a/catkin_ws/src/10-lane-control/ground_projection/launch/ground_projection.launch
+++ b/catkin_ws/src/10-lane-control/ground_projection/launch/ground_projection.launch
@@ -1,25 +1,12 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/10-lane-control/lane_control/launch/lane_controller_node.launch b/catkin_ws/src/10-lane-control/lane_control/launch/lane_controller_node.launch
index 606af2a415..a142e23192 100644
--- a/catkin_ws/src/10-lane-control/lane_control/launch/lane_controller_node.launch
+++ b/catkin_ws/src/10-lane-control/lane_control/launch/lane_controller_node.launch
@@ -3,22 +3,12 @@
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
-
diff --git a/catkin_ws/src/10-lane-control/lane_control/launch/vicon_for_lane_node.launch b/catkin_ws/src/10-lane-control/lane_control/launch/vicon_for_lane_node.launch
index 8fa558f083..80208b5d26 100644
--- a/catkin_ws/src/10-lane-control/lane_control/launch/vicon_for_lane_node.launch
+++ b/catkin_ws/src/10-lane-control/lane_control/launch/vicon_for_lane_node.launch
@@ -2,17 +2,10 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/10-lane-control/lane_filter/launch/lane_filter_node.launch b/catkin_ws/src/10-lane-control/lane_filter/launch/lane_filter_node.launch
index 1561d1c852..e84bb38143 100644
--- a/catkin_ws/src/10-lane-control/lane_filter/launch/lane_filter_node.launch
+++ b/catkin_ws/src/10-lane-control/lane_filter/launch/lane_filter_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,13 +8,7 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/10-lane-control/lane_filter/launch/lane_pose_visualizer_node.launch b/catkin_ws/src/10-lane-control/lane_filter/launch/lane_pose_visualizer_node.launch
index 3eff2231b3..69a7866275 100644
--- a/catkin_ws/src/10-lane-control/lane_filter/launch/lane_pose_visualizer_node.launch
+++ b/catkin_ws/src/10-lane-control/lane_filter/launch/lane_pose_visualizer_node.launch
@@ -1,19 +1,11 @@
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/10-lane-control/line_detector/launch/line_detector_node.launch b/catkin_ws/src/10-lane-control/line_detector/launch/line_detector_node.launch
index cebec844bb..b0f8e33b0b 100644
--- a/catkin_ws/src/10-lane-control/line_detector/launch/line_detector_node.launch
+++ b/catkin_ws/src/10-lane-control/line_detector/launch/line_detector_node.launch
@@ -1,6 +1,5 @@
-
@@ -10,15 +9,7 @@
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/apriltags2_ros/apriltags2_ros/launch/apriltag_detector_node.launch b/catkin_ws/src/20-indefinite-navigation/apriltags2_ros/apriltags2_ros/launch/apriltag_detector_node.launch
index 61e5738a14..9531963577 100644
--- a/catkin_ws/src/20-indefinite-navigation/apriltags2_ros/apriltags2_ros/launch/apriltag_detector_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/apriltags2_ros/apriltags2_ros/launch/apriltag_detector_node.launch
@@ -1,22 +1,18 @@
-
-
-
-
+
@@ -24,10 +20,6 @@
-
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltag_detector_node.launch b/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltag_detector_node.launch
index 749957562c..90ff87f422 100644
--- a/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltag_detector_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltag_detector_node.launch
@@ -3,27 +3,15 @@
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltags_postprocessing_node.launch b/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltags_postprocessing_node.launch
index 775645be29..6aa2a33cf8 100644
--- a/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltags_postprocessing_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/apriltags_ros/apriltags_ros/launch/apriltags_postprocessing_node.launch
@@ -1,25 +1,13 @@
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_calculation_node.launch b/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_calculation_node.launch
index a7dcf770b1..80a11aa295 100644
--- a/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_calculation_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_calculation_node.launch
@@ -1,16 +1,9 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_node.launch b/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_node.launch
index 84349f6f61..87fa0832b2 100644
--- a/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/auto_calibration/launch/auto_calibration_node.launch
@@ -1,16 +1,9 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/charging_control/launch/charging_control_node.launch b/catkin_ws/src/20-indefinite-navigation/charging_control/launch/charging_control_node.launch
index 0cb0cb9869..ddd57e5e4c 100644
--- a/catkin_ws/src/20-indefinite-navigation/charging_control/launch/charging_control_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/charging_control/launch/charging_control_node.launch
@@ -1,6 +1,5 @@
-
@@ -8,13 +7,7 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/actions_dispatcher_node.launch b/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/actions_dispatcher_node.launch
index 4bc041472a..f4bcb2be13 100644
--- a/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/actions_dispatcher_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/actions_dispatcher_node.launch
@@ -2,7 +2,6 @@
-
@@ -14,14 +13,7 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/graph_search_server_node.launch b/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/graph_search_server_node.launch
index c041c7ff66..82b4d91040 100644
--- a/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/graph_search_server_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/fleet_planning/launch/graph_search_server_node.launch
@@ -3,7 +3,6 @@
-
@@ -12,13 +11,8 @@
-
-
+
-
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node.launch b/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node.launch
index d4dfad52e9..84c6ec6f39 100644
--- a/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node.launch
@@ -1,18 +1,13 @@
-
-
-
+
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node_f9_test.launch b/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node_f9_test.launch
index bb7a19fe9b..3aef84b3be 100644
--- a/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node_f9_test.launch
+++ b/catkin_ws/src/20-indefinite-navigation/fsm/launch/fsm_node_f9_test.launch
@@ -1,18 +1,13 @@
-
-
-
+
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/fsm/launch/logic_gate_node.launch b/catkin_ws/src/20-indefinite-navigation/fsm/launch/logic_gate_node.launch
index 3dce91d24c..3bd7ca8e4f 100644
--- a/catkin_ws/src/20-indefinite-navigation/fsm/launch/logic_gate_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/fsm/launch/logic_gate_node.launch
@@ -1,18 +1,13 @@
-
-
-
+
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/servo_detector.launch b/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/servo_detector.launch
index 1dba09473f..9959c3a386 100644
--- a/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/servo_detector.launch
+++ b/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/servo_detector.launch
@@ -1,6 +1,5 @@
-
diff --git a/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/simple_servo.launch b/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/simple_servo.launch
index e4387582b1..601b24f4df 100644
--- a/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/simple_servo.launch
+++ b/catkin_ws/src/20-indefinite-navigation/indefinite_navigation/launch/simple_servo.launch
@@ -1,6 +1,5 @@
-
@@ -9,8 +8,7 @@
-
-
+
@@ -19,9 +17,6 @@
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/closed_loop_intersection_controller_node.launch b/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/closed_loop_intersection_controller_node.launch
index 4cb0e0ebcf..54c296a058 100644
--- a/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/closed_loop_intersection_controller_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/closed_loop_intersection_controller_node.launch
@@ -1,19 +1,13 @@
-
-
-
+
-
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/open_loop_intersection_controller_node.launch b/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/open_loop_intersection_controller_node.launch
index 260e5cb1b2..bab0728885 100644
--- a/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/open_loop_intersection_controller_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/intersection_control/launch/open_loop_intersection_controller_node.launch
@@ -1,18 +1,14 @@
-
-
+
-
+
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/maintenance_control/launch/maintenance_control_node.launch b/catkin_ws/src/20-indefinite-navigation/maintenance_control/launch/maintenance_control_node.launch
index 8cb80321fc..6c76a1d774 100644
--- a/catkin_ws/src/20-indefinite-navigation/maintenance_control/launch/maintenance_control_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/maintenance_control/launch/maintenance_control_node.launch
@@ -1,6 +1,5 @@
-
@@ -8,13 +7,7 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/navigation/launch/actions_dispatcher_node.launch b/catkin_ws/src/20-indefinite-navigation/navigation/launch/actions_dispatcher_node.launch
index a3e5560e3b..794776b106 100644
--- a/catkin_ws/src/20-indefinite-navigation/navigation/launch/actions_dispatcher_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/navigation/launch/actions_dispatcher_node.launch
@@ -2,20 +2,12 @@
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/navigation/launch/graph_search_server_node.launch b/catkin_ws/src/20-indefinite-navigation/navigation/launch/graph_search_server_node.launch
index 7572654504..60142ae0cb 100644
--- a/catkin_ws/src/20-indefinite-navigation/navigation/launch/graph_search_server_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/navigation/launch/graph_search_server_node.launch
@@ -3,21 +3,13 @@
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/navigation/launch/random_april_tag_turns_node.launch b/catkin_ws/src/20-indefinite-navigation/navigation/launch/random_april_tag_turns_node.launch
index ad7e16e31f..5ba76a9c91 100644
--- a/catkin_ws/src/20-indefinite-navigation/navigation/launch/random_april_tag_turns_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/navigation/launch/random_april_tag_turns_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,15 +8,10 @@
-
-
+
-
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/navigation/launch/sr_turns_node.launch b/catkin_ws/src/20-indefinite-navigation/navigation/launch/sr_turns_node.launch
index 492318c30e..7a33a71c1c 100644
--- a/catkin_ws/src/20-indefinite-navigation/navigation/launch/sr_turns_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/navigation/launch/sr_turns_node.launch
@@ -2,23 +2,15 @@
-
-
-
+
-
-
-
-
-
-
diff --git a/catkin_ws/src/20-indefinite-navigation/stop_line_filter/launch/stop_line_filter_node.launch b/catkin_ws/src/20-indefinite-navigation/stop_line_filter/launch/stop_line_filter_node.launch
index 902de9505f..b7d4635d07 100644
--- a/catkin_ws/src/20-indefinite-navigation/stop_line_filter/launch/stop_line_filter_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/stop_line_filter/launch/stop_line_filter_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,13 +8,7 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/20-indefinite-navigation/unicorn_intersection/launch/unicorn_intersection_node.launch b/catkin_ws/src/20-indefinite-navigation/unicorn_intersection/launch/unicorn_intersection_node.launch
index 858b3a97d0..2a9dbce0eb 100644
--- a/catkin_ws/src/20-indefinite-navigation/unicorn_intersection/launch/unicorn_intersection_node.launch
+++ b/catkin_ws/src/20-indefinite-navigation/unicorn_intersection/launch/unicorn_intersection_node.launch
@@ -1,6 +1,5 @@
-
@@ -8,13 +7,7 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/30-localization-and-planning/localization/launch/localization_node.launch b/catkin_ws/src/30-localization-and-planning/localization/launch/localization_node.launch
index ce46208d9c..5252c94245 100644
--- a/catkin_ws/src/30-localization-and-planning/localization/launch/localization_node.launch
+++ b/catkin_ws/src/30-localization-and-planning/localization/launch/localization_node.launch
@@ -3,27 +3,15 @@
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/coordination_ETHZ17.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/coordination_ETHZ17.launch
index deece685ce..a4d4ab4950 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/coordination_ETHZ17.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/coordination_ETHZ17.launch
@@ -4,7 +4,6 @@
-
@@ -14,23 +13,12 @@
-
-
+
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_coordinator_node.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_coordinator_node.launch
index 1966e9c218..81ef51a679 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_coordinator_node.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_coordinator_node.launch
@@ -1,6 +1,5 @@
-
@@ -10,11 +9,7 @@
-
-
-
-
-
+
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_node.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_node.launch
index 44bf17893f..131927ace7 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_node.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,17 +8,10 @@
-
-
+
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_signals.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_signals.launch
index 2db7ca804c..41540b6634 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_signals.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_duckiebot_signals.launch
@@ -1,12 +1,10 @@
-
-
@@ -16,7 +14,6 @@
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_intersection_detection.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_intersection_detection.launch
index 151786fa93..9a8f7f2237 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_intersection_detection.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/fake_intersection_detection.launch
@@ -1,6 +1,5 @@
-
@@ -11,18 +10,11 @@
-
-
+
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/general.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/general.launch
index 13aa7ca895..71473a582e 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/general.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/general.launch
@@ -1,10 +1,8 @@
-
-
@@ -14,7 +12,7 @@
-
+
@@ -23,7 +21,7 @@
-
+
@@ -33,42 +31,12 @@
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_stop.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_stop.launch
index 980c43ff4a..4f511ec2e6 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_stop.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_stop.launch
@@ -1,17 +1,13 @@
-
-
-
-
@@ -25,7 +21,6 @@
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_tl.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_tl.launch
index ef10bdadac..013e435259 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_tl.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_fake_duckiebot_gui_tl.launch
@@ -1,17 +1,13 @@
-
-
-
-
@@ -25,7 +21,6 @@
-
diff --git a/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_node.launch b/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_node.launch
index b573e447c3..5ff76ce368 100644
--- a/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_node.launch
+++ b/catkin_ws/src/40-coordination/explicit_coordinator/launch/simple_coordinator_node.launch
@@ -1,6 +1,5 @@
-
@@ -11,16 +10,7 @@
-
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_old.launch b/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_old.launch
deleted file mode 100644
index 9e036950b9..0000000000
--- a/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_old.launch
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_visualizer.launch b/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_visualizer.launch
index 397d6a26de..b0e11200c5 100644
--- a/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_visualizer.launch
+++ b/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_visualizer.launch
@@ -1,9 +1,7 @@
-
-
diff --git a/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_with_gui.launch b/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_with_gui.launch
index 4473f32ebe..eedd4662c6 100644
--- a/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_with_gui.launch
+++ b/catkin_ws/src/40-coordination/led_detection/launch/LED_detection_with_gui.launch
@@ -1,24 +1,15 @@
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_detection/launch/led_detection.launch b/catkin_ws/src/40-coordination/led_detection/launch/led_detection.launch
index 4b285ab239..4c31bfc22c 100644
--- a/catkin_ws/src/40-coordination/led_detection/launch/led_detection.launch
+++ b/catkin_ws/src/40-coordination/led_detection/launch/led_detection.launch
@@ -1,10 +1,8 @@
-
-
@@ -12,20 +10,11 @@
-
-
+
-
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/40-coordination/led_emitter/.autogenerated.md b/catkin_ws/src/40-coordination/led_emitter/.autogenerated.md
deleted file mode 100644
index f21224b93a..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/.autogenerated.md
+++ /dev/null
@@ -1,21 +0,0 @@
-
-
-
-
-
-## Package information
-
-[Link to package on Github](github:org=duckietown,repo=Software,path=40-coordination/led_emitter,branch=master18)
-
-### Essentials {nonumber="1"}
-
-Maintainer: [Andrea Censi](mailto:acensi@idsc.mavt.ethz.ch)
-
-### Description {nonumber="1"}
-
-TODO: description for `led_emitter` package.
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/Makefile b/catkin_ws/src/40-coordination/led_emitter/Makefile
deleted file mode 100644
index b75b928f20..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/catkin_ws/src/40-coordination/led_emitter/README.md b/catkin_ws/src/40-coordination/led_emitter/README.md
deleted file mode 100644
index b511dbcaa1..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/README.md
+++ /dev/null
@@ -1,11 +0,0 @@
-# Package `led_emitter` {#led_emitter}
-
-
-
-## In depth
-
-To test the LED emitter run the following command:
-
- $ roslaunch led_emitter led_emitter_node.launch veh:=![robot name]
-
-This launches the LED emitter node. Note that the either the coordination node or the led_joy_mapper node should be launched in order to be able to use the led_emitter_node.
diff --git a/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_ETHZ17.launch b/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_ETHZ17.launch
deleted file mode 100644
index fcecf070f3..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_ETHZ17.launch
+++ /dev/null
@@ -1,37 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_old.launch b/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_old.launch
deleted file mode 100644
index b2a3781aeb..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_old.launch
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_test.launch b/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_test.launch
deleted file mode 100644
index 98550e3795..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/launch/led_emitter_node_test.launch
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/mainpage.dox b/catkin_ws/src/40-coordination/led_emitter/mainpage.dox
deleted file mode 100644
index ca173297fa..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/mainpage.dox
+++ /dev/null
@@ -1,14 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b led_emitter
-
-
-
--->
-
-
-*/
diff --git a/catkin_ws/src/40-coordination/led_emitter/manifest.xml.old b/catkin_ws/src/40-coordination/led_emitter/manifest.xml.old
deleted file mode 100644
index d3697f3074..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/manifest.xml.old
+++ /dev/null
@@ -1,14 +0,0 @@
-
-
-
- led_emitter
-
-
- Ubuntu user
- BSD
-
- http://ros.org/wiki/led_emitter
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/src/.led_emitter_node.py.swp b/catkin_ws/src/40-coordination/led_emitter/src/.led_emitter_node.py.swp
deleted file mode 100644
index 1e8b61a41c..0000000000
Binary files a/catkin_ws/src/40-coordination/led_emitter/src/.led_emitter_node.py.swp and /dev/null differ
diff --git a/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node.py b/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node.py
deleted file mode 100755
index 5f255ef9c9..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node.py
+++ /dev/null
@@ -1,137 +0,0 @@
-#!/usr/bin/env python
-import rospy
-from rgb_led import *
-import sys
-import time
-from std_msgs.msg import Float32, Int8, String
-from rgb_led import RGB_LED
-from duckietown_msgs.msg import BoolStamped, CoordinationSignal
-
-
-class LEDEmitter(object):
- def __init__(self):
- self.led = RGB_LED()
- self.node_name = rospy.get_name()
- self.active = True
- self.pattern = [[0,0,0]]*5
- self.current_pattern_name = 'OFF'
- self.changePattern_(self.current_pattern_name)
-
- # Import protocol
- self.protocol = rospy.get_param("~LED_protocol")
-
- # If True, the LED turn on and off. Else, they are always on
- self.onOff = True
-
- if self.onOff:
- self.cycle = 1.0/self.protocol['signals']['CAR_SIGNAL_A']['frequency']
- self.is_on = False
- self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(self.cycle/(2.0)),self.cycleTimer)
-
- # Publish
- #self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
- self.pub_state = rospy.Publisher("~current_led_state",String,queue_size=1)
-
- # Subscribe
- self.sub_pattern = rospy.Subscriber("~change_color_pattern",String,self.changePattern)
- self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
-
- # Scale intensity of the LEDs
- scale = 0.8
- for _, c in self.protocol['colors'].items():
- for i in range(3):
- c[i] = c[i]*scale
-
- def cbSwitch(self, switch_msg): # active/inactive switch from FSM
- self.active = switch_msg.data
-
- def cycleTimer(self,event):
- if not self.active:
- return
- elif not self.onOff:
- # No oscillation
- for i in range(5):
- self.led.setRGB(i,[self.pattern[i][0],self.pattern[i][1],self.pattern[i][2]])
- else:
- # Oscillate
- if self.is_on:
- for i in range(5):
- self.led.setRGB(i,[0,0,0])
- self.is_on = False
- else:
- for i in range(5):
- self.led.setRGB(i,[self.pattern[i][0],self.pattern[i][1],self.pattern[i][2]])
- self.is_on = True
-
- def changePattern(self, msg):
- self.changePattern_(msg.data)
-
- def changePattern_(self, pattern_name):
- if pattern_name:
- if self.current_pattern_name == pattern_name:
- return
- else:
- self.current_pattern_name = pattern_name
-
- # With joystick
- if self.current_pattern_name == 'ON_WHITE':
- self.pattern = [self.protocol['colors']['white']]*5
- elif self.current_pattern_name == 'ON_RED':
- self.pattern = [self.protocol['colors']['red']]*5
- elif self.current_pattern_name == 'ON_BLUE':
- self.pattern = [self.protocol['colors']['blue']]*5
- elif self.current_pattern_name == 'ON_GREEN':
- self.pattern = [self.protocol['colors']['green']]*5
- elif self.current_pattern_name == 'CAR_SIGNAL_A':
- self.current_pattern_name = CoordinationSignal.SIGNAL_A
-
- elif self.current_pattern_name == 'SIGNAL_GREEN':
- self.current_pattern_name = CoordinationSignal.SIGNAL_GREEN
- elif self.current_pattern_name == 'OFF':
- self.current_pattern_name = CoordinationSignal.OFF
- else:
- self.pattern = [self.protocol['colors']['black']]*5
-
- # With coordination (new)
- if self.current_pattern_name == CoordinationSignal.SIGNAL_GREEN:
- color = self.protocol['signals'][pattern_name]['color']
- self.pattern = [self.protocol['colors'][color]]*5
- elif self.current_pattern_name == CoordinationSignal.OFF:
- self.pattern = [self.protocol['colors']['black']]*5
- else:
- color = self.protocol['signals'][pattern_name]['color']
- self.pattern = [self.protocol['colors']['black']]*5
- self.pattern[2] = self.protocol['colors'][color]
- self.pattern[0] = self.protocol['colors'][color]
- self.pattern[4] = self.protocol['colors'][color]
-
- # Change frequency (frequency does not change)
- self.cycle = self.protocol['signals'][pattern_name]['frequency']
- self.changeFrequency()
-
- # Change LEDs
- if not self.onOff:
- self.cycleTimer([])
-
- # Loginfo
- rospy.loginfo('[%s] Pattern changed to (%r), cycle: %s ' %(self.node_name,pattern_name,self.cycle))
-
- # Publish current pattern
- self.pub_state.publish(self.current_pattern_name)
-
- def changeFrequency(self):
- try:
- #self.cycle = msg.data
- self.cycle_timer.shutdown()
- #below, convert to hz
- d = 1.0/(2.0*self.cycle)
- self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d), self.cycleTimer)
- except ValueError as e:
- self.cycle = None
- self.current_pattern_name = None
- self.pub_state.publish(float(self.cycle))
-
-if __name__ == '__main__':
- rospy.init_node('led_emitter',anonymous=False)
- node = LEDEmitter()
- rospy.spin()
diff --git a/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node_old.py b/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node_old.py
deleted file mode 100755
index 66ee79f90c..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node_old.py
+++ /dev/null
@@ -1,92 +0,0 @@
-#!/usr/bin/env python
-import rospy
-from rgb_led import *
-import sys
-import time
-from std_msgs.msg import Float32, Int8, String
-from rgb_led import RGB_LED
-from duckietown_msgs.msg import BoolStamped
-
-
-class LEDEmitter(object):
- def __init__(self):
- self.led = RGB_LED()
- self.node_name = rospy.get_name()
- self.pub_state = rospy.Publisher("~current_led_state",Float32,queue_size=1)
- self.sub_pattern = rospy.Subscriber("~change_color_pattern", String, self.changePattern)
- self.sub_switch = rospy.Subscriber("~switch",BoolStamped,self.cbSwitch)
- self.cycle = None
-
- self.is_on = False
- self.active = True
-
- self.protocol = rospy.get_param("~LED_protocol") #should be a list of tuples
-
- self.pattern_off = [[0,0,0]] * 5
-
- scale = 0.5
- for _, c in self.protocol['colors'].items():
- for i in range(3):
- c[i] = c[i] * scale
-
- self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(.1), self.cycleTimer)
- self.current_pattern_name = None
- self.changePattern_('CAR_SIGNAL_A')
-
- def cbSwitch(self, switch_msg): # active/inactive switch from FSM
- self.active = switch_msg.data
-
-
- def cycleTimer(self,event):
- if not self.active:
- return
- if self.is_on:
- for i in range(5):
- self.led.setRGB(i, [0, 0, 0])
- self.is_on = False
- else:
- for i in range(5):
- self.led.setRGB(i, self.pattern[i])
- self.is_on = True
-
- def changePattern(self, msg):
- self.changePattern_(msg.data)
-
- def changePattern_(self, pattern_name):
- if pattern_name:
- if (self.current_pattern_name == pattern_name):
- return
- else:
- self.current_pattern_name = pattern_name
-
- rospy.loginfo('changePattern(%r)' % pattern_name)
- color = self.protocol['signals'][pattern_name]['color']
- self.cycle = self.protocol['signals'][pattern_name]['frequency']
- print("color: %s, freq (Hz): %s "%(color, self.cycle))
-
- self.pattern = [[0,0,0]] * 5
- self.pattern[2] = self.protocol['colors'][color]
- #print(self.pattern)
-
- if pattern_name in ['traffic_light_go', 'traffic_light_stop']:
- self.pattern = [self.protocol['colors'][color]] * 5
-
- self.changeFrequency()
-
- def changeFrequency(self):
- try:
- #self.cycle = msg.data
- self.cycle_timer.shutdown()
- #below, convert to hz
- d = 1.0/(2.0*self.cycle)
- self.cycle_timer = rospy.Timer(rospy.Duration.from_sec(d), self.cycleTimer)
- except ValueError as e:
- self.cycle = None
- self.current_pattern_name = None
- self.pub_state.publish(float(self.cycle))
-
-if __name__ == '__main__':
- rospy.init_node('led_emitter',anonymous=False)
- node = LEDEmitter()
- rospy.spin()
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node_test.py b/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node_test.py
deleted file mode 100755
index df416027ee..0000000000
--- a/catkin_ws/src/40-coordination/led_emitter/src/led_emitter_node_test.py
+++ /dev/null
@@ -1,32 +0,0 @@
-#!/usr/bin/env python
-import rospy
-from rgb_led import *
-import sys
-import time
-from std_msgs.msg import Float32, Int8
-import random
-
-class LEDEmitterTest(object):
- def __init__(self):
- self.node_name = rospy.get_name()
- self.pub_state = rospy.Publisher("~change_light_frequency",Float32,queue_size=10)
- self.sub_state = rospy.Subscriber("~current_led_state",Float32, self.changeState)
- self.pub_timer = rospy.Timer(rospy.Duration.from_sec(5.0),self.cycleTimer)
- self.state_list = [2.8, 4.1, 5.0] # In hz
- self.counter = 0
-
- def cycleTimer(self,event):
- self.pub_state.publish(self.state_list[self.counter])
- rospy.loginfo("Testing state " + str(self.state_list[self.counter]))
- self.counter = (self.counter+1) % len(self.state_list)
-
-
-
- def changeState(self,msg):
- rospy.loginfo("I see " + str(msg.data) + " as state")
-
-if __name__ == '__main__':
- rospy.init_node('led_emitter',anonymous=False)
- node = LEDEmitterTest()
- rospy.spin()
-
diff --git a/catkin_ws/src/40-coordination/led_interpreter/launch/LED_interpreter.launch b/catkin_ws/src/40-coordination/led_interpreter/launch/LED_interpreter.launch
index 2ef3ffee37..bb4d7c5938 100644
--- a/catkin_ws/src/40-coordination/led_interpreter/launch/LED_interpreter.launch
+++ b/catkin_ws/src/40-coordination/led_interpreter/launch/LED_interpreter.launch
@@ -1,9 +1,7 @@
-
-
@@ -13,7 +11,7 @@
-
+
diff --git a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node.launch b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node.launch
index 4fe114186c..5b68eb984f 100644
--- a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node.launch
+++ b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node.launch
@@ -3,28 +3,17 @@
-
-
-
+
-
-
-
-
-
-
+
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_old.launch b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_old.launch
deleted file mode 100644
index 1cc4e62d18..0000000000
--- a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_old.launch
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_test.launch b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_test.launch
index c95ff04a98..8a2914f0f1 100644
--- a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_test.launch
+++ b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_mapper_node_test.launch
@@ -2,20 +2,11 @@
-
-
-
+
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter.launch b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter.launch
index 7a052858de..0c786ef46b 100644
--- a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter.launch
+++ b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter.launch
@@ -1,44 +1,23 @@
-
-
-
-
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
+
diff --git a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter_test_old.launch b/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter_test_old.launch
deleted file mode 100644
index 4ce8135398..0000000000
--- a/catkin_ws/src/40-coordination/led_joy_mapper/launch/led_joy_with_led_emitter_test_old.launch
+++ /dev/null
@@ -1,29 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/led_emitter/CMakeLists.txt b/catkin_ws/src/40-coordination/led_pattern_switch/CMakeLists.txt
similarity index 88%
rename from catkin_ws/src/40-coordination/led_emitter/CMakeLists.txt
rename to catkin_ws/src/40-coordination/led_pattern_switch/CMakeLists.txt
index 0ead3710bf..950bdfc1d6 100644
--- a/catkin_ws/src/40-coordination/led_emitter/CMakeLists.txt
+++ b/catkin_ws/src/40-coordination/led_pattern_switch/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
-project(led_emitter)
+project(led_pattern_switch)
find_package(catkin REQUIRED COMPONENTS
diff --git a/catkin_ws/src/40-coordination/led_emitter/launch/led_pattern_switch_node.launch b/catkin_ws/src/40-coordination/led_pattern_switch/launch/led_pattern_switch_node.launch
similarity index 55%
rename from catkin_ws/src/40-coordination/led_emitter/launch/led_pattern_switch_node.launch
rename to catkin_ws/src/40-coordination/led_pattern_switch/launch/led_pattern_switch_node.launch
index 4a58b50737..3e8e1bef33 100644
--- a/catkin_ws/src/40-coordination/led_emitter/launch/led_pattern_switch_node.launch
+++ b/catkin_ws/src/40-coordination/led_pattern_switch/launch/led_pattern_switch_node.launch
@@ -1,18 +1,12 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/40-coordination/led_emitter/package.xml b/catkin_ws/src/40-coordination/led_pattern_switch/package.xml
similarity index 79%
rename from catkin_ws/src/40-coordination/led_emitter/package.xml
rename to catkin_ws/src/40-coordination/led_pattern_switch/package.xml
index a7ba72f572..81d6c5d97e 100644
--- a/catkin_ws/src/40-coordination/led_emitter/package.xml
+++ b/catkin_ws/src/40-coordination/led_pattern_switch/package.xml
@@ -1,8 +1,8 @@
- led_emitter
+ led_pattern_switch
1.0.0
- TODO: description for `led_emitter` package.
+ Does something related to changing the LED patters based on FSM state
Andrea Censi
diff --git a/catkin_ws/src/40-coordination/led_emitter/src/led_pattern_switch_node.py b/catkin_ws/src/40-coordination/led_pattern_switch/src/led_pattern_switch_node.py
similarity index 100%
rename from catkin_ws/src/40-coordination/led_emitter/src/led_pattern_switch_node.py
rename to catkin_ws/src/40-coordination/led_pattern_switch/src/led_pattern_switch_node.py
diff --git a/catkin_ws/src/40-coordination/rgb_led/include/rgb_led/rgb_led.py b/catkin_ws/src/40-coordination/rgb_led/include/rgb_led/rgb_led.py
index 42a7caee75..20c3a75477 100644
--- a/catkin_ws/src/40-coordination/rgb_led/include/rgb_led/rgb_led.py
+++ b/catkin_ws/src/40-coordination/rgb_led/include/rgb_led/rgb_led.py
@@ -13,16 +13,16 @@
# from math import fabs, floor
class RGB_LED():
- #
- # LED_REAR_LEFT = 0
- # LED_REAR_RIGH = 3
- # LED_TOP = 6
- # LED_FRONT_LEFT = 9
- # LED_FRONT_RIGHT = 12
-
- OFFSET_RED = 0
+#
+# LED_REAR_LEFT = 0
+# LED_REAR_RIGH = 3
+# LED_TOP = 6
+# LED_FRONT_LEFT = 9
+# LED_FRONT_RIGHT = 12
+
+ OFFSET_RED = 0
OFFSET_GREEN = 1
- OFFSET_BLUE = 2
+ OFFSET_BLUE = 2
def __init__(self, debug=False):
from Adafruit_PWM_Servo_Driver import PWM # @UnresolvedImport
@@ -40,9 +40,9 @@ def setRGBint24(self, led, color):
self.setRGBvint8(led, [r, g, b])
def setRGBvint8(self, led, color):
- self.setLEDBrightness(led, self.OFFSET_RED, color[0])
+ self.setLEDBrightness(led, self.OFFSET_RED , color[0])
self.setLEDBrightness(led, self.OFFSET_GREEN, color[1])
- self.setLEDBrightness(led, self.OFFSET_BLUE, color[2])
+ self.setLEDBrightness(led, self.OFFSET_BLUE , color[2])
def setRGB(self, led, color):
self.setRGBvint8(led, map(lambda f: int(f * 255), color))
diff --git a/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch b/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch
index 05006d064e..3bf7b72e77 100644
--- a/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch
+++ b/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch
@@ -1,6 +1,5 @@
-
@@ -10,17 +9,10 @@
-
-
+
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch.old b/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch.old
deleted file mode 100644
index dbaa7cdb5f..0000000000
--- a/catkin_ws/src/40-coordination/traffic_light/launch/traffic_light_node.launch.old
+++ /dev/null
@@ -1,25 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/catkin_ws/src/40-coordination/traffic_light/src/traffic_light_node.py b/catkin_ws/src/40-coordination/traffic_light/src/traffic_light_node.py
index 0b6b876adb..9df64cf4cd 100755
--- a/catkin_ws/src/40-coordination/traffic_light/src/traffic_light_node.py
+++ b/catkin_ws/src/40-coordination/traffic_light/src/traffic_light_node.py
@@ -8,97 +8,79 @@
class TrafficLight(object):
-
def __init__(self):
-
- # Hardcoded color values to configure traffic light
- # ATTENTION: This skript uses GRB instead of RGB logic to work with the
- # newest version of the traffic lights with surface mounted LEDs.
- self.green_color = [1, 0, 0]
- self.red_color = [0, 1, 0]
- self.yellow_color = [1, 1, 0]
- self.black_color = [0, 0, 0]
-
self.led = RGB_LED()
self.node_name = rospy.get_name()
self.cycle = None
- self.protocol = self.setupParameter(
- "~LED_protocol", []) # should be a list of tuples
+ self.protocol =self.setupParameter("~LED_protocol",[]) #should be a list of tuples
self.greenlight_freq = self.protocol['signals']['traffic_light_go']['frequency']
self.redlight_freq = self.protocol['signals']['traffic_light_stop']['frequency']
- self.traffic_light_list = self.setupParameter(
- "~traffic_light_list", [0, 2, 3, 4]) # order of lights
- self.greenlight_duration = self.setupParameter(
- "~greenlight_duration", 5) # in seconds
- self.allred_duration = self.setupParameter(
- "~allred_duration", 4) # in seconds
+ self.traffic_light_list = self.setupParameter("~traffic_light_list",[0,2,3,1]); #order of lights
+ self.greenlight_duration = self.setupParameter("~greenlight_duration",5) #in seconds
+ self.allred_duration = self.setupParameter("~allred_duration",4) #in seconds
self.redlight_t = 1.0/self.redlight_freq
self.greenlight_t = 1.0/self.greenlight_freq
- self.green_on = False
- self.green_i = 0
+ self.green_on=False
+ self.green_i = 0;
self.green = self.traffic_light_list[self.green_i]
- self.redlightlist = self.traffic_light_list[:self.green_i] + \
- self.traffic_light_list[(self.green_i+1):]
- self.traffic_light_state = {
- 0: False, 4: False, 2: False, 3: False} # All LEDs are off
+ self.green_color = [0,1,0] #Hardcoded but should be parameter
+ self.yellow_color = [1,0.65,0] #Hardcoded but should be parameter
+ self.redlightlist = self.traffic_light_list[:self.green_i] + self.traffic_light_list[(self.green_i+1):];
+ self.traffic_light_state = {0:False,1:False,2:False,3:False} #All LEDs are off
self.yellowlightlist = []
- self.traffic_cycle = rospy.Timer(rospy.Duration(
- (self.greenlight_duration+self.allred_duration)), self.switchGreen)
- self.redLED_cycle = rospy.Timer(
- rospy.Duration(0.5*self.redlight_t), self.freqred)
- self.greenLED_cycle = rospy.Timer(
- rospy.Duration(0.5*self.greenlight_t), self.freqgreen)
+ self.traffic_cycle = rospy.Timer(rospy.Duration((self.greenlight_duration+self.allred_duration)),self.switchGreen)
+ self.redLED_cycle = rospy.Timer(rospy.Duration(0.5*self.redlight_t),self.freqred)
+ self.greenLED_cycle = rospy.Timer(rospy.Duration(0.5*self.greenlight_t),self.freqgreen)
- def switchGreen(self, event):
- self.yellowlightlist = []
- self.green_i = (self.green_i+1) % 4 # Move to next light in list
+ def switchGreen(self,event):
+ self.yellowlightlist=[]
+ self.green_i = (self.green_i+1)%4 #Move to next light in list
self.green = self.traffic_light_list[self.green_i]
- self.green_on = True
- self.redlightlist = self.traffic_light_list[:self.green_i] + \
- self.traffic_light_list[(self.green_i+1):]
- rospy.sleep(self.greenlight_duration) # Keep the green light on
- self.green_on = False # Turn off the green light
+ self.green_color = [0,1,0]
+ self.green_on=True
+ self.redlightlist = self.traffic_light_list[:self.green_i] + self.traffic_light_list[(self.green_i+1):];
+ rospy.sleep(self.greenlight_duration) #Keep the green light on
+ self.green_on = False #Turn off the green light
self.yellowlightlist = [self.green]
rospy.sleep(self.allred_duration)
self.redlightlist = self.traffic_light_list[0:]
- def freqred(self, event):
+ def freqred(self,event):
for light in self.redlightlist:
- if self.traffic_light_state[light] == True:
- self.led.setRGB(light, self.black_color)
- self.traffic_light_state[light] = False
+ if self.traffic_light_state[light]==True:
+ self.led.setRGB(light,[0,0,0])
+ self.traffic_light_state[light]=False
else:
- self.led.setRGB(light, self.red_color)
- self.traffic_light_state[light] = True
+ self.led.setRGB(light,[1,0,0])
+ self.traffic_light_state[light]=True
for light in self.yellowlightlist:
- if self.traffic_light_state[light] == True:
- self.led.setRGB(light, self.black_color)
- self.traffic_light_state[light] = False
+ if self.traffic_light_state[light]==True:
+ self.led.setRGB(light,[0,0,0])
+ self.traffic_light_state[light]=False
else:
- self.led.setRGB(light, self.yellow_color)
- self.traffic_light_state[light] = True
+ self.led.setRGB(light,[1,1,0])
+ self.traffic_light_state[light]=True
- def freqgreen(self, event):
- if self.green_on == False: # Exit if lights should all be red
+ def freqgreen(self,event):
+ if self.green_on==False: #Exit if lights should all be red
return
- if self.traffic_light_state[self.green] == True:
- self.led.setRGB(self.green, self.black_color)
- self.traffic_light_state[self.green] = False
+ if self.traffic_light_state[self.green]==True:
+ self.led.setRGB(self.green,[0,0,0])
+ self.traffic_light_state[self.green]=False
else:
- self.led.setRGB(self.green, self.green_color)
- self.traffic_light_state[self.green] = True
+ self.led.setRGB(self.green,self.green_color)
+ self.traffic_light_state[self.green]=True
- def setupParameter(self, param_name, default_value):
- value = rospy.get_param(param_name, default_value)
- # Write to parameter server for transparancy
- rospy.set_param(param_name, value)
- rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
+ def setupParameter(self,param_name,default_value):
+ value = rospy.get_param(param_name,default_value)
+ rospy.set_param(param_name,value) #Write to parameter server for transparancy
+ rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
return value
-
if __name__ == '__main__':
- rospy.init_node('traffic_light', anonymous=False)
+ rospy.init_node('traffic_light',anonymous=False)
node = TrafficLight()
rospy.spin()
+
diff --git a/catkin_ws/src/45-communication/tcp_communication/launch/tcp_communication_client.launch b/catkin_ws/src/45-communication/tcp_communication/launch/tcp_communication_client.launch
index f894a31dcf..47e7011368 100644
--- a/catkin_ws/src/45-communication/tcp_communication/launch/tcp_communication_client.launch
+++ b/catkin_ws/src/45-communication/tcp_communication/launch/tcp_communication_client.launch
@@ -1,6 +1,5 @@
-
@@ -8,14 +7,7 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/mdoap_controller_node.launch b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/mdoap_controller_node.launch
index f73ce32752..affffb49c5 100644
--- a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/mdoap_controller_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/mdoap_controller_node.launch
@@ -1,6 +1,5 @@
-
@@ -8,13 +7,7 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/obstacle_safety_node.launch b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/obstacle_safety_node.launch
index 2af54a3e29..eda5019d5c 100644
--- a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/obstacle_safety_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/obstacle_safety_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,17 +8,11 @@
-
-
+
-
-
-
-
-
diff --git a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/simple_stop_controller_node.launch b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/simple_stop_controller_node.launch
index 0b881e2ef7..2df9cf6fc3 100644
--- a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/simple_stop_controller_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/simple_stop_controller_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,15 +8,10 @@
-
-
+
-
-
-
-
diff --git a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/static_object_detector_node.launch b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/static_object_detector_node.launch
index f2be732383..4be32b3eff 100644
--- a/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/static_object_detector_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/mdoap/launch/static_object_detector_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,17 +8,11 @@
-
-
+
-
-
-
-
-
diff --git a/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/intersection_supervisor_node.launch b/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/intersection_supervisor_node.launch
index 7033bf866d..e453ef96f9 100644
--- a/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/intersection_supervisor_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/intersection_supervisor_node.launch
@@ -1,17 +1,11 @@
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/lane_supervisor_node.launch b/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/lane_supervisor_node.launch
index a202e5bf08..dfd4332a24 100644
--- a/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/lane_supervisor_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/parallel_autonomy/launch/lane_supervisor_node.launch
@@ -2,17 +2,10 @@
-
-
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/steering_wheel/launch/steering_wheel_node.launch b/catkin_ws/src/50-misc-additional-functionality/steering_wheel/launch/steering_wheel_node.launch
index 130e3b5e47..22ee2f29d0 100644
--- a/catkin_ws/src/50-misc-additional-functionality/steering_wheel/launch/steering_wheel_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/steering_wheel/launch/steering_wheel_node.launch
@@ -1,6 +1,5 @@
-
@@ -9,15 +8,10 @@
-
-
+
-
-
-
-
diff --git a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_avoidance_control_node.launch b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_avoidance_control_node.launch
index 6fdc50ba24..024355ed73 100644
--- a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_avoidance_control_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_avoidance_control_node.launch
@@ -1,17 +1,12 @@
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_detection_node.launch b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_detection_node.launch
index 7ebbd53bd8..ef62b7308f 100644
--- a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_detection_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_detection_node.launch
@@ -1,17 +1,12 @@
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter.launch b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter.launch
index c1ac1f612f..20383485c5 100644
--- a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter.launch
@@ -1,17 +1,12 @@
-
-
-
-
-
-
+
diff --git a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter_node.launch b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter_node.launch
index 1da3b03018..3daea1ed73 100644
--- a/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter_node.launch
+++ b/catkin_ws/src/50-misc-additional-functionality/vehicle_detection/launch/vehicle_filter_node.launch
@@ -1,13 +1,10 @@
-
-
-
-
+
diff --git a/catkin_ws/src/70-convenience-packages/duckie_rr_bridge/launch/duckie_host.launch b/catkin_ws/src/70-convenience-packages/duckie_rr_bridge/launch/duckie_host.launch
index aff932527d..feb9af9f3f 100644
--- a/catkin_ws/src/70-convenience-packages/duckie_rr_bridge/launch/duckie_host.launch
+++ b/catkin_ws/src/70-convenience-packages/duckie_rr_bridge/launch/duckie_host.launch
@@ -3,16 +3,11 @@
-
-
-
+
-
-
-
diff --git a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/formula_D.launch b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/formula_D.launch
index 32bb31598d..49d577e938 100644
--- a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/formula_D.launch
+++ b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/formula_D.launch
@@ -1,12 +1,9 @@
-
-
-
@@ -17,7 +14,6 @@
-
diff --git a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/indefinite_navigation.launch b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/indefinite_navigation.launch
index 0c5c7d58de..7ae399ab98 100644
--- a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/indefinite_navigation.launch
+++ b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/indefinite_navigation.launch
@@ -1,32 +1,23 @@
+
+
-
-
-
-
-
-
+
+
-
-
-
-
-
-
-
-
-
-
-
+
+
+
-
-
+
+
+
diff --git a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/joystick.launch b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/joystick.launch
index 927a261cf6..d783328695 100644
--- a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/joystick.launch
+++ b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/joystick.launch
@@ -1,23 +1,17 @@
-
-
-
-
-
-
+
-
@@ -27,7 +21,6 @@
-
@@ -35,7 +28,6 @@
-
@@ -43,7 +35,6 @@
-
diff --git a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/master.launch b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/master.launch
index 36d4a9bfe6..a29f78548f 100644
--- a/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/master.launch
+++ b/catkin_ws/src/70-convenience-packages/duckietown_demos/launch/master.launch
@@ -35,7 +35,6 @@
-
@@ -69,11 +68,9 @@
-
-
@@ -86,7 +83,6 @@
-
@@ -112,7 +108,6 @@
-
@@ -141,11 +136,6 @@
-
-
-
-
-
@@ -168,7 +158,6 @@
-
@@ -176,7 +165,6 @@
-
@@ -190,7 +178,6 @@
-
@@ -208,10 +195,6 @@
-
-
-
-
@@ -237,12 +220,6 @@
-
-
-
-
-
-
@@ -439,12 +416,10 @@
-
-
@@ -478,7 +453,6 @@
-
@@ -486,7 +460,6 @@
-
@@ -503,7 +476,6 @@
-
@@ -525,16 +497,6 @@