class IRobotProcessing():
def __init__(self):
self.__vr = 0
self.__vl = 0
self.bot = Robot() # Open the robot
def setVR(self, vr):
self.__vr = vr # Setup the speed of right wheel
def setVL(self, vl):
self.__vl = vl # Setup the speed of left wheel
def getVR(self):
return self.__vr
def getVL(self):
return self.__vl
# Let the robot move by this class data
def run(self):
self.bot.setWheelSpeed(self.__vr, self.__vl)
# Let the robot move by the other class argument
def move(self, vr, vl):
self.bot.setWheelSpeed(vr, vl)
# Turn off the wheel motors.
def stop(self):
self.bot.setWheelSpeed(0, 0)
# Turn off the robot
def close(self):
self.bot.close()
# reset all the internal data of the robot.
def reset(self):
self.bot.reset()
def getDistanceAngle(self, pre_right_encoder, pre_left_encoder):
right_encoder, left_encoder = self.getEncoder()
difference_right = right_encoder - pre_right_encoder
difference_left = left_encoder - pre_left_encoder
# right_distance = difference_right / 508.8 * np.pi * 72
# left_distance = difference_left / 508.8 * np.pi * 72
# Formula for iRobot Create2:
# Distance: (current encoder - pre encodeer) / 508.8 * pi * 72
# Angle: (right distance - left ddstance)/ 235.0
# Note: When the encoder counted to 65536 (2^16), the encoder will count back to O
if difference_right > 60000:
right_distance = ((right_encoder - 65536) -
pre_right_encoder) / 508.8 * np.pi * 72
elif difference_right < -60000:
right_distance = ((65536 - pre_right_encoder) +
right_encoder) / 508.8 * np.pi * 72
else:
right_distance = difference_right / 508.8 * np.pi * 72
if difference_left > 60000:
left_distance = (
(left_encoder - 65536) - pre_left_encoder) / 508.8 * np.pi * 72
elif difference_left < -60000:
left_distance = (
(65536 - pre_left_encoder) + left_encoder) / 508.8 * np.pi * 72
else:
left_distance = difference_left / 508.8 * np.pi * 72
angle = math.degrees((right_distance - left_distance) / 235.0)
distance = (right_distance + left_distance) / 2
return distance, angle, right_distance, left_distance, right_encoder, left_encoder
def getEncoder(self):
right_encoder, left_encoder = self.bot.getEncoderCounts()
'''
if vr < 0:
right_encoder = 65536 - right_encoder
if vl < 0:
left_encoder = 65536 - left_encoder
'''
print "right encoder: " + str(right_encoder)
print "left encoder: " + str(left_encoder)
return right_encoder, left_encoder
def lineFollowing(self, status, expectation_distance, pre_cx, cx,
rotation_direction, small_distance, angle):
# status: check whether there are horizontal lines in images and the robot close to the node(corner). 1 means both of them are ture.
# expectation_distance: The distance the robot need to move when close the the node.
# pre_cx: the pre center of the contour
# cx: the current center of the contour
# small_distance: use to compare with the expectation_distance
if status == 1:
if small_distance < expectation_distance:
self.setVR(15)
self.setVL(15)
self.run()
else:
self.setVR(0)
self.setVL(0)
self.run()
status = 0
'''
if angle >= 85 or angle < -85:
status = 0
distance = 0
angle = 0
'''
else:
if cx == -1:
print 'Out of track'
'''
if rotation_direction < 0 or pre_cx < 320:
self.setVR(15)
self.setVL(-15)
self.run()
elif rotation_direction > 0 or pre_cx > 320:
self.setVR(-15)
self.setVL(15)
self.run()
else:
self.setVR(15)
self.setVL(15)
self.run()
'''
if angle > 0:
self.setVR(15)
self.setVL(-15)
self.run()
else:
self.setVR(-15)
self.setVL(15)
self.run()
else:
if cx >= 340 and cx < 440:
self.setVR(5)
self.setVL(20)
self.run()
print 'Turning right'
# time.sleep(3)
# bot.setTurnSpeed(0)
# time.sleep(2)
elif cx >= 440:
self.setVR(5)
self.setVL(25)
self.run()
print "Turning right"
elif cx < 340 and cx > 300:
if rotation_direction > 0:
self.setVR(20)
self.setVL(5)
self.run()
elif rotation_direction < 0:
self.setVR(5)
self.setVL(25)
self.run()
else:
self.setVR(40)
self.setVL(40)
self.run()
print 'Going forward'
# time.sleep(1)
# bot.setTurnSpeed(0)
# time.sleep(2)
elif cx <= 300 and cx > 200:
self.setVR(20)
self.setVL(5)
self.run()
print 'Turning left'
# time.sleep(3)
# bot.setTurnSpeed(0)
# time.sleep(2)
elif cx <= 200:
self.setVR(25)
self.setVL(5)
self.run()
print "Turning left"
if cx != -1:
return cx, status
else:
return pre_cx, status
THE SOFTWARE.
'''
from breezycreate2 import Robot
import time
import numpy as np
import math
# Create a Create2. This will automatically try to connect to your robot over serial
bot = Robot()
# Play a note to let us know you're alive!
# +bot.playNote('A4', 100)
# Tell the Create2 to turn right slowly
pre_right_encoder, pre_left_encoder = bot.getEncoderCounts()
print pre_right_encoder, pre_left_encoder
# bot.setTurnSpeed(-20)
vl = 30
vr = 30
bot.setWheelSpeed(vr, vl)
time.sleep(5)
# bot.setTurnSpeed(20)
# time.sleep(2)
# bot.setForwardSpeed(200)
# time.sleep(5)
# distance = bot.getDistance()
# distance, angle = bot.getDisAng()