diff --git a/Software/Python/Examples/Line_Sensor/README.md b/Software/Python/Examples/Line_Sensor/README.md new file mode 100644 index 00000000..fdb3eaa0 --- /dev/null +++ b/Software/Python/Examples/Line_Sensor/README.md @@ -0,0 +1,13 @@ +# LINE FOLLOWER SENSOR + +These files showcase how to use the line follower with a GoPiGo3. + +For these files to work, the DI Sensors drivers must first be installed. On Raspbian for Robots they are pre-installed already. + +## Installation +If you need to install the DI Sensors repo, this is how to do it: + +`curl -kL dexterindustries.com/update_sensors | bash` + +## More +See more at https://github.com/DexterInd/DI_Sensors \ No newline at end of file diff --git a/Software/Python/Examples/Line_Sensor/__init__.py b/Software/Python/Examples/Line_Sensor/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/Software/Python/Examples/Line_Sensor/basic_example.py b/Software/Python/Examples/Line_Sensor/basic_example.py index 16d52d1f..95ae122e 100755 --- a/Software/Python/Examples/Line_Sensor/basic_example.py +++ b/Software/Python/Examples/Line_Sensor/basic_example.py @@ -28,13 +28,12 @@ ''' from __future__ import print_function from __future__ import division -from builtins import input # the above lines are meant for Python3 compatibility. # they force the use of Python3 functionality for print(), # the integer division and input() # mind your parentheses! -import line_sensor +from line_follower import line_sensor import time def get_sensorval(): diff --git a/Software/Python/Examples/Line_Sensor/line_follower-basic_example.py b/Software/Python/Examples/Line_Sensor/line_follower-basic_example.py index 1606926b..6a69dc19 100755 --- a/Software/Python/Examples/Line_Sensor/line_follower-basic_example.py +++ b/Software/Python/Examples/Line_Sensor/line_follower-basic_example.py @@ -28,24 +28,37 @@ ''' from __future__ import print_function from __future__ import division -from builtins import input # the above lines are meant for Python3 compatibility. # they force the use of Python3 functionality for print(), # the integer division and input() # mind your parentheses! -import line_sensor +import easygopigo3 as easy import time -def get_sensorval(): - while True: - val=line_sensor.read_sensor() - if val[0]!=-1: - return val - #else: - #Read once more to clear buffer and remove junk values - # val=line_sensor.read_sensor() -while True: - l0,l1,l2,l3,l4=get_sensorval() - print (l0,l1,l2,l3,l4) - #time.sleep(.05) +sensor_readings = None + +gpg = easy.EasyGoPiGo3() + +try: + my_linefollower = gpg.init_line_follower() + time.sleep(0.1) +except: + print('Line Follower not responding') + time.sleep(0.2) + exit() +my_linefollower.read_position() +my_linefollower.read_position() + + +# start +gpg.forward() +while not my_linefollower.read_position() == "black": + if my_linefollower.read_position() == 'center': + gpg.forward() + if my_linefollower.read_position() == 'left': + gpg.left() + if my_linefollower.read_position() == 'right': + gpg.right() +gpg.stop() + diff --git a/Software/Python/Examples/Line_Sensor/line_sensor.py b/Software/Python/Examples/Line_Sensor/line_sensor.py deleted file mode 100755 index 091e5a31..00000000 --- a/Software/Python/Examples/Line_Sensor/line_sensor.py +++ /dev/null @@ -1,225 +0,0 @@ -#!/usr/bin/env python -# Dexter Industries line sensor python library -# -# This library provides the basic functions to access the sensor data from the line sensor -# -# Have a question about this example? Ask on the forums here: http://forum.dexterindustries.com/c/gopigo -# -# Karan Nayan -# Initial Date: 13 Dec 2015 -# Last Updated: 05 Apr 2017 -# http://www.dexterindustries.com/ -''' -## License - Copyright (C) 2017 Dexter Industries - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -''' -from __future__ import print_function -#from __future__ import division -from builtins import input -# the above lines are meant for Python3 compatibility. -# they force the use of Python3 functionality for print(), -# the integer division and input() -# mind your parentheses! - - -import smbus -import time -import math -import RPi.GPIO as GPIO -import struct -import operator -import pickle - -debug =0 - -rev = GPIO.RPI_REVISION -if rev == 2 or rev == 3: - bus = smbus.SMBus(1) -else: - bus = smbus.SMBus(0) - -# I2C Address of Arduino -address = 0x06 - -# Command Format -# analogRead() command format header -aRead_cmd = [3] - -# This allows us to be more specific about which commands contain unused bytes -unused = 0 - -#Add a multipler to each sensor -multp=[-10,-5,0,5,10] - -#TRAIN for the first time -#reading when all sensors are at white -# white=[767,815,859,710,700] -white_line=[0]*5 -#reading when all sensors are black -# black=[1012,1013,1015,1003,1004] -black_line=[0]*5 -#difference between black and white -# range_col=list(map(operator.sub, black, white)) -range_col=[0]*5 - -dir_path="/home/pi/Dexter/" -file_b=dir_path+'black_line.txt' -file_w=dir_path+'white_line.txt' -file_r=dir_path+'range_line.txt' -# Function declarations of the various functions used for encoding and sending -# data from RPi to Arduino - -# Write I2C block -def write_i2c_block(address, block): - try: - return bus.write_i2c_block_data(address, 1, block) - except IOError: - if debug: - print ("IOError") - return -1 - - -def read_sensor(): - try: - #if sensor>=0 and sensor <=4: - bus.write_i2c_block_data(address, 1, aRead_cmd + [unused, unused, unused]) - #time.sleep(.1) - #bus.read_byte(address) - number = bus.read_i2c_block_data(address, 1) - #time.sleep(.05) - return number[0]* 256 + number[1],number[2]* 256 + number[3],number[4]* 256 + number[5],number[6]* 256 + number[7],number[8]* 256 + number[9] - - #return number[0]* 256 + number[1] - - time.sleep(.05) - except IOError: - return -1,-1,-1,-1,-1 - - -def get_sensorval(): - - # updated to avoid an infinite loop - attempt = 0 - while attempt < 5: - val=read_sensor() - # print (val) - if val[0]!=-1: - return val - else: - #Read once more to clear buffer and remove junk values - val=read_sensor() - attempt = attempt + 1 - - return val - - -def set_black_line(): - global black_line,white_line,range_col - for i in range(5): - val=read_sensor() - # print (val) - if val[0]!=-1: - black_line=val - else: - black_line=[-1]*5 - range_col=list(map(operator.sub, black_line, white_line)) - with open(file_b, 'wb') as f: - pickle.dump(black_line, f) - with open(file_r, 'wb') as f: - pickle.dump(range_col, f) - - -def get_black_line(): - global black_line - #load default values from files - try: - with open(file_b, 'rb') as f: - black_line = pickle.load(f) - except Exception as e: - print ("Line Follower: failed getting black line values!") - print (e) - black_line=[0]*5 - return black_line - - -def set_white_line(): - global white_line,black_line,range_col - for i in range(5): - val=read_sensor() - # print (val) - if val[0]!=-1: - white_line=val - else: - white_line=[-1]*5 - range_col=list(map(operator.sub, black_line, white_line)) - with open(file_w, 'wb') as f: - pickle.dump(white_line, f) - with open(file_r, 'wb') as f: - pickle.dump(range_col, f) - - -def get_white_line(): - global white_line - #load default values from files - try: - with open(file_w, 'rb') as f: - white_line = pickle.load(f) - except: - white_line=[0]*5 - return white_line - - -def get_range(): - global range_col - #load default values from files - try: - with open(file_r, 'rb') as f: - range_col = pickle.load(f) - except: - range_col=[0]*5 - return range_col - - -def line_position(): - global black_line,white_line,range_col - #load default values from files - try: - with open(file_b, 'rb') as f: - black_line = pickle.load(f) - except: - black_line=[0]*5 - - try: - with open(file_w, 'rb') as f: - white_line = pickle.load(f) - except: - white_line=[0]*5 - - try: - with open(file_r, 'rb') as f: - range_col = pickle.load(f) - except: - range_col=[0]*5 - - curr=get_sensorval() - diff_val=list(map(operator.sub, curr, white_line)) - curr_pos=0 - percent_black_line=[0]*5 - for i in range(5): - # casting to int when moving to Python 3 - percent_black_line[i]=(diff_val[i]*100/range_col[i]) - curr_pos+=percent_black_line[i]*multp[i] - return curr_pos diff --git a/Software/Python/easygopigo3.py b/Software/Python/easygopigo3.py index 15cf0d4e..a05b6f74 100644 --- a/Software/Python/easygopigo3.py +++ b/Software/Python/easygopigo3.py @@ -8,6 +8,7 @@ # import select import time import os +import math from I2C_mutex import Mutex import easysensors try: @@ -183,48 +184,6 @@ def stop(self): """ self.set_motor_dps(self.MOTOR_LEFT + self.MOTOR_RIGHT, 0) - - def backward(self): - """ - Move the `GoPiGo3`_ backward. - - For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. - Default ``speed`` is set to ``300`` - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. - - """ - self.set_motor_dps(self.MOTOR_LEFT + self.MOTOR_RIGHT, - self.get_speed() * -1) - - def right(self): - """ - Move the `GoPiGo3`_ to the right. - - For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. - Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. - - .. important:: - | The robot will activate only the left motor, whilst the right motor will be completely stopped. - | This causes the robot to rotate in very short circles. - - """ - self.set_motor_dps(self.MOTOR_LEFT, self.get_speed()) - self.set_motor_dps(self.MOTOR_RIGHT, 0) - - def left(self): - """ - Move the `GoPiGo3`_ to the left. - - For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. - Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. - - .. important:: - | The robot will activate only the right motor, whilst the left motor will be completely stopped. - | This causes the robot to rotate in very short circles. - - """ - self.set_motor_dps(self.MOTOR_LEFT, 0) - self.set_motor_dps(self.MOTOR_RIGHT, self.get_speed()) - def forward(self): """ Move the `GoPiGo3`_ forward. @@ -347,6 +306,178 @@ def drive_degrees(self, degrees, blocking=True): time.sleep(0.1) return + + def backward(self): + """ + Move the `GoPiGo3`_ backward. + + For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. + Default ``speed`` is set to ``300`` - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. + + """ + self.set_motor_dps(self.MOTOR_LEFT + self.MOTOR_RIGHT, + self.get_speed() * -1) + + def right(self): + """ + Move the `GoPiGo3`_ to the right. + + For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. + Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. + + .. important:: + | The robot will activate only the left motor, whilst the right motor will be completely stopped. + | This causes the robot to rotate in very short circles. + + """ + self.set_motor_dps(self.MOTOR_LEFT, self.get_speed()) + self.set_motor_dps(self.MOTOR_RIGHT, 0) + + def spin_right(self): + """ + Rotate the `GoPiGo3` towards the right while staying on the same spot. + + This differs from the :py:meth:`~easygopigo3.EasyGoPiGo3.right` method as both wheels will be rotating but in different directions. + This causes the robot to spin in place, as if doing a pirouette. + + For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. + Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. + + .. important:: + You can achieve the same effect by calling ``steer(100, -100)`` (method :py:meth:`~easygopigo3.EasyGoPiGo3.steer`). + + """ + self.set_motor_dps(self.MOTOR_LEFT, self.get_speed()) + self.set_motor_dps(self.MOTOR_RIGHT, self.get_speed()* -1) + + def left(self): + """ + Move the `GoPiGo3`_ to the left. + + For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. + Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. + + .. important:: + | The robot will activate only the right motor, whilst the left motor will be completely stopped. + | This causes the robot to rotate in very short circles. + + """ + self.set_motor_dps(self.MOTOR_LEFT, 0) + self.set_motor_dps(self.MOTOR_RIGHT, self.get_speed()) + + def spin_left(self): + """ + Rotate the `GoPiGo3` towards the left while staying on the same spot. + + This differs from the :py:meth:`~easygopigo3.EasyGoPiGo3.left` method as both wheels will be rotating but in different directions. + This causes the robot to spin in place, as if doing a pirouette. + + For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. + Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. + + .. important:: + You can achieve the same effect by calling ``steer(-100, 100)`` (method :py:meth:`~easygopigo3.EasyGoPiGo3.steer`). + + """ + self.set_motor_dps(self.MOTOR_LEFT, self.get_speed() * -1) + self.set_motor_dps(self.MOTOR_RIGHT, self.get_speed() ) + + def steer(self, left_percent, right_percent): + """ + Control each motor in order to get a variety of turning movements. + + Each motor is assigned a percentage of the current speed value. + While there is no limit on the values of ``left_percent`` and ``right_percent`` parameters, + they are expected to be between **-100** and **100**. + + :param int left_percent: Percentage of current speed value that gets applied to left motor. The range is between **-100** (for backward rotation) and **100** (for forward rotation). + :param int right_percent: Percentage of current speed value that gets applied to right motor. The range is between **-100** (for backward rotation) and **100** (for forward rotation). + + For setting the motor speed, use :py:meth:`~easygopigo3.EasyGoPiGo3.set_speed`. + Default ``speed`` is set to **300** - see :py:meth:`~easygopigo3.EasyGoPiGo3.__init__`. + + .. important:: + Setting both ``left_percent`` and ``right_percent`` parameters to **100** will result in the same behavior as the :py:meth:`~easygopigo3.EasyGoPiGo3.forward` method. + The other behavior for :py:meth:`~easygopigo3.EasyGoPiGo3.backward` method will be experienced if both parameters are set to **-100**. + + Setting both ``left_percent`` and ``right_percent`` to **0** will stop the GoPiGo from moving. + + """ + self.set_motor_dps(self.MOTOR_LEFT, self.get_speed() * left_percent / 100) + self.set_motor_dps(self.MOTOR_RIGHT, self.get_speed() * right_percent / 100) + + + + def orbit(self, degrees, radius_cm=0, blocking=True): + """ + Control the GoPiGo so it will orbit around an object + + :param int degrees: Degrees to steer. **360** for full rotation. Negative for left turn. + :param int radius_cm: Radius in `cm` of the circle to drive. Default is **0** (turn in place). + :param boolean blocking = True: Set it as a blocking or non-blocking method. + """ + speed = self.get_speed() + radius = radius_cm * 10 + + # the total distance to drive in mm + drive_distance = math.pi * abs(radius) * abs(degrees) / 180 # / 180 is shorter than radius * 2 / 360 + + # the distance in mm to add to one motor and subtract from the other + drive_difference = ((self.WHEEL_BASE_CIRCUMFERENCE * degrees) / 360) + + # the number of degrees each wheel needs to turn on average to get the necessary distance + distance_degrees = ((drive_distance / self.WHEEL_CIRCUMFERENCE) * 360) + + # the difference between motor travel in degrees + difference_degrees = ((drive_difference / self.WHEEL_CIRCUMFERENCE) * 360) + + # the distance each wheel needs to turn + left_target = (distance_degrees + difference_degrees) + right_target = (distance_degrees - difference_degrees) + + # if it's a left turn + if degrees < 0: + MOTOR_FAST = self.MOTOR_RIGHT + MOTOR_SLOW = self.MOTOR_LEFT + fast_target = right_target + slow_target = left_target + else: + MOTOR_FAST = self.MOTOR_LEFT + MOTOR_SLOW = self.MOTOR_RIGHT + fast_target = left_target + slow_target = right_target + + # determine driving direction from the speed + direction = 1 + if speed < 0: + direction = -1 + speed = -speed + + # calculate the motor speed for each motor + fast_speed = speed + slow_speed = abs((speed * slow_target) / fast_target) + + # set the motor speeds + self.set_motor_limits(MOTOR_FAST, dps = fast_speed) + self.set_motor_limits(MOTOR_SLOW, dps = slow_speed) + + # get the starting position of each motor + StartPositionLeft = self.get_motor_encoder(self.MOTOR_LEFT) + StartPositionRight = self.get_motor_encoder(self.MOTOR_RIGHT) + + # Set each motor target position + self.set_motor_position(self.MOTOR_LEFT, (StartPositionLeft + (left_target * direction))) + self.set_motor_position(self.MOTOR_RIGHT, (StartPositionRight + (right_target * direction))) + + if blocking: + while self.target_reached( + StartPositionLeft + (left_target * direction), + StartPositionRight + (right_target * direction)) is False: + time.sleep(0.1) + + return + + def target_reached(self, left_target_degrees, right_target_degrees): """ Checks if (*wheels have rotated for a given number of degrees*): diff --git a/Software/Python/easysensors.py b/Software/Python/easysensors.py index 51386bbb..1e0e977f 100644 --- a/Software/Python/easysensors.py +++ b/Software/Python/easysensors.py @@ -3,6 +3,7 @@ # needed for duck typing import gopigo3 +import operator mutex = Mutex() @@ -29,7 +30,6 @@ def debug(in_str): try: from line_follower import line_sensor - from line_follower import scratch_line # is_line_follower_accessible not really used, just in case is_line_follower_accessible = True @@ -37,7 +37,6 @@ def debug(in_str): try: sys.path.insert(0, '/home/pi/GoPiGo/Software/Python/line_follower') import line_sensor - import scratch_line is_line_follower_accessible = True except: is_line_follower_accessible = False @@ -1772,9 +1771,16 @@ def __init__(self, port="I2C", gpg=None, use_mutex=False): Sensor.__init__(self, port, "INPUT", gpg, use_mutex) if line_sensor.read_sensor() == [-1, -1, -1, -1, -1]: raise IOError("Line Follower not responding") + self.white_line = line_sensor.get_white_line() + self.black_line = line_sensor.get_black_line() + self._calculate_threshold() except: raise + def _calculate_threshold(self): + self.range_sensor = list(map(operator.sub, self.black_line, self.white_line)) + self.threshold = [a+b/2 for a,b in zip(self.white_line, self.range_sensor)] + def read_raw_sensors(self): """ Read the 5 IR sensors of the `Line Follower`_ sensor. @@ -1802,7 +1808,9 @@ def get_white_calibration(self): Also, for fully calibrating the sensor, the :py:class:`~easysensors.LineFollower.get_black_calibration` method also needs to be called. """ - return line_sensor.get_white_line() + self.white_line = line_sensor.get_white_line() + self._calculate_threshold() + return self.white_line def get_black_calibration(self): """ @@ -1815,7 +1823,9 @@ def get_black_calibration(self): Also, for fully calibrating the sensor, the :py:class:`~easysensors.LineFollower.get_white_calibration` method also needs to be called. """ - return line_sensor.get_black_line() + self.black_line = line_sensor.get_black_line() + self._calculate_threshold() + return self.black_line def read(self): """ @@ -1830,7 +1840,17 @@ def read(self): Please use :py:meth:`~easysensors.LineFollower.get_black_calibration` or :py:meth:`~easysensors.LineFollower.get_white_calibration` methods before calling this method. """ - five_vals = scratch_line.absolute_line_pos() + raw_vals = line_sensor.get_sensorval() + five_vals = [0]*5 + for i in range(5): + if raw_vals[i] == -1: + five_vals[i] = -1 + elif raw_vals[i] > self.threshold[i]: + five_vals[i] = 1 + else: + five_vals[i] = 0 + # reverse the readings so the one on the left shows up at the left + five_vals = five_vals[::-1] return five_vals diff --git a/docs/source/api-basic/structure.rst b/docs/source/api-basic/structure.rst index 6ba3594a..aa8ff375 100644 --- a/docs/source/api-basic/structure.rst +++ b/docs/source/api-basic/structure.rst @@ -116,7 +116,11 @@ GoPiGo3 Movement easygopigo3.EasyGoPiGo3.stop easygopigo3.EasyGoPiGo3.backward easygopigo3.EasyGoPiGo3.right + easygopigo3.EasyGoPiGo3.spin_right easygopigo3.EasyGoPiGo3.left + easygopigo3.EasyGoPiGo3.spin_left + easygopigo3.EasyGoPiGo3.steer + easygopigo3.EasyGoPiGo3.orbit easygopigo3.EasyGoPiGo3.forward easygopigo3.EasyGoPiGo3.drive_cm easygopigo3.EasyGoPiGo3.drive_inches diff --git a/docs/source/conf.py b/docs/source/conf.py index fa96d782..fc69af51 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -104,7 +104,7 @@ def _check_deps(): # General information about the project. project = u'GoPiGo3' -copyright = u'2017, Dexter Industries' +copyright = u'2018, Dexter Industries' author = u'Robert Lucian Chiriac' # The version info for the project you're documenting, acts as replacement for