hasFoundSpot = False
straightLine = 0
goRight=True
bookTurns=5
while not hasFoundSpot:
currentColor = cl.value()
if currentColor != C_BLACK and currentColor != C_NO_COLOR and currentColor != C_WHITE and currentColor == SPOT:
hasFoundSpot = checkForSpot(SPOT)
straightLine = 0
elif hasBumpedBook() or isBookNearBy() or straightLine > 3000 or ts.value():
if bookTurns <= 5:
bookTurns = bookTurns + 1
turnRight()
RMC.on(-100)
LMC.on(-100)
sleep(1)
turnLeft()
else:
if backUpThenTurn(SPOT):
hasFoundSpot = True
straightLine = 0
elif isOnBlackLineOrTable(currentColor):
# Turn & go
if backUpThenTurn(SPOT):
hasFoundSpot = True
straightLine = 0
print("usDistCmVal = ", usDistCmVal, " irDistVal = ", irDistVal,
" irHeadVal = ", irHeadVal, " compassVal = ", compassVal
) # print all sensor vals, regardless of which changed
prev_compassVal = compassVal
# if beaconLock is False and we're outside the compass North range, reset cmpGenHeading to False
if compassVal < (compassGoal - 40) or compassVal > (compassGoal +
40):
cmpGenHeading = False
if usDistCmVal < 50:
print("Obstacle Detected! Forward motion stopped.")
if spd > 0:
spd = 0
if pilot_mode < 0:
# stop motors
mL.on(0, brake=False)
mR.on(0, brake=False)
print("Auto-pilot collision avoidance - backing up")
# backup at speed 5 with equal speed applied to left and right motors
spd = -5
# do so for 3 seconds
mL.on(speed=spd, brake=False)
mR.on_for_seconds(speed=spd,
seconds=3,
brake=False,
block=True)
# stop motors
mL.on(0, brake=False)
mR.on(0, brake=False)
print("Auto-pilot collision avoidance - turning")
spd = 5
m.reset()
time.sleep(0.1)
start_time = time.time()
m.stop_action = 'brake'
# m.ramp_up_sp(5000) #### unsupported (on PiStorms only?)
# m._ramp_down_sp(5000) #### unsupported (on PiStorms only?)
print("position_p = ", m.position_p) # powerup default is 7000
print("position_i = ", m.position_i) # powerup default is 0
print("position_d = ", m.position_d) # powerup default is 37500
print("speed_p = ", m.speed_p) # powerup default is 15000
print("speed_i = ", m.speed_i) # powerup default is 300
print("speed_d = ", m.speed_d) # powerup default is 7500
m.on(SpeedDPS(0))
t1 = time.time()
for i in range(0, 910, 10):
# print("START --------> m.on...", round((time.time() - t1), 4))
print(i, " Increment SpeedDPS --------> m.on(SpeedDPS(i)) ",
round((time.time() - t1), 4))
m.on(SpeedDPS(i))
#m.on_to_position(SpeedDPS(100), (i * 360), brake=False, block=True) ### this method FAILS TO BLOCK
#m.run_to_rel_pos(position_sp=(i * 360), speed_sp=600, stop_action="hold") ### no claim of blocking
#m.on_for_degrees(SpeedDPS(900), (i * 360), brake=True, block=True) ### this method FAILS TO BLOCK
# m.on_for_seconds(speed=i, seconds=8, brake=False)
# print("END --------> m.on...")
# #time.sleep(2)
# print("stop_action is ", m.stop_action, round((time.time() - t1), 4))
# print("state is ", m.state, round((time.time() - t1), 4))
print("Finding Next Spot...")
SPOT = HOME
if i == 2:
SPOT = GOAL
i = i - 1
hasFoundSpot = False
while not hasFoundSpot:
currentColor = cl.value()
if currentColor != C_BLACK and currentColor != C_NO_COLOR and currentColor != C_WHITE and currentColor == SPOT:
# Found the spot!
RMC.off()
LMC.off()
RMC.on(25 * POLARITY)
LMC.on(25 * POLARITY)
sleep(0.1)
stopMotors()
# Double Check Sensor
idx = 10
while idx > 0:
currentColor = cl.value()
sleep(0.1)
if currentColor != SPOT:
idx = -2
idx = idx - 1
if idx != -3:
nagHumans()
start_pos = 0
end_pos = 0
start_time = 0.0
end_time = 0.0
initial_power = b1.measured_voltage
mA = LargeMotor(OUTPUT_A)
mB = LargeMotor(OUTPUT_B)
time.sleep(0.1)
mA.reset()
time.sleep(0.1)
start_time = time.time()
print("starting motors")
while (True):
print("ramping motors up")
for i in range(0, 1001, 5):
mA.on(SpeedDPS(i))
mB.on(SpeedDPS(i))
time.sleep(0.05)
print("ramping motors down")
for i in range(1000, 0, -5):
mA.on(SpeedDPS(i))
mB.on(SpeedDPS(i))
time.sleep(0.05)
print("finished full cycle: ", time.time() - start_time)
print("current votage: ", (b1.measured_voltage / 1000000))
print("votage drop since start: ",
((initial_power - b1.measured_voltage) / 1000000))
class R3ptar:
def __init__(self,
turn_motor_port: str = OUTPUT_A,
move_motor_port: str = OUTPUT_B,
scare_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.turn_motor = MediumMotor(address=turn_motor_port)
self.move_motor = LargeMotor(address=move_motor_port)
self.scare_motor = LargeMotor(address=scare_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.noise = Sound()
def keep_driving_by_ir_beacon(self, speed: float = 100):
while True:
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.move_motor.on(speed=-speed, brake=False, block=False)
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=-50, brake=False, block=False)
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=50, brake=False, block=False)
self.move_motor.on(speed=speed, brake=False, block=False)
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=-50, brake=False, block=False)
self.move_motor.on(speed=-speed, brake=False, block=False)
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.turn_motor.on(speed=50, brake=False, block=False)
self.move_motor.on(speed=-speed, brake=False, block=False)
else:
self.turn_motor.off(brake=True)
self.move_motor.off(brake=False)
def bite_by_ir_beacon(self, speed: float = 100):
while True:
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.scare_motor.on_for_seconds(speed=speed,
seconds=1,
brake=True,
block=False)
self.noise.play_file(
wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.scare_motor.on_for_seconds(speed=-speed,
seconds=1,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def run_away_if_chased(self):
while True:
if self.color_sensor.reflected_light_intensity > 30:
self.move_motor.on_for_seconds(speed=50,
seconds=4,
brake=True,
block=False)
for i in range(2):
self.turn_motor.on_for_seconds(speed=50,
seconds=1,
brake=False,
block=True)
self.turn_motor.on_for_seconds(speed=-50,
seconds=1,
brake=False,
block=True)
def bite_if_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.scare_motor.on_for_seconds(speed=100,
seconds=1,
brake=True,
block=False)
self.noise.play_file(
wav_file='/home/robot/sound/Snake hiss.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.scare_motor.on_for_seconds(speed=-10,
seconds=10,
brake=True,
block=True)
def main(self, speed: float = 100):
Process(target=self.bite_by_ir_beacon, daemon=True).start()
Process(target=self.bite_if_touched, daemon=True).start()
Process(target=self.run_away_if_chased, daemon=True).start()
self.keep_driving_by_ir_beacon(speed=speed)
sleep(1) lm.on_for_rotations(50, 3) # 매개 변수 이름을 명시하지 않아도 된다. sleep(1) #endregion #region 2.시간당 지정된 스피드로 rotations 바퀴만큼 회전 lm.on_for_rotations(speed=SpeedDPS(500), rotations=3) # DPS(degrees per second) 초당 500도를 도는 속도로 3바퀴 회전 sleep(1) lm.on_for_rotations(speed=SpeedRPS(1), rotations=3) # RPS(rotations per second) 초당 1 바퀴를 도는 속도로 3바퀴 회전 sleep(1) #endregion #endregion #region break, block # break : True 인 경우 모터가 동작을 완료하면 모터를 고정위치에 붙잡는다. # block : 현재 명령이 완료 될 때까지 프로그램 실행을 일시 중지 #endregion #region 계속 회전하기 => on(speed, brake=True, block=False) lm.on(speed=80, brake=False) # 정격 최고 속도의 45 %의 속도로 계속 회전 sleep(2) lm.off() # 정지 #endregion #region Run a single motor until it stops moving # # 그리퍼의 팔이 바닥까지 내려갈 때 어떤 각도 또는 시간동안 내려가야하는지 정확히 알 수 없다. # # 이 명령은 일반적으로이 on() 명령 다음에 실행된다. # # 움직이는 휠을 잡고 멈 추면 스크립트가 종료된다. lm.on(speed=20) lm.wait_until_not_moving() #endregion
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B large_motor = LargeMotor(OUTPUT_B) large_motor.on(speed=50) large_motor.wait_until_not_moving()
#!/usr/bin/env python3
from time import sleep
from ev3dev2 import list_devices
from ev3dev2.port import LegoPort
from ev3dev2.motor import OUTPUT_A, LargeMotor, SpeedPercent
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import UltrasonicSensor
p1 = LegoPort(INPUT_1)
p1.mode = 'ev3-uart'
p1.set_device = 'lego-ev3-us'
sleep(0.5)
s = UltrasonicSensor(INPUT_1)
m = LargeMotor(OUTPUT_A)
print("Running motor...")
while True:
dist = s.distance_centimeters
if dist < 50:
m.on(SpeedPercent(30))
else:
m.on(SpeedPercent(-30))
sleep(0.05)
LMC.on(leftSpeed)
i = 0
while light_intensity > 20 and i < ticks:
i = i + 1
sleep(0.005)
light_intensity = cl.reflected_light_intensity
if light_intensity < 20:
LMC.off()
RMC.off()
leds.all_off()
while True:
if (int(round(time.time() * 1000)) - start_time) > 20000:
LMC.off()
RMC.off()
RMC.on(-50)
LMC.on(-50)
sleep(5)
RMC.on(50)
LMC.on(50)
sleep(5)
start_time = int(round(time.time() * 1000))
light_intensity = cl.reflected_light_intensity
if light_intensity > 20:
# White Part of the Board
LMC.off()
RMC.off()
# This code attempts micro adjustments
doTurn(25,-25,25,'LEFT')
leds.set_color('LEFT', 'AMBER')
leds.set_color('RIGHT', 'AMBER')
client, clientInfo = s.accept()
print ('Connected')
leds.set_color('LEFT', 'GREEN')
leds.set_color('RIGHT', 'GREEN')
while True:
data = client.recv(size)
if data:
#print(data, file=sys.stderr)
cmd = Command.unpickled(data)
if cmd:
leftMotor.on(speed=cmd.left_drive)
rightMotor.on(speed=cmd.right_drive)
do_kick = cmd.do_kick > 0
if do_kick != kicking:
kicking = do_kick
if do_kick:
kickMotor.run_to_abs_pos(position_sp=-100, speed_sp=kick_power*max_kick//screenh, stop_action="hold")
kick_power = kick_power - (25 if kick_power > 25 else kick_power)
else:
kickMotor.run_to_abs_pos(position_sp=-10, speed_sp=200, stop_action="coast")
kick_power = kick_power + (1 if kick_power < screenh else 0)
display.rectangle(x1=0, y1=0, x2=screenw, y2=kick_power)
display.update()
except:
class AthenaRobot(object):
# constructors for the robot with default parameters of wheel radius and ports
def __init__(self,
wheelRadiusCm=2.75,
leftMotorPort=OUTPUT_C,
rightMotorPort=OUTPUT_B,
leftSensorPort=INPUT_4,
rightSensorPort=INPUT_1):
#self is the current object, everything below for self are member variables
self.wheelRadiusCm = wheelRadiusCm
self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
self.leftMotor = LargeMotor(leftMotorPort)
self.rightMotor = LargeMotor(rightMotorPort)
self.leftSensor = ColorSensor(leftSensorPort)
self.rightSensor = ColorSensor(rightSensorPort)
# run a distance in centimeters at speed of centimeters per second
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, False)
self.rightMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, block)
# turn a angle in degrees, positive means turn right and negative means turn left.
def turn(self, degree, brake=True, block=True):
# 1.9 is a scale factor from experiments
degreesToRun = degree * 1.9
# Turn at the speed of 20
self.leftMotor.on_for_degrees(20, degreesToRun, brake, False)
self.rightMotor.on_for_degrees(-20, degreesToRun, brake, block)
# run until find a game line
def onUntilGameLine(self,
consecutiveHit=5,
speed=10,
sleepTime=0.01,
white_threshold=85,
black_threshold=30,
brake=True):
# Start motor at passed speed.
self.leftMotor.on(speed)
self.rightMotor.on(speed)
# flags for whether both left and right wheel are in position
leftLineSquaredWhite = False
rightLineSquaredWhite = False
leftConsecutiveWhite = 0
rightConsecutiveWhite = 0
# first aligned on white
while (not leftLineSquaredWhite or not rightLineSquaredWhite):
left_reflected = self.leftSensor.reflected_light_intensity
right_reflected = self.rightSensor.reflected_light_intensity
# left to detect white
if (left_reflected > white_threshold):
leftConsecutiveWhite += 1
else:
leftConsecutiveWhite = 0
# reset to zero
if (leftConsecutiveWhite >= consecutiveHit):
self.leftMotor.off()
leftLineSquaredWhite = True
# right to detect white
if (right_reflected > white_threshold):
rightConsecutiveWhite += 1
else:
rightConsecutiveWhite = 0
# reset to zero
if (rightConsecutiveWhite >= consecutiveHit):
self.rightMotor.off()
rightLineSquaredWhite = True
print(
"left_reflected: {0:3d}, right_reflected: {1:3d}, leftConsecutiveWhite: {2:3d}, rightConsecutiveWhite: {3:3d}"
.format(left_reflected, right_reflected, leftConsecutiveWhite,
rightConsecutiveWhite),
file=sys.stderr)
sleep(sleepTime)
print("*********** White Line Reached *********", file=sys.stderr)
leftLineSquaredBlack = False
rightLineSquaredBlack = False
leftConsecutiveBlack = 0
rightConsecutiveBlack = 0
# now try black
self.leftMotor.on(speed)
self.rightMotor.on(speed)
while (not leftLineSquaredBlack or not rightLineSquaredBlack):
left_reflected = self.leftSensor.reflected_light_intensity
right_reflected = self.rightSensor.reflected_light_intensity
# left to detect black
if (left_reflected < black_threshold):
leftConsecutiveBlack += 1
else:
leftConsecutiveBlack = 0
# reset to zero
if (leftConsecutiveBlack >= consecutiveHit):
self.leftMotor.off()
leftLineSquaredBlack = True
# right to detect black
if (right_reflected < black_threshold):
rightConsecutiveBlack += 1
else:
rightConsecutiveBlack = 0
# reset to zero
if (rightConsecutiveBlack >= consecutiveHit):
self.rightMotor.off()
rightLineSquaredBlack = True
print(
"left_reflected: {0:3d}, right_reflected: {1:3d}, leftConsecutiveBlack: {2:3d}, rightConsecutiveBlack: {3:3d}"
.format(left_reflected, right_reflected, leftConsecutiveBlack,
rightConsecutiveBlack),
file=sys.stderr)
sleep(sleepTime)
self.leftMotor.off()
self.rightMotor.off()
#Go to the Bridge
def goToBridge(self):
# start from base, run 12.5 cm at 20cm/s
self.run(12.5, 20)
sleep(.2)
# turn right 70 degree
self.turn(70)
sleep(.1)
print("test", file=sys.stderr)
# run 90 cm at speed of 30 cm/s
self.run(90, 30, False)
sleep(.1)
# run until hit game line
self.onUntilGameLine()
sleep(.1)
# move forward 2cm at 15cm/s
self.run(2, 15)
# turn left 90 degree
self.turn(-90)
# move forward 13 cm at 20cm/s
self.run(13, 20)
sleep(.1)
# run until hit game line
self.onUntilGameLine()
class Printer:
colors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red', 'white',
'brown')
def __init__(self, pixel_size):
self._touch = TouchSensor(INPUT_1)
self._color = ColorSensor(INPUT_4)
self._color.mode = 'COL-COLOR'
self._fb_motor = LargeMotor(OUTPUT_C)
self._lr_motor = LargeMotor(OUTPUT_B)
self._ud_motor = Motor(OUTPUT_A)
self._x_res = utilities.MAX_X_RES / int(pixel_size)
self._y_res = utilities.MAX_Y_RES / int(pixel_size)
self._padding_left = utilities.MAX_PADDING_LEFT / int(pixel_size)
self._padding_right = utilities.MAX_PADDING_RIGHT / int(pixel_size)
self._is_pen_up = True
self._pen_calibrated = False
self._ud_ratio = 5
self._fb_ratio = 4
self._lr_ratio = 1
self._pen_up_val = -3 * self._ud_ratio
self._pen_down_val = -self._pen_up_val
self._pen_up_down_speed = 10
self._pen_left_speed = 20
self._pen_right_speed = 20
self._paper_scroll_speed = 20
self._pixel_size = pixel_size
self._p_codes = []
self._current_line = 0
def _pen_up(self, val):
print("{} {}".format('PEN_UP', val))
if val > 0:
self._ud_motor.on_for_degrees(self._pen_up_down_speed,
val * self._pen_up_val)
self._is_pen_up = True
def _pen_down(self, val):
print("{} {}".format('PEN_DOWN', val))
if val > 0:
self._ud_motor.on_for_degrees(self._pen_up_down_speed,
val * self._pen_down_val)
self._is_pen_up = False
def _pen_left(self, val):
print("{} {}".format('PEN_LEFT', val))
if val > 0:
self._lr_motor.on_for_degrees(
self._pen_left_speed,
int(self._pixel_size) * val * self._lr_ratio)
def _pen_right(self, val):
print("{} {}".format('PEN_RIGHT', val))
if val > 0:
self._lr_motor.on_for_degrees(
self._pen_right_speed,
-int(self._pixel_size) * val * self._lr_ratio)
def _paper_scroll(self, val):
print("{} {}".format('SCROLL', val))
if val > 0:
if val == 1:
self._current_line += val
print("-------------- Line {} --------------".format(
self._current_line))
self._fb_motor.on_for_degrees(
self._paper_scroll_speed,
int(self._pixel_size) * val * self._fb_ratio)
def _finish_calibration(self):
self._pen_calibrated = True
def calibrate(self, quick_calibration):
speaker = Sound()
if quick_calibration:
speaker.speak("Quick calibration")
print("Quick calibration...")
else:
speaker.speak("Calibrating")
print("Calibrating...")
btn = Button()
self._lr_motor.reset()
self._ud_motor.reset()
self._fb_motor.reset()
self._lr_motor.on_for_degrees(
self._pen_left_speed,
self._x_res * self._pixel_size * self._lr_ratio)
self._lr_motor.reset()
self._ud_motor.on(-15)
self._touch.wait_for_pressed()
self._ud_motor.stop()
time.sleep(1)
self._ud_motor.on(15)
self._touch.wait_for_released()
self._ud_motor.on_for_degrees(10, 40)
self._ud_motor.stop()
time.sleep(1)
speaker.speak(
"Insert calibration paper and press the touch sensor")
self._touch.wait_for_pressed()
speaker.speak("Adjust pen height")
print("Adjust pen height...")
while not self._pen_calibrated:
self._lr_motor.on_for_degrees(
self._pen_right_speed,
-100 * self._pixel_size * self._lr_ratio)
self._lr_motor.on_for_degrees(
self._pen_left_speed,
100 * self._pixel_size * self._lr_ratio)
time.sleep(1)
if btn.up:
self._pen_up(1)
elif btn.down:
self._pen_down(1)
elif btn.right:
self._finish_calibration()
elif btn.left:
self._pen_down(4)
self._lr_motor.reset()
if not self._is_pen_up:
self._pen_up(1)
self._pen_left(self._x_res)
for _ in range(2):
self._pen_right(self._x_res)
self._pen_left(self._x_res)
for _ in range(8):
self._pen_right(self._padding_left)
for _ in range(int(self._x_res)):
self._pen_right(1)
self._pen_left(self._x_res + self._padding_left)
self._lr_motor.reset()
speaker.speak(
"Insert a blank piece of paper and press the touch sensor")
self._touch.wait_for_pressed()
self._fb_motor.on(5)
val = 0
print("Scrolling the piece of paper to its starting position...")
while self.colors[val] == 'unknown':
val = self._color.value()
self._fb_motor.reset()
print("Calibration done")
def _interpret_p_codes(self, p_codes):
btn = Button()
speaker = Sound()
self._current_line = 0
abort = False
print("---------- p_codes:----------")
print("-------------- Line {} --------------".format(
self._current_line))
for x in p_codes:
if btn.down:
speaker.speak("Aborting")
print("\nAborting...")
abort = True
break
if x[0] == utilities.Command.PEN_UP:
if not self._is_pen_up:
self._pen_up(x[1])
elif x[0] == utilities.Command.PEN_DOWN:
if self._is_pen_up:
self._pen_down(x[1])
elif x[0] == utilities.Command.PEN_RIGHT:
self._pen_right(x[1])
elif x[0] == utilities.Command.PEN_LEFT:
self._pen_left(x[1])
elif x[0] == utilities.Command.SCROLL:
self._paper_scroll(x[1])
if not self._is_pen_up:
self._pen_up(1)
self._ud_motor.stop()
return abort
def draw(self, path=None, multicolor=False):
speaker = Sound()
if path is not None:
if multicolor:
speaker.speak("Printing multi color image")
print("\nPrinting multi color image...")
else:
speaker.speak("Printing single color image")
print("\nPrinting single color image...")
binarized, img_x, img_y = utilities.binarize_image(
path, self._x_res, self._y_res, multicolor)
else:
binarized, img_x, img_y = utilities.generate_and_binarize_test_image(
self._pixel_size)
speaker.speak("Printing test page")
print("\nPrinting test page...")
quick_calibration = False
for layer, i in zip(binarized, range(1, utilities.NUM_OF_COLORS)):
speaker.speak("Insert a {} pen and press the touch sensor".format(
utilities.palette_names[i]))
self._touch.wait_for_pressed()
self.calibrate(quick_calibration)
p_codes = utilities.binarized_image_to_p_codes(
layer, img_x, img_y, self._padding_left)
if self._interpret_p_codes(p_codes):
break
self._paper_scroll(self._y_res)
self._fb_motor.reset()
self._ud_motor.reset()
self._lr_motor.reset()
quick_calibration = True
speaker = Sound()
speaker.speak("Printing finished")
print("Printing finished")
print("speed_d = ", m.speed_d) # powerup default is 7500
start_time = time.time()
""" ### quick test
m.position = 0
print(m.position) """
for j in range(50, 551, 10):
m.speed_p = 15000
m.speed_i = j
m.speed_d = 7500
tachospeed_min = 999999.99
tachospeed_max = 0.0
start_pos = 0
start_time = time.time()
m.on(SpeedDPS(goal_DPS))
for i in range(100):
t1 = time.time()
old_pos = encoder_pos
time.sleep(0.05)
encoder_pos = m.position
dt = time.time() - t1
d_deg = encoder_pos - old_pos
tachospeed = d_deg / dt
d_deg_avg = ((d_deg_avg + d_deg) / (2))
tachospeed_avg = ((tachospeed_avg + tachospeed) / (2))
if tachospeed < tachospeed_min:
tachospeed_min = tachospeed
if tachospeed > tachospeed_max:
tachospeed_max = tachospeed
# if abs(d_deg) > 100:
#!/usr/bin/env python3
from time import sleep
from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent
from ev3dev2.sensor.lego import TouchSensor
touch_start_button = TouchSensor()
motor_conveyor = LargeMotor(OUTPUT_A)
motor_fan = LargeMotor(OUTPUT_B)
motor_water = MediumMotor(OUTPUT_C)
while True:
touch_start_button.wait_for_pressed()
motor_conveyor.on(SpeedPercent(8))
sleep(1)
motor_water.on_for_rotations(SpeedPercent(10), 0.5)
sleep(1.5)
motor_water.on_for_rotations(SpeedPercent(10), -0.5)
sleep(1.5)
motor_fan.on(SpeedPercent(100))
motor_conveyor.on(SpeedPercent(3))
sleep(4)
motor_fan.off()
motor_conveyor.off()
# Make the robot turn so the compass reads the correct angle
# Start turning clockwise very slowly.
while True:
compass_angle = compass.value()
if degrees - 1 <= compass_angle <= degrees + 1:
break
# We also need to be careful when degrees is 0. A compass angle of 359 is super close,
# and so we need to check this case separately
elif degrees == 0 and compass_angle >= 359:
break
difference = compass_angle - degrees
if difference > 180:
difference = difference - 360
elif difference < -180:
difference = difference + 360
# Now difference is in between -180 and 180.
# If it's negative, we need to turn clockwise
if difference < 0:
# Motors are more powerful when difference is large
m_l.on((difference - 20) / 5)
m_r.on(-(difference - 20) / 5)
else:
# Motors are more powerful when difference is large
m_l.on((difference + 20) / 5)
m_r.on(-(difference + 20) / 5)
wait_for_tick()
# Go forwards
m_l.on_for_seconds(50, distance / robot_speed, block=False)
m_r.on_for_seconds(50, distance / robot_speed)
x = x + 1
# lift the rear part of the car
lm_lifter.on_for_degrees(-100, lm_lifter.degrees)
# stop drive motor
lm_move.stop()
mm_move.stop()
# reset gyro sensor
gy.mode = 'GYRO-RATE'
gy.mode = 'GYRO-ANG'
log.info('prepare for the initial position')
# prepare for the initial position
# detect the upper position of the lifter
lm_lifter.on( -100, brake=True)
while( not ts.is_pressed ):
sleep(0.05)
# approaching the initial position with high speed
lm_lifter.on_for_rotations(90, 7)
# nearly approached the initial position, approaching with lower speed
lm_lifter.on_for_degrees(20, 240)
# clear the lcd display
lcd.clear()
# show the steps
lcd.text_pixels( str(steps), True, 80, 50, font='courB18')
lcd.update()
log.info('wait user to supply the steps')
if x == 32 or x == 120: # space or x key pushed
spd = 0
turnRatio = 0
if x == 119: # w key pushed
if spd < 90: # limit max frwd speed to 90
spd = spd + 5
if x == 115: # s key pushed
if spd > -30: # limit max rvrs speed to -30
spd = spd - 5
if x == 100: # d key pushed (turn more to the Right)
if turnRatio > -0.5:
turnRatio = turnRatio - 0.1
if x == 97: # a key pushed (turn more to the Left)
if turnRatio < 0.5:
turnRatio = turnRatio + 0.1
if turnRatio <= 0:
mLspd = spd
mRspd = spd * (1 + turnRatio)
if turnRatio > 0:
mRspd = spd
mLspd = spd * (1 - turnRatio)
mL.on(mLspd, brake=False)
mR.on(mRspd, brake=False)
if x == 120: # x key means exit
break
print("x, spd, turnRatio, mLspd, mRspd ", x, spd, turnRatio, mLspd, mRspd)
time.sleep(.05)
class Balance:
def __init__(self, motor, sensor, midPoint):
self.motor = LargeMotor(motor)
self.sensor = UltrasonicSensor(sensor)
self.midPoint = midPoint
self.motorPositionUp = self.motor.position - 20
self.motorPositionDown = self.motor.position + 20
self.integral = 0
self.integralCount = 0
def getSensorState(self):
"""
Returns sensor measure based on an axis positioned on
the mid point
"""
return -1 * (self.sensor.distance_centimeters - self.midPoint)
def rotateAxis(self, speed):
"""
Rotates axis on the tacho motor by speed value
"""
# Limits rotation to limit object speed
if self.motor.position >= self.motorPositionDown and speed > 0:
self.motor.stop(stop_action="brake")
elif self.motor.position <= self.motorPositionUp and speed < 0:
self.motor.stop(stop_action="brake")
else:
#print(speed)
self.motor.on(speed, block=False)
def calculateSpeed(self, Kp=1, Kd=0, Ki=0, verbose=False):
"""
Calculates speed in next cicle based on
a PID control
"""
initialMeasure = self.getSensorState()
sleep(0.015)
finalMeasure = self.getSensorState()
# Fix closeness to the sensor
if finalMeasure < -30:
finalMeasure = 8.4
speed = finalMeasure - initialMeasure
if -3 < finalMeasure < 3 and (self.integral <= 10
and self.integral >= -10
and self.integralCount < 10):
self.integral = self.integral + (finalMeasure)
self.integralCount = self.integralCount + 1
else:
self.integral = 0
self.integralCount = 0
value = (Kp * finalMeasure) + (Kd * speed) + self.integral * Ki
# Fix unbalanced values
if value > 100:
value = 100.0
if value < -100:
value = -100.0
#print("\n-Distancia: %f, Velocidad %f, Integracion %f" % (finalMeasure, speed, self.integral))
return value / 3.2
print("toggled sample_mode (i); now set to... ", sample_mode)
if sample_mode > 0:
try:
# cycle through positions and take data samples
for i in range(
30, 151,
15): ### cycle through increasing distances from door
## move BPDDbot to potision i
### rotate to face SE wall
print("moving to position ", i,
" ...rotating to face SE wall")
while compassVal < SEwalldeg - 1 or compassVal > SEwalldeg + 1:
compassVal = cmp.value(0)
#print("compasVal = ", compassVal)
mL.on(3, brake=False)
mL.on(0, brake=True)
time.sleep(.25)
print("compassVal = ", compassVal)
usDistCmVal = us.distance_centimeters
print("usDistCmVal = ", usDistCmVal)
## drive to distance SEWALLDIST (40cm)
print("moving to position ", i,
" ...driving to correct distance from SE wall")
while usDistCmVal < SEWALLDIST - 1 or usDistCmVal > SEWALLDIST + 1:
usDistCmVal = us.distance_centimeters
if usDistCmVal < SEWALLDIST - 1:
mL.on(-5, brake=False)
mR.on(-5, brake=False)
if usDistCmVal > SEWALLDIST + 1:
from ev3dev2.sensor.lego import GyroSensor
import time
from ev3dev2.sound import Sound
m1 = LargeMotor(OUTPUT_C)
m2 = LargeMotor(OUTPUT_D)
m3 = LargeMotor(OUTPUT_B)
us = UltrasonicSensor()
sound = Sound()
print(us.distance_centimeters, file=sys.stderr)
a_min = m3.position + 10
m3.on(-10, block=False)
while m3.position < a_min:
a_min = m3.position
time.sleep(0.1)
m3.off()
print("a_min", a_min, file=sys.stderr)
a_max = m3.position - 10
m3.on(10, block=False)
while m3.position > a_max:
a_max = m3.position
time.sleep(0.1)
m3.off()
print("a_max", a_max, file=sys.stderr)
from ev3dev2.motor import LargeMotor, SpeedPercent, OUTPUT_B, OUTPUT_C # Importing the sleep() class from the time module from time import sleep # Moving the right large motor # Set a variable for your left large motor, connected to port B l_motor = LargeMotor(OUTPUT_B) # Set a variable for your right large motor, connected to port C r_motor = LargeMotor(OUTPUT_C) # 1. Move the motor with on() and sleep() methods # a. turning it on at speed 70 # b. telling the robot to wait 1 second # c. turning the robot off l_motor.on(70) sleep(1) l_motor.off() r_motor.on(70) sleep(1) r_motor.off() # 2. Move the motors with the on_for_seconds() method # - Speed 70% for 1 second r_motor.on_for_seconds(SpeedPercent(70), 1) l_motor.on_for_seconds(SpeedPercent(70), 1) # 3. Move the motors with the on_for_rotations() method # - Speed 70% for 2 wheel rotations l_motor.on_for_rotations(SpeedPercent(70), 2)
class MindCuber(object):
scan_order = [
5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54,
51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43,
44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31
]
hold_cube_pos = 85
rotate_speed = 400
flip_speed = 300
flip_speed_push = 400
def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(address=OUTPUT_A)
self.turntable = LargeMotor(address=OUTPUT_B)
self.colorarm = MediumMotor(address=OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def shutdown_robot(self):
log.info('Shutting down')
self.shutdown = True
if self.rgb_solver:
self.rgb_solver.shutdown = True
for x in (self.flipper, self.turntable, self.colorarm):
# We are shutting down so do not 'hold' the motors
x.stop_action = 'brake'
x.off(False)
def signal_term_handler(self, signal, frame):
log.error('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.error('Caught SIGINT')
self.shutdown_robot()
def apply_transformation(self, transformation):
self.state = [self.state[t] for t in transformation]
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def rotate_cube_1(self):
self.rotate_cube(1, 1)
def rotate_cube_2(self):
self.rotate_cube(1, 2)
def rotate_cube_3(self):
self.rotate_cube(-1, 1)
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def rotate_cube_blocked_1(self):
self.rotate_cube_blocked(1, 1)
def rotate_cube_blocked_2(self):
self.rotate_cube_blocked(1, 2)
def rotate_cube_blocked_3(self):
self.rotate_cube_blocked(-1, 1)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def colorarm_middle(self):
log.info("colorarm_middle()")
self.colorarm.on_to_position(SpeedDPS(600), -750)
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_remove(self):
log.info("colorarm_remove()")
self.colorarm.on_to_position(SpeedDPS(600), 0)
def colorarm_remove_halfway(self):
log.info("colorarm_remove_halfway()")
self.colorarm.on_to_position(SpeedDPS(600), -400)
def scan_face(self, face_number):
log.info("scan_face() %d/6" % face_number)
if self.shutdown:
return
if self.flipper.position > 35:
self.flipper_away(100)
self.colorarm_middle()
self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb
self.k += 1
i = 1
target_pos = 115
self.colorarm_corner(i)
# The gear ratio is 3:1 so 1080 is one full rotation
self.turntable.reset()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
1080,
block=False)
self.turntable.wait_until('running')
while True:
# 135 is 1/8 of full rotation
if self.turntable.position >= target_pos:
current_color = self.color_sensor.rgb
self.colors[int(MindCuber.scan_order[self.k])] = current_color
i += 1
self.k += 1
if i == 9:
# Last face, move the color arm all the way out of the way
if face_number == 6:
self.colorarm_remove()
# Move the color arm far enough away so that the flipper
# arm doesn't hit it
else:
self.colorarm_remove_halfway()
break
elif i % 2:
self.colorarm_corner(i)
if i == 1:
target_pos = 115
elif i == 3:
target_pos = 380
else:
target_pos = i * 135
else:
self.colorarm_edge(i)
if i == 2:
target_pos = 220
elif i == 8:
target_pos = 1060
else:
target_pos = i * 135
if self.shutdown:
return
if i < 9:
raise ScanError('i is %d..should be 9' % i)
self.turntable.wait_until_not_moving()
self.turntable.off()
self.turntable.reset()
log.info("\n")
def scan(self):
log.info("scan()")
self.colors = {}
self.k = 0
self.scan_face(1)
self.flip()
self.scan_face(2)
self.flip()
self.scan_face(3)
self.rotate_cube(-1, 1)
self.flip()
self.scan_face(4)
self.rotate_cube(1, 1)
self.flip()
self.scan_face(5)
self.flip()
self.scan_face(6)
if self.shutdown:
return
log.info("RGB json:\n%s\n" % json.dumps(self.colors))
self.rgb_solver = RubiksColorSolverGeneric(3)
self.rgb_solver.enter_scan_data(self.colors)
self.rgb_solver.crunch_colors()
self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))
# This is only used if you want to rotate the cube so U is on top, F is
# in the front, etc. You would do this if you were troubleshooting color
# detection and you want to pause to compare the color pattern on the
# cube vs. what we think the color pattern is.
'''
log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
self.rotate_cube(-1, 1)
self.flip()
self.flipper_away()
self.rotate_cube(1, 1)
input('Paused')
'''
def move(self, face_down):
log.info("move() face_down %s" % face_down)
position = self.state.index(face_down)
actions = {
0: ["flip", "flip"],
1: [],
2: ["rotate_cube_2", "flip"],
3: ["rotate_cube_1", "flip"],
4: ["flip"],
5: ["rotate_cube_3", "flip"]
}.get(position, None)
for a in actions:
if self.shutdown:
break
getattr(self, a)()
def run_kociemba_actions(self, actions):
log.info('Action (kociemba): %s' % ' '.join(actions))
total_actions = len(actions)
for (i, a) in enumerate(actions):
if self.shutdown:
break
if a.endswith("'"):
face_down = list(a)[0]
rotation_dir = 1
elif a.endswith("2"):
face_down = list(a)[0]
rotation_dir = 2
else:
face_down = a
rotation_dir = 3
log.info("Move %d/%d: %s%s (a %s)" %
(i, total_actions, face_down, rotation_dir, pformat(a)))
self.move(face_down)
if rotation_dir == 1:
self.rotate_cube_blocked_1()
elif rotation_dir == 2:
self.rotate_cube_blocked_2()
elif rotation_dir == 3:
self.rotate_cube_blocked_3()
log.info("\n")
def resolve(self):
if self.shutdown:
return
cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
output = check_output(cmd).decode('ascii')
if 'ERROR' in output:
msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
output)
log.error(msg)
print(msg)
sys.exit(1)
actions = output.strip().split()
self.run_kociemba_actions(actions)
self.cube_done()
def cube_done(self):
self.flipper_away()
def wait_for_cube_insert(self):
rubiks_present = 0
rubiks_present_target = 10
log.info('wait for cube...to be inserted')
while True:
if self.shutdown:
break
dist = self.infrared_sensor.proximity
# It is odd but sometimes when the cube is inserted
# the IR sensor returns a value of 100...most of the
# time it is just a value less than 50
if dist < 50 or dist == 100:
rubiks_present += 1
log.info("wait for cube...distance %d, present for %d/%d" %
(dist, rubiks_present, rubiks_present_target))
else:
if rubiks_present:
log.info('wait for cube...cube removed (%d)' % dist)
rubiks_present = 0
if rubiks_present >= rubiks_present_target:
log.info('wait for cube...cube found and stable')
break
time.sleep(0.1)
class Dinor3x:
FAST_WALK_SPEED = 80
NORMAL_WALK_SPEED = 40
SLOW_WALK_SPEED = 20
def __init__(self,
jaw_motor_port: str = OUTPUT_A,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.jaw_motor = MediumMotor(address=jaw_motor_port)
self.left_motor = LargeMotor(address=left_motor_port)
self.right_motor = LargeMotor(address=right_motor_port)
self.tank_driver = MoveTank(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.steer_driver = MoveSteering(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
self.roaring = False
self.walk_speed = self.NORMAL_WALK_SPEED
def roar_by_ir_beacon(self):
"""
Dinor3x roars when the Beacon button is pressed
"""
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.roaring = True
self.open_mouth()
self.roar()
elif self.roaring:
self.roaring = False
self.close_mouth()
def change_speed_by_color(self):
"""
Dinor3x changes its speed when detecting some colors
- Red: walk fast
- Green: walk normally
- White: walk slowly
"""
if self.color_sensor.color == ColorSensor.COLOR_RED:
self.speaker.speak(text='RUN!',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.walk_speed = self.FAST_WALK_SPEED
self.walk(speed=self.walk_speed)
elif self.color_sensor.color == ColorSensor.COLOR_GREEN:
self.speaker.speak(text='Normal',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.walk_speed = self.NORMAL_WALK_SPEED
self.walk(speed=self.walk_speed)
elif self.color_sensor.color == ColorSensor.COLOR_WHITE:
self.speaker.speak(text='slow...',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.walk_speed = self.SLOW_WALK_SPEED
self.walk(speed=self.walk_speed)
def walk_by_ir_beacon(self):
"""
Dinor3x walks or turns according to instructions from the IR Beacon
- 2 top/up buttons together: walk forward
- 2 bottom/down buttons together: walk backward
- Top Left / Red Up: turn left on the spot
- Top Right / Blue Up: turn right on the spot
- Bottom Left / Red Down: stop
- Bottom Right / Blue Down: calibrate to make the legs straight
"""
# forward
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right:
self.walk(speed=self.walk_speed)
# backward
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.walk(speed=-self.walk_speed)
# turn left on the spot
elif self.ir_sensor.top_left(channel=self.ir_beacon_channel):
self.turn(speed=self.walk_speed)
# turn right on the spot
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.turn(speed=-self.walk_speed)
# stop
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel):
self.tank_driver.off(brake=True)
# calibrate legs
elif self.ir_sensor.bottom_right(channel=self.ir_beacon_channel):
self.calibrate_legs()
def calibrate_legs(self):
self.tank_driver.on(left_speed=10, right_speed=20)
self.touch_sensor.wait_for_released()
self.tank_driver.off(brake=True)
self.left_motor.on(speed=40)
self.touch_sensor.wait_for_pressed()
self.left_motor.off(brake=True)
self.left_motor.on_for_rotations(rotations=-0.2,
speed=50,
brake=True,
block=True)
self.right_motor.on(speed=40)
self.touch_sensor.wait_for_pressed()
self.right_motor.off(brake=True)
self.right_motor.on_for_rotations(rotations=-0.2,
speed=50,
brake=True,
block=True)
self.left_motor.reset()
self.right_motor.reset()
def walk(self, speed: float = 100):
self.calibrate_legs()
self.steer_driver.on(steering=0, speed=-speed)
def turn(self, speed: float = 100):
self.calibrate_legs()
if speed >= 0:
self.left_motor.on_for_degrees(degrees=180,
speed=speed,
brake=True,
block=True)
else:
self.right_motor.on_for_degrees(degrees=180,
speed=-speed,
brake=True,
block=True)
self.tank_driver.on(left_speed=speed, right_speed=-speed)
def close_mouth(self):
self.jaw_motor.on_for_seconds(speed=-20,
seconds=1,
brake=False,
block=False)
def open_mouth(self):
self.jaw_motor.on_for_seconds(speed=20,
seconds=1,
block=False,
brake=False)
def roar(self):
self.speaker.play_file(wav_file='T-rex roar.wav',
volume=100,
play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE)
self.jaw_motor.on_for_degrees(speed=40,
degrees=-60,
block=True,
brake=True)
for i in range(12):
self.jaw_motor.on_for_seconds(speed=-40,
seconds=0.05,
block=True,
brake=True)
self.jaw_motor.on_for_seconds(speed=40,
seconds=0.05,
block=True,
brake=True)
self.jaw_motor.on_for_seconds(speed=20,
seconds=0.5,
brake=False,
block=True)
def main(self):
self.close_mouth()
while True:
self.roar_by_ir_beacon()
self.change_speed_by_color()
self.walk_by_ir_beacon()
class Spik3r:
def __init__(self,
sting_motor_port: str = OUTPUT_D,
go_motor_port: str = OUTPUT_B,
claw_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.sting_motor = LargeMotor(address=sting_motor_port)
self.go_motor = LargeMotor(address=go_motor_port)
self.claw_motor = MediumMotor(address=claw_motor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.dis = Display()
self.speaker = Sound()
def sting_by_ir_beacon(self):
while True:
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.sting_motor.on_for_degrees(speed=-75,
degrees=220,
brake=True,
block=False)
self.speaker.play_file(wav_file='/home/robot/sound/Blip 1.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
self.sting_motor.on_for_seconds(speed=-100,
seconds=1,
brake=True,
block=True)
self.sting_motor.on_for_seconds(speed=100,
seconds=1,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def be_noisy_to_people(self):
while True:
if self.color_sensor.reflected_light_intensity > 30:
for i in range(4):
self.speaker.play_file(
wav_file='/home/robot/sound/Blip 2.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def pinch_if_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.claw_motor.on_for_seconds(speed=50,
seconds=1,
brake=True,
block=True)
self.claw_motor.on_for_seconds(speed=-50,
seconds=0.3,
brake=True,
block=True)
def keep_driving_by_ir_beacon(self):
while True:
if self.ir_sensor.top_left(channel=self.ir_beacon_channel) and \
self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.go_motor.on(speed=91, block=False, brake=False)
elif self.ir_sensor.top_right(channel=self.ir_beacon_channel):
self.go_motor.on(speed=-100, brake=False, block=False)
else:
self.go_motor.off(brake=True)
def main(self):
self.dis.image_filename(filename='/home/robot/image/Evil.bmp',
clear_screen=True)
self.dis.update()
# FIXME: .sting_by_ir_beacon stops responding after a while
Process(target=self.be_noisy_to_people, daemon=True).start()
Process(target=self.sting_by_ir_beacon, daemon=True).start()
Process(target=self.pinch_if_touched, daemon=True).start()
self.keep_driving_by_ir_beacon()
class MindstormsGadget(AlexaGadget):
"""
A Mindstorms gadget that performs movement based on voice commands.
"""
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Ev3dev initialization
self.leds = Leds()
self.sound = Sound()
self.drive = LargeMotor(OUTPUT_B)
self.dealmotor = MediumMotor(OUTPUT_A)
self.bcolor = ColorSensor(INPUT_1)
self.pushsensor = TouchSensor(INPUT_2)
self.leftmargin = 0
self.rigtmargin = 0
self._init_reset()
def _init_reset(self):
self.numCards = 2
self.numPlayers = 0
self.game = 'blackjack'
self.degreeStep = 180
self.players = ["red","yellow"] #Default player
self._send_event(EventName.SPEECH, {'speechOut': "Restart game"})
def on_connected(self, device_addr):
"""
Gadget connected to the paired Echo device.
:param device_addr: the address of the device we connected to
"""
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
logger.info("{} connected to Echo device".format(self.friendly_name))
def on_disconnected(self, device_addr):
"""
Gadget disconnected from the paired Echo device.
:param device_addr: the address of the device we disconnected from
"""
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
logger.info("{} disconnected from Echo device".format(self.friendly_name))
def on_custom_mindstorms_gadget_control(self, directive):
"""
Handles the Custom.Mindstorms.Gadget control directive.
:param directive: the custom directive with the matching namespace and name
"""
try:
payload = json.loads(directive.payload.decode("utf-8"))
print("Control payload: {}".format(payload), file=sys.stderr)
control_type = payload["type"]
if control_type == "move":
# Expected params: [direction, duration, speed]
self._move(payload["direction"], int(payload["duration"]), int(payload["speed"]))
if control_type == "command":
# Expected params: [command]
self._activate(payload["command"])
if control_type == "dealcard":
# Expected params: [command] = Number of Cards
# Expected params: [player]
num = payload["command"]
player = payload["player"]
speak = "Give "+ num + " card to " + player
if player == "all":
if (self.numPlayers == 0):
self.numPlayers = 2
for i in range(int(num)):
for j in range(self.numPlayers):
self._dealcard(1,j)
else:
try:
num = self.players.index(player)
self._dealcard(int(payload["command"]),num)
except ValueError:
speak = "There is no user " + player
print (speak,file=sys.stderr)
self._send_event(EventName.SPEECH, {'speechOut': speak})
except KeyError:
print("Missing expected parameters: {}".format(directive), file=sys.stderr)
def _dealcard(self, num, player):
"""
Deal number of cards to player
"""
if num < 1:
num = 1
degree = self._calcDegree(player)
print("Give player : {} card : {} Move angle : {}".format(player, num,degree), file=sys.stderr)
self.drive.on_to_position(SpeedPercent(10),degree)
self.drive.wait_until_not_moving()
for i in range(num):
self.dealmotor.on_for_degrees(SpeedPercent(-100), 340)
self.dealmotor.wait_until_not_moving()
time.sleep(1)
self.dealmotor.on_for_degrees(SpeedPercent(100), 270)
self.dealmotor.wait_until_not_moving()
time.sleep(1)
def _calcDegree (self,player):
degree = (player*self.degreeStep)+self.leftmargin
return degree
def _gameinit(self,game):
"""
Check and start game
"""
if (self.numPlayers == 0):
self.numPlayers = 2
if game == "poker":
self.numCards = 5
if game == "blackjack":
self.numCards = 2
if game == "rummy":
self.numCards = 7
if (self.numPlayers == 2):
self.numCards = 10
speak = ""
for i in range(self.numPlayers):
print("Player : {} Color : {}".format(i,self.players[i]), file=sys.stderr)
speak = speak + self.players[i]
speak = "Start " + game + "with " + str(self.numPlayers) + "player " + speak
self._send_event(EventName.SPEECH, {'speechOut': speak})
self._findboundary()
self.drive.on_to_position(SpeedPercent(10),self.leftmargin)
time.sleep(1)
print("Game : {} Number of Card : {}".format(game,self.numCards), file=sys.stderr)
for i in range(self.numCards):
for j in range(self.numPlayers):
self._dealcard(1,j)
def _findboundary (self):
"Move to left until sensor pressed "
self.drive.on(SpeedPercent(10))
self.pushsensor.wait_for_pressed ()
self.drive.stop()
"Get position"
self.rightmargin = self.drive.position
print("Right position : {} ".format(self.rightmargin), file=sys.stderr)
self.drive.on(SpeedPercent(-10))
time.sleep(1)
self.pushsensor.wait_for_pressed ()
self.drive.stop()
"Get position + offset 45 for not to close limitation"
self.leftmargin = self.drive.position
self.leftmargin = self.leftmargin + 60
print("Left position : {} ".format(self.leftmargin), file=sys.stderr)
self.degreeStep = int(abs((self.leftmargin - self.rightmargin)/self.numPlayers))
print("Degree steps : {} ".format(self.degreeStep), file=sys.stderr)
def _addUser (self):
if self.numPlayers == 0:
self.players.clear()
player = self.bcolor.color_name
self.players.append(player.lower())
print("Player {} color: {}".format(self.players[self.numPlayers],player), file=sys.stderr)
self.numPlayers += 1
self._send_event(EventName.PLAYER, {'player': player})
def _move(self, direction, duration: int, speed: int, is_blocking=False):
"""
Handles move commands from the directive.
Right and left movement can under or over turn depending on the surface type.
:param direction: the move direction
:param duration: the duration in seconds
:param speed: the speed percentage as an integer
:param is_blocking: if set, motor run until duration expired before accepting another command
"""
print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr)
if direction in Direction.FORWARD.value:
self.drive.on_for_degrees(SpeedPercent(10),90)
if direction in Direction.BACKWARD.value:
self.drive.on_for_degrees(SpeedPercent(-10),90)
if direction in Direction.STOP.value:
self.drive.off()
def _send_event(self, name: EventName, payload):
"""
Sends a custom event to trigger a sentry action.
:param name: the name of the custom event
:param payload: the sentry JSON payload
"""
self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload)
def _activate(self, command, speed=50):
"""
Handles preset commands.
:param command: the preset command
:param speed: the speed if applicable
"""
print("Activate command: ({}, {})".format(command, speed), file=sys.stderr)
if command in Command.GAMES.value:
self.game = command
print("Play game: {}".format(self.game), file=sys.stderr)
self._gameinit(self.game)
if command in Command.RESET_GAME.value:
# Reset game
self._init_reset()
if command in Command.ADD_CMD.value:
self._addUser()
time.sleep(3) start_pos = 0 target_end_pos = 0 start_time = 0.0 end_time = 0.0 target_speedDPS = 900 adjusted_speedDPS = 0 m = LargeMotor(OUTPUT_A) time.sleep(0.1) m.reset() time.sleep(0.1) start_time = time.time() m.on(SpeedDPS(0)) t1 = time.time() """ Notes: - LPF technique needs to be more generalized than it is currently (v0.1, 3/1/2020) - needs calculation that takes actual acceleration/decceleration, in DPS, into consideration - current implementation ramps up from '0' DPS to target_speedDPS and back down to '0' - need to alter to change from current DPS to new DSP (not assume starting at '0') - current implementation ramps up/down based on static time duration, rather than constant accel/decel rate Possible implementation based on ---> y = (m * x) + b ---- y = new adjusted_speedDPS ---- m (slope) = ramp_rateDPS ---- x delta_t (change in time - measured in seconds)
polarCoordscmpMean = np.array([], dtype=np.int32)
print("r pushed - reinitialized polarCoords* arrays")
if x == 105: # i pushed - toggle sample mode on and off
sample_mode *= -1
print("toggled sample_mode (i); now set to... ", sample_mode)
if sample_mode > 0:
try:
# rototate to cmp angle and take data samples
for i in range(360):
## move motor to new cmps angle
while compassVal < i - 1 or compassVal > i + 1:
compassVal = cmp.value(0)
print("compasVal = ", compassVal)
mL.on(1, brake=False)
mL.on(0, brake=True)
time.sleep(2) # pause to let the shaking stop before collecting data
# init the data collection arrays for each angle/collection series
polarCoordsUSr = np.array([], dtype=np.int32)
polarCoordscmpDeg = np.array([], dtype=np.int32)
# take 100 samples
for j in range(100):
usDistCmVal = us.distance_centimeters
compassVal = cmp.value(0)
polarCoordsUSr = np.append(polarCoordsUSr, usDistCmVal)
polarCoordscmpDeg = np.append(polarCoordscmpDeg, compassVal)
print("")
print ("polarCoordsUSr.size = ", polarCoordsUSr.size)
print("min max mean std")
print(np.min(polarCoordsUSr), np.max(polarCoordsUSr), np.mean(polarCoordsUSr), np.std(polarCoordsUSr))
while not halt: # Read the Gyro and calulate error gyroError = targetAngle - gyro.angle print(gyroError) # Calulate the differential between motor speeds if abs(gyroError) < 2.5: differential = 0 else: differential = 2 * gyroError # Make sure the differentialis are not too wild # If you want the robot to spin then max(differential) >= 2 * forwardSpeed differential = min(max(differential, -60), 60) # Calculate the motor speeds lmSpeed = forwardSpeed + differential / 2 rmSpeed = forwardSpeed - differential / 2 # Make sure the motor speeds are not too wild lmSpeed = min(max(lmSpeed, -50), 50) rmSpeed = min(max(rmSpeed, -50), 50) # Set motor speeds for this iteration of the loop # motor.on(speed) turn the motor on for ever, so don't forget to stop leftMotor.on(lmSpeed) rightMotor.on(rmSpeed) print(lmSpeed, ', ', rmSpeed) # Small delay to help read screen print, too slow and the robot motion will be jerky sleep(0.2)
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.button import Button
btn = Button()
ma = LargeMotor('outA')
mb = LargeMotor('outD')
cl = ColorSensor()
cl.mode='COL-AMBIENT'
while True:
if btn.any():
break
ma.on(speed=10)
mb.on(speed=10)
print(cl.value())
ma.off()
mb.off()
class Robot:
# Import stuff
from ev3dev2.motor import MoveTank, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, LargeMotor
from ev3dev2.sensor.lego import ColorSensor, GyroSensor, InfraredSensor, TouchSensor, UltrasonicSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sound import Sound
from ev3dev2.button import Button
from ev3dev2.led import Leds, LED_COLORS, LED_DEFAULT_COLOR, LED_GROUPS, LEDS
from ev3dev2.display import Display
import ev3dev2.fonts as fonts
import math
def __init__(self, filename):
"""
Stores all the properties of the robot, such as wheel circumference, motor ports, ect. Also provides methods for higher-level
interaction with the robot, such as driving in a straight line at a specific heading, following a line, or driving until hitting
an obstacle. Also instantiates motor and sensor objects for direct use later, if necessary.
Reads ``filename`` for robot properties; ``filename`` should be a .ini or .cfg file in INI format. See the current robot.cfg
for format example.
"""
# Load and read the config file
from configparser import SafeConfigParser
conf = SafeConfigParser()
conf.read(filename)
# Set the "ForFLL" flag based on it's status in the config file (used in some input validation)
self.ForFLL = bool(conf.get('Other', 'ForFLL') == "TRUE")
# Instantiate objects for controlling things on the brick itself (Speaker, Buttons, Lights, and the LCD)
self.spkr = self.Sound()
self.btn = self.Button()
self.led = self.Leds()
self.disp = self.Display()
# Read the drive motor ports from the config file, and store. "eval()" is used because the port names "OUTPUT_#",
# where # is a capital letter, A - D, are variables used as constants, and reading as a string does not work.
self.LeftMotor = eval(conf.get('Drivebase', 'LeftMotorPort'))
self.RightMotor = eval(conf.get('Drivebase', 'RightMotorPort'))
# Read and store the wheel circumference and width between the wheels
self.WheelCircumference = float(
conf.get('Drivebase', 'WheelCircumference'))
self.WidthBetweenWheels = float(
conf.get('Drivebase', 'WidthBetweenWheels'))
# Read and store MotorInverted and GyroInverted. Both are relative to the robot, and drive functions will not work correctly if these
# values are incorrect, and they are the first things to check if the drive functions do not work correctly.
self.MotorInverted = bool(
conf.get('Drivebase', 'MotorInverted') == "TRUE")
self.GyroInverted = bool(
conf.get('Drivebase', 'GyroInverted') == "TRUE")
# Reads and stores the gear ratio value.
self.GearRatio = float(conf.get('Drivebase', 'GearRatio'))
# Reads and stores the PID gains for driving in a straight line.
self.kp = float(conf.get('Drivebase', 'kp'))
self.ki = float(conf.get('Drivebase', 'ki'))
self.kd = float(conf.get('Drivebase', 'kd'))
# Read the sensor ports from the config file, and store. "eval()" is used because the port names "INPUT_#",
# where # is a number, 1 - 4, are variables used as constants, and reading as a string does not work.
self.ColorPort = eval(conf.get('Sensors', 'ColorPort'))
self.GyroPort = eval(conf.get('Sensors', 'GyroPort'))
self.InfraredPort = eval(conf.get('Sensors', 'InfraredPort'))
self.TouchPort = eval(conf.get('Sensors', 'TouchPort'))
self.UltrasonicPort = eval(conf.get('Sensors', 'UltrasonicPort'))
# Reads and stores the PID gains for following a line.
self.kpLine = float(conf.get('Sensors', 'kpLine'))
self.kiLine = float(conf.get('Sensors', 'kiLine'))
self.kdLine = float(conf.get('Sensors', 'kdLine'))
# Read the auxillary motor ports, if any, from the config file, and store. "eval()" is used because the port names "OUTPUT_#",
# where # is a capital letter, A - D, are variables used as constants, and reading as a string does not work.
self.AuxMotor1 = eval(conf.get('AuxMotors', 'AuxMotor1'))
self.AuxMotor2 = eval(conf.get('AuxMotors', 'AuxMotor2'))
# Instantiate the auxillary motor objects, checking if they are connected first.
if self.AuxMotor1 is not None:
try:
self.m1 = MediumMotor(self.AuxMotor1)
except:
print("Aux Motor 1 Not Connected!")
self.spkr.beep('-f 220')
if self.AuxMotor2 is not None:
try:
self.m2 = MediumMotor(self.AuxMotor2)
except:
print("Aux Motor 2 Not Connected!")
self.spkr.beep('-f 220')
sleep(0.3)
self.spkr.beep('-f 220')
# Instantiate the sensor objects
self.cs = ColorSensor(self.ColorPort)
self.gs = GyroSensor(self.GyroPort)
# Only instantiate auxillary sensors if the config file shows they exist
if self.InfraredPort is not None:
try:
self.ir = InfraredSensor(self.InfraredPort)
except:
pass
if self.UltrasonicPort is not None:
try:
self.us = UltrasonicSensor(self.UltrasonicPort)
except:
pass
if self.TouchPort is not None:
try:
self.ts = TouchSensor(self.TouchPort)
except:
pass
# Instantiate the drive motor objects, as well as the motorset objects for controlling both simultainiously
self.tank = MoveTank(self.LeftMotor, self.RightMotor)
self.steer = MoveSteering(self.LeftMotor, self.RightMotor)
self.lm = LargeMotor(self.LeftMotor)
self.rm = LargeMotor(self.RightMotor)
# Reset the gyro angle to zero by switching modes
self.gs._ensure_mode(self.gs.MODE_GYRO_G_A)
self.cs._ensure_mode(self.cs.MODE_COL_REFLECT)
# If the motors are inverted xor the gear ratio is negative, set the motor poloraity to be inverted,
# so normal motor commands will run the motors in the opposite direction.
if self.MotorInverted ^ (self.GearRatio / abs(self.GearRatio) == -1):
self.lm.polarity = "inversed"
self.rm.polarity = "inversed"
self.tank.set_polarity = "inversed"
else:
self.lm.polarity = "normal"
self.rm.polarity = "normal"
self.tank.set_polarity = "normal"
# Set the integer GyroInvertedNum to reflect the boolean GyroInverted, with -1 = True, and 1 = False,
# for use in calculations later
if self.GyroInverted:
self.GyroInvertedNum = -1
else:
self.GyroInvertedNum = 1
# Beep to signify the robot isdone initialization (it takes a while)
self.spkr.beep()
def reflectCal(self):
"""
"Calibrate" the color sensor using high and low setpoints, and a simple linear mapping.
The adjusted value can then be accessed using robot.correctedRLI, and the raw value can be acessed using
robot.cs.reflected_light_intensity.
Use: When called, the robot will emit a high-pitched beep. This is a signal to place the robot on a white surface
and press the center button. The robot then emits a low-pitched beep. This is a signal to repeat the procedure with a
black surface.
"""
# These variables need to be accessed by the correctedRLI function, and thus need to be global.
global reflHighVal
global reflLowVal
global reflRate
# Signal for white
self.spkr.beep('-f 880')
# Wait for user conformation of a white surface
self.btn.wait_for_bump('enter')
# Set High fixpoint
reflHighVal = (self.cs.reflected_light_intensity)
# Conformation of completion
self.spkr.beep()
# Signal for black
self.spkr.beep('-f 220')
# Wait for user conformation of a black surface
self.btn.wait_for_bump('enter')
# Set Low fixpoint
reflLowVal = self.cs.reflected_light_intensity
# Conformation of completion
self.spkr.beep()
# Calculate the slope of the linear function that maps the fixpoints to 0 - 100
reflRate = 100 / (reflHighVal - reflLowVal)
@property
def correctedRLI(self):
"""
Returns the reflected light intensity from the color sensor, scaled based on the high and low values created by reflectCal
This means LineFollow can use 50 as the target, even though the actual reading for black might be 20, and white 75, as it
will be scaled to 0 - 100.
"""
# Calculates adjusted value with a linear mapping. To see how this works go here: https://www.desmos.com/calculator/d4mudhrdng
value = min([
100,
max([
0, (self.cs.reflected_light_intensity * reflRate) +
(-reflLowVal * reflRate)
])
])
return (value)
@property
def correctedAngle(self):
"""
Retuns the gyro value corrected for the orientation of the gyro in the robot; turning right will always increase the value,
and turning left will always decrease the value. The raw value can be accessed with robot.gs.angle_and_rate[0]
Angle and rate is used to prevent switching modes and resetting the angle.
"""
# Multiply the gyro angle by -1 if the gyro is mounted upside-down relative to the motors in the robot.
# GyroInvertedNum is set up in __init__()
return (self.gs.angle_and_rate[0] * self.GyroInvertedNum)
def zeroGyro(self):
"""
Reset the gyro angle to zero by switching modes. gyro.reset would have been used instead of this function,
but it does not work
"""
self.gs._ensure_mode(self.gs.MODE_GYRO_RATE)
self.gs._ensure_mode(self.gs.MODE_GYRO_G_A)
def DriveAtHeading(self, Heading, Distance, Speed, Stop):
"""
Moves the robot in a specified direction at a specified speed for a certian number of centimeters, while using the gyro sensor to keep the robot moving in a straight line.
``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero.
``Distance``: The distance to drive, in centimeters (positive only).
``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards.
``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm.
"""
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
if Distance <= 0:
print(
"Distance must be greater than zero. Use negative speed to drive backwards."
)
return
elif Distance > 265:
# The longest distance on an FLL table (diagonal) is about 265cm.
if self.ForFLL:
print("Please don't use silly distances (max = 265cm)")
return
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
# "Reset" motor encoders by subtracting start values
left_motor_start = self.lm.degrees
right_motor_start = self.rm.degrees
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
# Determine the sign of the speed, for PID correction
sign = Speed / abs(Speed)
# Find number of degrees that motors need to rotate to reach the desired number of cm.
target = (Distance * 360) / self.WheelCircumference * abs(
self.GearRatio)
# Find the average of the left and right encoders, as they could be different from PID correction
avg = abs((left_motor_change + right_motor_change) / 2)
# Initialize variables for PID control
integral = 0.0
last_error = 0.0
derivative = 0.0
# Check if the motors have gone far enough
while avg < target:
# Read the gyro
current_angle = self.correctedAngle
# Calculate the PID components
error = current_angle - Heading
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kp * error) + (self.ki * integral) +
(self.kd * derivative), 100]), -100
])
# Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop.
if Stop == False:
speedMult = 1
else:
# Check if the robot has gone 70% or more of the distance. If so, start slowing down
if (target - avg) <= (0.3 * target):
# Calculate the pecrentage of the distance left to travel
targDist = 1 - (avg / target)
# Calculate speedMult based on pecentage; linear function was designed to bring the robot to a
# smooth stop while still reaching the target, resulting in 20% of the intial speed at end.
speedMult = ((8 / 3) * targDist) + 0.2
else:
speedMult = 1
# Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above).
self.steer.on(-turn_native_units, (Speed * speedMult))
# Update corrected encoder values
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
avg = abs((left_motor_change + right_motor_change) / 2)
# If the robot is to stop, stop the motors. Otherwise, leave the motors on and return.
if not Stop == False:
self.steer.stop()
def degrees2power(self, currentDifference):
"""
Returns a power value based on the degrees left to turn, using a linear function that allows the robot
to reach the desired angle, while slowing down as to not overshoot. Used only in GyroTurn.
"""
if currentDifference == 0:
# Minimum power value is 5
return (5)
# Using and returning absolute values, for simplicity. GyroTurn fixes the sign.
currentDifference = abs(currentDifference)
# Return calculated value
return (min([50, max([5, ((0.125 * currentDifference) + 4.875)])]))
def GyroTurn(self, Heading):
"""
Turns the robot to ``Heading``, slowing down as it approaches. Will unwind any full circles
(if the gyro reads 540, and the value given is 90, the robot will spin until reaching 90, not 450).
"""
# Initalize the sign variable (used to correct overshoot)
sign = 1
# If the current heading is equal to the desired heading, no turning is needed.
if Heading - self.correctedAngle == 0:
return
# Read the gyro, and store in currentHeading
currentHeading = self.correctedAngle
# Continue turning until the robot is within 1 degree of the target (can be reduced if necessary)
while (currentHeading > 0.5 + Heading) or (currentHeading <
Heading - 0.5):
# Calculate the difference between where the robot should be and where it is
currentDifference = Heading - currentHeading
# The sign variable defines the direction in which to turn. It should have the same sign as the currentDifference variable.
# If not, multiply by -1 to fix it.
if ((sign > 0) and
(currentDifference < 0)) or ((sign < 0) and
(currentDifference > 0)):
sign *= -1
# Set the power variable to the absolute power (calculated by degrees2power) multiplied by the sign variable
power = sign * self.degrees2power(currentDifference)
# Start turning
self.tank.on(power, -1 * power)
# Update currentHeading
currentHeading = self.correctedAngle
# When the loop finishes, stop the motors.
self.tank.stop()
def ArcTurn(self, Degrees, Radius, Speed):
"""
Drive the robot in an arc with a specified radius, for a certain number of degrees.
``Degrees``: The number or degrees to drive around the arc. Positive will turn the front of the robot right, negative left (car turn).
``Radius``: The radius of the arc to drive, in cm. Must be positive.
``Speed``: The speed at which to drive around the arc, in percentage of full power (same units as EV3-G). Negative will drive
backwards.
"""
# Ensure the parameters are within reasonable limits, and adjust/reject as necessary.
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
if Radius <= 0:
print(
"Radius must be greater than zero. Use negative degrees to turn the opposite direction."
)
return
# No point in driving in circles
Degrees = math.fmod(Degrees, 360)
# Read the angle of the robot, and store in startHeading
startHeading = self.correctedAngle
# If both Degrees and Speed have the same sign, the right wheel should be slowed down.
# Otherwise, slow down the left wheel.
if ((Degrees > 0) and (Speed > 0)) or ((Degrees < 0) and (Speed < 0)):
self.tank.on(Speed,
(Radius - self.WidthBetweenWheels) * Speed / Radius)
else:
self.tank.on((Radius - self.WidthBetweenWheels) * Speed / Radius,
Speed)
# If Degrees is positive, wait for the degrees turned to become >= Degrees (to turn).
# Otherwise, wait for the degrees turned to become <= Degrees
if Degrees > 0:
while (self.correctedAngle - startHeading) < Degrees:
pass
else:
while (self.correctedAngle - startHeading) > Degrees:
pass
# Stop the motors
self.tank.stop()
def DriveBump(self, Heading, Speed):
"""
Moves the robot in a specified direction at a specified speed until it hits something, while using the gyro sensor to keep the robot moving in a straight line.
``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero.
``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards.
"""
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
# Check and store the sign of the input speed (for PID correction), and convert the target speed to encoder ticks per second
sign = Speed * -1 / abs(Speed)
target = abs((self.lm.max_speed * Speed) / 100)
# Initialize variables for PID control
integral = 0.0
last_error = 0.0
derivative = 0.0
# Read the gyro
current_angle = self.correctedAngle
# Calculate the PID components
error = current_angle - Heading
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kp * error) + (self.ki * integral) +
(self.kd * derivative), 100]), -100
])
# Start the motors without speed regulation, using the Steering value and Speed
lsrs = self.steer.get_speed_steering(
turn_native_units, Speed) # lsrs = left-speed right-speed
lsNative = lsrs[0]
rsNative = lsrs[1]
self.lm.on(SpeedNativeUnits(lsNative))
self.rm.on(SpeedNativeUnits(rsNative))
# Wait for motors to get up to speed, then check and store the average speed (between the two motors)
time.sleep(1)
avgSpd = abs((self.lm.speed + self.rm.speed) / 2)
# Check if the motors have slowed down (because the robot hit something)
while avgSpd > 0.90 * target:
# Read the gyro
current_angle = self.correctedAngle
# Calculate the PID components
error = current_angle - Heading
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kp * error) + (self.ki * integral) +
(self.kd * derivative), 100]), -100
])
# Start the motors without speed regulation, using the Steering value and Speed
lsrs = self.steer.get_speed_steering(turn_native_units, Speed)
lsNative = lsrs[0]
rsNative = lsrs[1]
self.lm.on(SpeedNativeUnits(lsNative))
self.rm.on(SpeedNativeUnits(rsNative))
# Check and store the average speed again
avgSpd = abs((self.lm.speed + self.rm.speed) / 2)
# Stop the motors
self.lm.stop()
self.rm.stop()
def AuxMotorBumpStop(self, Speed, Threshold, Port):
"""
Similar to DriveBump, will run an auxillary motor until the speed drops below ``Threshold``% of ``Speed``.
``Speed``: Speed at which to run the motor, in motor percentage (Same units as EV3-G).
``Threshold``: Percentage of ``Speed`` that the motor speed must drop below before shutting off.
``Port``: Motor port.
"""
# Most if statments are only because there are two possible ports; one case for each port
# Ensure values are within reasonable, and adjust/reject as necessary
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
if Threshold <= 0:
print("Threshold must be greater than zero and less than one")
return
elif Threshold > 100:
print(
"Threshold must be greater than zero and less than or equal to 100"
)
return
# Different commands for different motors, check which motor to use
if Port == self.AuxMotor1:
# Find the target speed in encoder ticks per second
target = abs((self.m1.max_speed * Speed) / 100)
# Start the motor, with the calculated speed
self.m1.on(SpeedNativeUnits(target))
# Wait for the motor to get up to speed
time.sleep(0.5)
# Set motrspeed to the current motor speed
motrspeed = abs(self.m1.speed)
else:
# Everything here is the same as the first case statement, but using m2 instead of m1 (motor 2, not motor 1)
target = abs((self.m2.max_speed * Speed) / 100)
self.m2.on(SpeedNativeUnits(target))
time.sleep(0.5)
motrspeed = abs(self.m2.speed)
# Keep the motor on until the motor speed drops below Threshold% of Speed
while motrspeed > (target * Threshold) / 100:
if Port == self.AuxMotor1:
# Update motrspeed until the motor slows down enough to stop
motrspeed = abs(self.m1.speed)
else:
# Same as before, just for the other motor
motrspeed = abs(self.m2.speed)
# Shut off the motor
if Port == self.AuxMotor1:
self.m1.off()
else:
self.m2.off()
def LineStop(self, Heading, Speed, Stop):
"""
Moves the robot in a specified direction at a specified speed until a line (White-Black) is seen, while using the gyro sensor to keep the robot moving in a straight line.
``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero.
``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards.
``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm.
"""
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
# Check and store the sign of the input speed for PID correction
sign = Speed / abs(Speed)
# Set the color sensor to "Color" mode
self.cs._ensure_mode(self.cs.MODE_COL_COLOR)
# Set the brick light to solid amber
#EV3.SetLEDColor("ORANGE", "NORMAL")
# Initialize variables for PID control and end checking
integral = 0.0
last_error = 0.0
derivative = 0.0
end = False
seenWhite = False
# Check if the motors have gone far enough
while not end:
# Read the gyro
current_angle = self.correctedAngle
# Calculate the PID components
error = current_angle - Heading
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kp * error) + (self.ki * integral) +
(self.kd * derivative), 100]), -100
])
# Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above).
self.steer.on(-turn_native_units, (Speed))
# Check if the sensor is seeing white
color = self.cs.color
if color == self.cs.COLOR_WHITE:
seenWhite = True
elif (color == self.cs.COLOR_BLACK) and seenWhite:
end = True
elif color == self.cs.COLOR_BLUE:
seenWhite = seenWhite
else:
seenWhite = False
# If the robot is to stop, stop the motors. Otherwise, leave the motors on.
if not Stop == False:
self.steer.stop()
# Set the brick light back to green flashing
#EVS.SetLEDColor("GREEN", "PULSE")
def LineFollow(self, Distance, Speed, Stop):
"""
Follows the edge of a line for a specific distance at a specific speed.
``Distance``: The distance to drive, in centimeters (positive only).
``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). (Positive only; there's no going back)
``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm.
"""
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
if Speed > 50:
Speed = 50
print("Speed must be between -50 and 50 (inclusive).")
elif Speed < -50:
Speed = -50
print("Speed must be between -50 and 50 (inclusive).")
# "Reset" motor encoders by subtracting start values
left_motor_start = self.lm.degrees
right_motor_start = self.rm.degrees
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
# Determine the sign of the speed, for PID correction
sign = Speed / abs(Speed)
# Find number of degrees that motors need to rotate to reach the desired number of cm.
target = (Distance * 360) / self.WheelCircumference * abs(
self.GearRatio)
# Find the average of the left and right encoders, as they could be different from PID correction
avg = abs((left_motor_change + right_motor_change) / 2)
# Initialize variables for PID control
integral = 0.0
last_error = 0.0
derivative = 0.0
# Check if the motors have gone far enough
while avg < target:
# Read the gyro
current_RLI = self.correctedRLI
# Calculate the PID components
error = 50 - current_RLI
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kpLine * error) + (self.kiLine * integral) +
(self.kdLine * derivative), 100]), -100
])
# Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop.
if Stop == False:
speedMult = 1
else:
# Check if the robot has gone 70% or more of the distance. If so, start slowing down
if (target - avg) <= (0.3 * target):
# Calculate the pecrentage of the distance left to travel
targDist = 1 - (avg / target)
# Calculate speedMult based on pecentage; linear function was designed to bring the robot to a
# smooth stop while still reaching the target.
speedMult = ((8 / 3) * targDist) + 0.2
else:
speedMult = 1
# Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above).
self.steer.on(-turn_native_units, (Speed * speedMult))
# Update corrected encoder values
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
avg = abs((left_motor_change + right_motor_change) / 2)
# If the robot is to stop, stop the motors. Otherwise, leave the motors on and return.
if not Stop == False:
self.steer.stop()
def TriangleAvoid(self, Heading, Distance, Speed):
"""
Moves the robot in a specified direction at a specified speed for a certian number of centimeters, while using the gyro sensor to keep the robot moving in a straight line.
``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero.
``Distance``: The distance to drive, in centimeters (positive only).
``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards.
``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm.
"""
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
if Distance <= 0:
print(
"Distance must be greater than zero. Use negative speed to drive backwards."
)
return
elif Distance > 265:
# The longest distance on an FLL table (diagonal) is about 265cm.
if self.ForFLL:
print("Please don't use silly distances (max = 265cm)")
return
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
if not self.us:
print("Avoidance Functions need the US sensor to work.")
return
# "Reset" motor encoders by subtracting start values
left_motor_start = self.lm.degrees
right_motor_start = self.rm.degrees
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
# Determine the sign of the speed, for PID correction
sign = Speed / abs(Speed)
# Find number of degrees that motors need to rotate to reach the desired number of cm.
target = (Distance * 360) / self.WheelCircumference * abs(
self.GearRatio)
# Find the average of the left and right encoders, as they could be different from PID correction
avg = abs((left_motor_change + right_motor_change) / 2)
# Find the number of centimeters driven and left to go
driven_so_far = (avg * self.WheelCircumference) / 360
left_to_go = Distance - driven_so_far
# Initialize variables for PID control
integral = 0.0
last_error = 0.0
derivative = 0.0
# Check if the motors have gone far enough
while avg < target:
# Read the gyro and ultrasonic sensors
current_angle = self.correctedAngle
dist_to_obstacle = self.us.distance_centimeters
# Calculate the PID components
error = current_angle - Heading
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kp * error) + (self.ki * integral) +
(self.kd * derivative), 100]), -100
])
# Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop.
# Check if the robot has gone 70% or more of the distance. If so, start slowing down
if (target - avg) <= (0.3 * target):
# Calculate the pecrentage of the distance left to travel
targDist = 1 - (avg / target)
# Calculate speedMult based on pecentage; linear function was designed to bring the robot to a
# smooth stop while still reaching the target, resulting in 20% of the intial speed at end.
speedMult = ((8 / 3) * targDist) + 0.2
else:
speedMult = 1
if (dist_to_obstacle <= 30) and (left_to_go > 30):
self.steer.off(brake=False)
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
avg = abs((left_motor_change + right_motor_change) / 2)
driven_so_far = (avg * self.WheelCircumference) / 360
dist_to_obstacle = self.us.distance_centimeters
start_angle = self.correctedAngle
while self.us.distance_centimeters < 60:
self.tank.on(-10, 10)
self.tank.off()
self.GyroTurn(self.correctedAngle - 5)
end_angle = self.correctedAngle
degrees_turned = abs(end_angle - start_angle)
first_hypotenuse = dist_to_obstacle / math.cos(
math.radians(degrees_turned))
self.DriveAtHeading(end_angle, first_hypotenuse, 20, True)
second_triangle_short_leg = math.sqrt((first_hypotenuse**2) -
(dist_to_obstacle**2))
second_triangle_long_leg = Distance - (driven_so_far +
dist_to_obstacle + 2.21)
second_triangle_second_angle = math.degrees(
math.atan(second_triangle_long_leg /
second_triangle_short_leg))
degrees_to_turn = 90 - second_triangle_second_angle
self.GyroTurn(Heading)
self.GyroTurn(Heading + degrees_to_turn)
second_hypotenuse = math.sqrt((second_triangle_long_leg**2) +
(second_triangle_short_leg**2))
self.TriangleAvoid(self.correctedAngle, second_hypotenuse, 20)
self.GyroTurn(Heading)
return
# Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above).
self.steer.on(-turn_native_units, (Speed * speedMult))
# Update corrected encoder values
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
avg = abs((left_motor_change + right_motor_change) / 2)
driven_so_far = (avg * self.WheelCircumference) / 360
# Stop the motors.
self.steer.stop()
def CircleAvoid(self, Heading, Distance, Speed):
"""
Moves the robot in a specified direction at a specified speed for a certian number of centimeters, while using the gyro sensor to keep the robot moving in a straight line.
``Heading``: The angle at which to drive, with the direction the gyro was last calibrated in being zero.
``Distance``: The distance to drive, in centimeters (positive only).
``Speed``: The speed at which to drive, in motor percentage (same speed units as EV3-G). A negative value will make the robot drive backwards.
``Stop``: Stop motors after completion. If ``FALSE``, motors will continue running after ``Distance`` has been traveled. Otherwise, motors will stop after ``Distance`` cm.
"""
# Ensure values are within reasonable limits, and change them if necessary (Idiotproofing).
if Distance <= 0:
print(
"Distance must be greater than zero. Use negative speed to drive backwards."
)
return
elif Distance > 265:
# The longest distance on an FLL table (diagonal) is about 265cm.
if self.ForFLL:
print("Please don't use silly distances (max = 265cm)")
return
if Speed > 75:
Speed = 75
print("Speed must be between -75 and 75 (inclusive).")
elif Speed < -75:
Speed = -75
print("Speed must be between -75 and 75 (inclusive).")
if not self.us:
print("Avoidance Functions need the US sensor to work.")
return
# "Reset" motor encoders by subtracting start values
left_motor_start = self.lm.degrees
right_motor_start = self.rm.degrees
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
# Determine the sign of the speed, for PID correction
sign = Speed / abs(Speed)
# Find number of degrees that motors need to rotate to reach the desired number of cm.
target = (Distance * 360) / self.WheelCircumference * abs(
self.GearRatio)
# Find the average of the left and right encoders, as they could be different from PID correction
avg = abs((left_motor_change + right_motor_change) / 2)
# Find the number of centimeters driven and left to go
driven_so_far = (avg * self.WheelCircumference) / 360
left_to_go = Distance - driven_so_far
# Initialize variables for PID control
integral = 0.0
last_error = 0.0
derivative = 0.0
# Check if the motors have gone far enough
while avg < target:
# Read the gyro and ultrasonic sensors
current_angle = self.correctedAngle
dist_to_obstacle = self.us.distance_centimeters
# Calculate the PID components
error = current_angle - Heading
integral = integral + error
derivative = error - last_error
last_error = error
# Calculate Steering value based on PID components and kp, ki, and kd
turn_native_units = sign * max([
min([(self.kp * error) + (self.ki * integral) +
(self.kd * derivative), 100]), -100
])
# Check if the motors will stop at the end. If not, the speed will be adjusted to come to a smooth stop.
# Check if the robot has gone 70% or more of the distance. If so, start slowing down
if (target - avg) <= (0.3 * target):
# Calculate the pecrentage of the distance left to travel
targDist = 1 - (avg / target)
# Calculate speedMult based on pecentage; linear function was designed to bring the robot to a
# smooth stop while still reaching the target, resulting in 20% of the intial speed at end.
speedMult = ((8 / 3) * targDist) + 0.2
else:
speedMult = 1
if (dist_to_obstacle <= 30) and (left_to_go > 30):
self.steer.off(brake=False)
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
avg = abs((left_motor_change + right_motor_change) / 2)
driven_so_far = (avg * self.WheelCircumference) / 360
dist_to_obstacle = self.us.distance_centimeters
start_angle = self.correctedAngle
while self.us.distance_centimeters < 60:
self.tank.on(-10, 10)
self.tank.off()
self.GyroTurn(self.correctedAngle - 5)
end_angle = self.correctedAngle
degrees_turned = abs(end_angle - start_angle)
second_point_y = dist_to_obstacle * math.tan(
math.radians(degrees_turned))
third_point_x = Distance - driven_so_far
x_numerator = -(third_point_x**2) * second_point_y
y_numerator = -((dist_to_obstacle**2) * third_point_x) - (
(second_point_y**2) * third_point_x) + (
(third_point_x**2) * dist_to_obstacle)
denominator = -2 * third_point_x * second_point_y
h = x_numerator / denominator
k = y_numerator / denominator
radius = math.sqrt((h**2) + (k**2))
begin_arc_angle = math.degrees(math.atan(h / -k))
arc_angle = -2 * begin_arc_angle
self.GyroTurn(Heading + begin_arc_angle)
self.ArcTurn(arc_angle, radius, 10)
self.GyroTurn(Heading)
return
# Start the motors, using the steering value calculated by the PID control, and the input speed multiplied by speedMult (0 - 1, see above).
self.steer.on(-turn_native_units, (Speed * speedMult))
# Update corrected encoder values
left_motor_now = self.lm.degrees
right_motor_now = self.rm.degrees
left_motor_change = left_motor_now - left_motor_start
right_motor_change = right_motor_now - right_motor_start
avg = abs((left_motor_change + right_motor_change) / 2)
driven_so_far = (avg * self.WheelCircumference) / 360
# Stop the motors.
self.steer.stop()
