def gotoXY(self, x, y, speed=30, block=True):
''' Move to absolute position (x,y).
moves axes simultaneously.
homeXY must be called first.
Args:
x: position of the x-axis (mm)
y: position of the y-axis (mm)
speed: the speed of the axes (mm/s)
block (bool): block until move is complete
'''
# start move to x position
self.xMotor.on_to_position(SpeedNativeUnits(speed * self.xRatio),
x * self.xRatio,
brake=False,
block=False)
# start move to y position
self.yMotor.on_to_position(SpeedNativeUnits(speed * self.yRatio),
y * self.yRatio,
brake=False,
block=False)
if block:
# sleep until either move is complete or motors stall
self.xMotor.wait_until_not_moving(self.defaultTimeout)
self.yMotor.wait_until_not_moving(self.defaultTimeout)
def driveStraightForDistance(self,
distance,
speedLeft=None,
speedRight=None,
stopAtEnd=True):
''' Drive for given distance.
Args:
distance: distance to drive in mm
speedLeft: speed in mm/s (left motor)
speedRight: speed in mm/s (right motor)
stopAtEnd (bool): brake at end of movement
'''
if speedLeft == None or speedLeft == 0:
speedLeft = self.defaultSpeed
if speedRight == None or speedRight == 0:
speedRight = self.defaultSpeed
degPerSecLeft = 360 * speedLeft / (pi * self.wheel_diameter)
degPerSecRight = 360 * speedRight / (pi * self.wheel_diameter)
driveTime = StopWatch()
driveTime.start()
self.leftDriveMotor.on(SpeedNativeUnits(degPerSecLeft), block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSecRight), block=False)
while driveTime.value_ms <= abs(distance / speedLeft) * 1000:
sleep(0.02)
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(SpeedPercent(35).get_speed_pct(m), 35)
self.assertEqual(SpeedDPS(300).get_speed_pct(m), 300 / 1050 * 100)
self.assertEqual(SpeedNativeUnits(300).get_speed_pct(m), 300 / 1050 * 100)
self.assertEqual(SpeedDPM(30000).get_speed_pct(m), (30000 / 60) / 1050 * 100)
self.assertEqual(SpeedRPS(2).get_speed_pct(m), 360 * 2 / 1050 * 100)
self.assertEqual(SpeedRPM(100).get_speed_pct(m), (360 * 100 / 60) / 1050 * 100)
def follow_line(self,
kp,
ki,
kd,
side,
speed=SpeedPercent(50),
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
target_light = self.sLS_left
other_light = self.sLS_right
if target_light_intensity is None:
time.sleep(1)
target_light_intensity = self.get_light_intensitiy(side)
buffed = target_light_intensity + 2.5
target_light_intensity_ls = other_light.reflected_light_intensity - 3
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = -speed.to_native_units(self.motor_left)
MAX_SPEED = SpeedNativeUnits(self.max_speed / 2)
switch_locked = False
while follow_for(self, **kwargs):
if args.light_mode == 'ambient':
target = target_light.ambient_light_intensity
other = other_light.ambient_light_intensity
else:
target = target_light.reflected_light_intensity
other = other_light.reflected_light_intensity
print("{:.3f}->{:.3f} | {:.3f}->{:.3f} | {:.3f} <-> {:.3f} | {}".
format(target, target_light_intensity, other,
target_light_intensity_ls, target_light_intensity,
target_light_intensity_ls, follow_left_edge))
if other < buffed and not switch_locked:
follow_left_edge = False
target_light = self.sLS_right
other_light = self.sLS_left
_tmp = target_light_intensity
target_light_intensity = target_light_intensity_ls
target_light_intensity_ls = _tmp
else:
follow_left_edge = True
target_light = self.sLS_left
other_light = self.sLS_right
#if reflected_light_intensity > buffed*2 and follow_left_edge is False and reflected_light_intensity_ls > target_light_intensity_ls*2:
# follow_left_edge = True
# switch_locked = False
error = target_light_intensity - target
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = -((kp * error) + (ki * integral) +
(kd * derivative))
if not follow_left_edge:
turn_native_units *= -1
#if error < last_error:
# speed_native_units -= 2
#elif error > last_error:
# speed_native_units += 5
right_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
left_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
#print(turn_native_units, speed_native_units)
# DEBUG
#print('Error: %s' % error)
#print('reflected_light_intesity: : %s' % reflected_light_intensity)
#print('integral: %s' % integral)
#print('derivative: %s' % derivative)
#print('last_error: %s' % last_error)
#print('turn_native_units: %s' % turn_native_units)
#print('left_speed: %s' % left_speed)
#print('right_speed: %s' % right_speed)
if left_speed > MAX_SPEED:
print("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
if right_speed > MAX_SPEED:
print("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
# Have we lost the line?
if target >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
#self.mMT.stop()
#raise LineFollowErrorLostLine("we lost the line")
pass
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
#self.mMT.on(left_speed, right_speed)
self.mMT.stop()
def followLineDist(self,
distance,
sensor,
sideToFollow,
stopAtEnd,
speed,
gain_multiplier=1):
''' Drive forward following line.
Uses direct control of drive motors and PD control.
Args:
distance: distance to follow line in mm
sensor: colorsensor to use for line following
sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT
stopAtEnd (bool): Stop motors at end of line following
speed: speed in mm/s
gain_multiplier: multiplier for P and D gains
'''
degPerSec = 360 * speed / (pi * self.wheel_diameter)
propGain = 6.0 * gain_multiplier
derivGain = 6 * gain_multiplier
target = (self.blackThreshold + self.whiteThreshold) / 2
print("******* starting line following ********")
# print("error,Pterm,Dterm")
# initialize term for derivative calc
previousError = target - avgReflection(sensor, 2)
i = 0
driveTime = StopWatch()
driveTime.start()
while driveTime.value_ms <= abs(distance / speed) * 1000:
# calc error and proportional term
error = target - avgReflection(sensor, 2)
Pterm = propGain * error
# calc d(error)/d(t) and derivative term
d_error_dt = error - previousError
Dterm = derivGain * d_error_dt
previousError = error
if sideToFollow == LineEdge.RIGHT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
if sideToFollow == LineEdge.LEFT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
# eprint("line following complete")
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
# Play sound when we stop line following
sound.beep()
def follow_line(self,
kp,
ki,
kd,
speed,
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
prox_treshold=50,
**kwargs):
"""
PID line follower
``kp``, ``ki``, and ``kd`` are the PID constants.
``speed`` is the desired speed of the midpoint of the robot
``target_light_intensity`` is the reflected light intensity when the color sensor
is on the edge of the line. If this is None we assume that the color sensor
is on the edge of the line and will take a reading to set this variable.
``follow_left_edge`` determines if we follow the left or right edge of the line
``white`` is the reflected_light_intensity that is used to determine if we have
lost the line
``off_line_count_max`` is how many consecutive times through the loop the
reflected_light_intensity must be greater than ``white`` before we
declare the line lost and raise an exception
``sleep_time`` is how many seconds we sleep on each pass through
the loop. This is to give the robot a chance to react
to the new motor settings. This should be something small such
as 0.01 (10ms).
``follow_for`` is called to determine if we should keep following the
line or stop. This function will be passed ``self`` (the current
``MoveTank`` object). Current supported options are:
- ``follow_for_forever``
- ``follow_for_ms``
``**kwargs`` will be passed to the ``follow_for`` function
Example:
.. code:: python
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
from ev3dev2.sensor.lego import ColorSensor
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.cs = ColorSensor()
try:
# Follow the line for 4500ms
tank.follow_line(
kp=11.3, ki=0.05, kd=3.2,
speed=SpeedPercent(30),
follow_for=follow_for_ms,
ms=4500
)
except Exception:
tank.stop()
raise
"""
assert self.cs, "ColorSensor must be defined"
if target_light_intensity is None:
target_light_intensity = self.cs.reflected_light_intensity
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = speed.to_native_units(self.left_motor)
print(speed_native_units)
MAX_SPEED = SpeedNativeUnits(self.max_speed)
button = Button()
while follow_for(self, **kwargs):
if "down" in button.buttons_pressed:
self.stop()
return
reflected_light_intensity = self.cs.reflected_light_intensity
error = reflected_light_intensity - target_light_intensity
integral = integral + error
derivative = error - last_error
print("target: {:^20} light: {:^20}, deri: {:^5}, integral: {:^5}".
format(target_light_intensity, reflected_light_intensity,
derivative, integral))
if derivative > 6: #or abs(integral) > 100:
self.stop()
print("Wrong way")
return
# self.on_for_degrees(SpeedPercent(-10), SpeedPercent(10), 100)
# integral = 0
# last_error = 0
# derivative = 0
# off_line_count = 0
# time.sleep(1)
# continue
if integral > 150:
self.stop()
print("Too high integral")
integral = -100.0
# We are correcting heading too much to the left, make correction to right
self.on_for_degrees(SpeedPercent(30), SpeedPercent(-30), 150)
self.on_until_target(target_light_intensity)
continue
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd *
derivative)
if not follow_left_edge:
turn_native_units *= -1
left_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
# Is distance to wall too close?
#if self.us.proximity < prox_treshold:
# print("Too close")
# self.stop()
# in_line = reflected_light_intensity >= white
# turn_help = self.get_next_turn()
# # If we are still in line, safe to turn straight away
# if in_line:
# self.on_for_degrees(turn_help[0], turn_help[1], turn_help[2])
# else:
# self.on_for_seconds(SpeedPercent(-10), SpeedPercent(-10), 1)
# self.on_for_degrees(turn_help[0], turn_help[1], turn_help[2])
# # Reset values for new straigth line
# integral = 0
# last_error = 0
# derivative = 0
# off_line_count = 0
# time.sleep(2)
# continue
# Have we lost the line?
if reflected_light_intensity >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
self.stop()
print("Lost line")
raise LineFollowErrorLostLine("we lost the line")
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
self.on(left_speed, right_speed)
self.stop()
