def followLine(v, stop_func, p0=0.0, sensor="right"):
target = 50.0
kp = 0.88
kd = 0.10
ki = 0.02
scale = 0.25
sensor_func = getColorRight
speed = v
error = 0.0
last_error = 0.0
delta = 0.0
integral = 0.0
loop_count = 0
pdi = 0.0
change = 0.0
if sensor == "left":
sensor_func = getColorLeft
scale = scale * -1.0
print("left")
while stop_func(loop_count, pdi, change, p0):
loop_count += 1
error = (target - sensor_func()) / 50.0
delta = last_error - error
last_error = error
integral = integral + error
pdi = (kp * error + kd * delta + ki * integral)
change = pdi * scale * speed
motor_l.run(speed + change)
motor_r.run(speed - change)
def compute_robot_spin_degrees(self, left_angle, right_angle):
"Uses configuration of robot and math to figure out gyro value"
if self.debug:
print("compute spin degrees", left_angle, right_angle)
# assert ((right_angle >= 0 and left_angle <= 0) or (right_angle <= 0 and left_angle >= 0))
spin_left = right_angle > 0
positive_wheel_degrees = right_angle if right_angle > 0 else left_angle
# how far has the wheel going forward moved?
# C = 2*pi*r
# distance wheel traveled = (radians traveled)*wheel_radius
# wheel_distance = self.deg2rad(positive_wheel_degrees) * self.wheel_radius
# print("Forward wheel has moved (inches)", wheel_distance)
# so, one wheel has gone part of circle the above distance;
# what angle for this zero-radius turn has it gone?
# C = 2*pi*r
# part of circle = (radians traveled on circle) * (radius of circle)
# robot_spin_degrees = wheel_distance / self.axis_radius
# print("my robot_spin_degrees: ", robot_spin_degrees)
# I keep screwing this up, so here's it explained, and simplified:
# https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=12&ved=2ahUKEwi8y-K0pM_oAhVAgXIEHdapC8cQFjALegQIAxAB&url=https%3A%2F%2Fsheldenrobotics.com%2Ftutorials%2FDetailed_Turning_Tutorial.pdf&usg=AOvVaw3FCQ_gyV_xrhMu2iZsDdBl
# Number of Degrees to Program = (Cturn) / (Cwheel) x (theta)
# positive_wheel_degrees = (self.axis_radius/self.wheel_radius) * robot_spin_degree
robot_spin_degrees = positive_wheel_degrees * (self.wheel_radius /
self.axis_radius)
return robot_spin_degrees if not spin_left else -robot_spin_degrees
def turnDegrees(self, degrees):
self.gyro.reset_angle(0)
initial_power = 300
end_power = 50
left_motor_power = initial_power
right_motor_power = initial_power * -1
if degrees < 0:
left_motor_power = initial_power * -1
right_motor_power = initial_power
initial_turn = abs(degrees * .75)
self.left_motor.run(left_motor_power)
self.right_motor.run(right_motor_power)
angle = self.gyro.angle()
print("Angle: " + str(angle))
while abs(angle) < initial_turn:
wait(10)
angle = self.gyro.angle()
print("Angle: " + str(angle))
left_motor_power = end_power
right_motor_power = end_power * -1
if degrees < 0:
left_motor_power = end_power * -1
right_motor_power = end_power
self.left_motor.run(left_motor_power)
self.right_motor.run(right_motor_power)
end_degrees = (abs(degrees) - 1)
angle = self.gyro.angle()
print("Angle: " + str(angle))
while abs(angle) < end_degrees:
wait(10)
angle = self.gyro.angle()
print("Angle: " + str(angle))
self.left_motor.stop(Stop.BRAKE)
self.right_motor.stop(Stop.BRAKE)
print("Final Angle: " + str(self.gyro.angle()))
def Ziel(): #Definition state Ziel
global state
print("State: Ziel")
brick.sound.beep #Roboter piept
wait(500) #Wartet 0,5 Sekunden
brick.sound.beep #Roboter piept
state = 5 #Wechselt state zu 5
def printMatrix(matrix):
text = "-----\n"
for x in range(len(matrix)):
for y in range(len(matrix[0])):
text += symbols[int(matrix[x][y])]
text = text + "\n"
print(text)
def MusicPlay():
while True:
# create variables for note and duration and set them to 0 and 1, then create delay as 2.
# This will call the first and second items from the list. 0 is always the first item.
track = random.randint(0, 2)
if track == 0:
trackmusic = MKombat
elif track == 1:
trackmusic = bevCop
elif track == 2:
trackmusic = misImpo
trackmusic = misImpo
print(track)
print(trackmusic)
note = 0
duration = 1
delay = 2
#Determines the length of the music so the program doesn't crash
length = len(trackmusic) / 3
lentest = 0
# using the FOR loop we can say that for every item in a list do something...
for x in trackmusic:
# play the note and duration on the brick, then wait the listed delay
brick.sound.beep(trackmusic[note], trackmusic[duration])
wait(trackmusic[delay])
# move to the next note and duration in the list by adding 3 to each variable
note += 3
duration += 3
# test the length of the song compared to how much has been played
lentest += 1
if lentest == length:
break
def Music():
global list
global darudeList
global megaList
changeSong = False
list = darudeList
MusicVar = False
global SelectButton
global selectionMade
global touch
while True:
changeSong = False
#Changes to the next track, then rolls track back to start if too many
if SelectButton.pressed():
brick.sound.beep(1000,100)
if MusicVar == False:
MusicVar = True
else:
MusicVar = False
print('triggeredSelect')
while SelectButton.pressed():
wait(5)
#Detects if Pause has been triggered off (starts on for music selection)
#then creates a loop of the select track
if not TOUCHED:
selectionMade = 1
print(MusicVar)
if selectionMade == 1:
if MusicVar == False:
list = darudeList
else:
list = megaList
while True:
MusicPlay()
break
def object_sound():
global object_detected
global object_detected_1
global object_detected_2
global object_detect_run
global object_detect_run_2
global object_detect_loop
global object_detect_sound_on
global ObjectDetectSoundPrint
if object_detected and object_detect_run and object_detect_sound_on:
if ObjectDetectSoundPrint:
print("Object Detect Sound Triggered:", " Object_detected_1= ", object_detected_1, " Object_detected_2= ", object_detected_2 )
if (object_detected_1 and not object_detect_run_2):
sound_tools.play_file(SoundFile.DETECTED)
elif(object_detected_2 and not object_detect_run_1):
sound_tools.play_file(SoundFile.DETECTED)
elif object_detected_1 and not object_detected_2:
sound_tools.play_file(SoundFile.DETECTED)
sound_tools.play_file(SoundFile.ONE)
elif not object_detected_1 and object_detected_2:
sound_tools.play_file(SoundFile.DETECTED)
sound_tools.play_file(SoundFile.TWO)
elif object_detected_1 and object_detected_2:
sound_tools.play_file(SoundFile.DETECTED)
sound_tools.play_file(SoundFile.ONE)
sound_tools.play_file(SoundFile.TWO)
def bilderZeigen():
'''
Zeigt alle Bilder
'''
# Ist zu kompliziert im Moment
import inspect
members = inspect.getmembers(ImageFile)
members = [(name, data) for (name, data) in members
if not name.startswith('_')]
i = 0
while True:
name, data = members[i]
print('ImageFile.{}'.format(name))
brick.display.clear()
brick.display.image(data)
brick.display.text("")
brick.display.text('ImageFile.{}'.format(name))
wait(50)
buttons = brick.buttons()
while not any(buttons):
buttons = brick.buttons()
wait(10)
if Button.RIGHT in buttons:
i += 1
if Button.LEFT in buttons:
i -= 1
if Button.CENTER in buttons:
pass
if Button.UP in buttons:
i = 0
if Button.DOWN in buttons:
break
print("i={}".format(i))
if abs(i) >= len(members):
i = 0
def run(self, angle: int):
"""
Start turing in place for a specified angle.
----------
angle : int – angle to turn (clockwise - positive).
"""
# Setup starting values
stop_watch = StopWatch()
self.pid.setpoint = angle
self.gyro_sensor.reset_angle(0)
last_angle = angle + 100
last_rotation = 0
while not abs(self.pid.setpoint -
last_angle) < TurnToAngleBehavior.ACCURACY_ANGLE:
angle = self.gyro_sensor.angle()
rotation = self.pid(angle)
if last_rotation != rotation:
self.bot.drive(0, rotation)
last_angle = angle
last_rotation = rotation
print(stop_watch.time(), ' -- angle:', angle, ', rotation: ',
rotation, ', error: ', (angle - self.pid.setpoint))
def connect(self):
tools.print('Connecting to remote brick: ' + self.server_brick_name)
self._client = messaging.BluetoothMailboxClient()
self._client.connect(self.server_brick_name)
tools.print('Connected to %r.' % self.server_brick_name)
self.cmd_mbx = RPCMailbox(COMMAND_MAILBOX_NAME, self._client)
self.res_mbx = RPCMailbox(RESULT_MAILBOX_NAME, self._client)
def doubleGreen():
contGreen = 0
motorMovementTurn.drive(-64, 0)
wait(100)
motorMovementForward.drive(0, 0)
wait(100)
print("Loop from confirmed Double Sensor")
for i in range(1000):
if sensorColorLeft.color() == 2 and sensorColorRight.color() == 2:
contGreen += 1
if contGreen >= 999:
brick.sound.beep()
print("Green")
brick.display.text("Left Green", (60, 50))
motorMovementForward.drive(-100, 0)
wait(512)
motorMovementTurn.drive(0, 128)
wait(1256)
motorMovementTurn.drive(0, 128)
while sensorColorRight.reflection() > 30:
wait(10)
motorMovementTurn.drive(0, -256)
wait(430)
brick.display.clear()
contGreen = 0
def main():
brick.sound.beep()
sens1 = LegoPort(address ='ev3-ports:in1') # which port?? 1,2,3, or 4
#sens2 = LegoPort(address ='ev3-ports:in2') # which port?? 1,2,3, or 4
sens1.mode = 'ev3-analog'
#sens2.mode = 'ev3-analog'
utime.sleep(0.5)
sensor_left=MySensor(Port.S1) # same port as above
sensor_right=MySensor(Port.S2) # same port as above
left_motor = Motor(Port.A)
right_motor = Motor(Port.D)
speed = 0
value = 100
while True:
left_color = sensor_left.readvalue()
right_color = sensor_right.readvalue()
print('left sensor is ', left_color)
print('right sensor is ', right_color)
left_motor.run(speed)
right_motor.run(speed)
def printAngle():
global DTFlag # signal to write motor drive time in log
i=0
told = watch.time()/1000 # units of seconds
vbat = brick.battery.voltage()/1000.0 # convert millivolts to V
logfile = open ("log4.csv", "w") # open data log file
logfile.write("seconds,motor_angle,gyroM,Dg1,Dg2,Dg3\n")
logfile.write("# EV3 logfile w2.py Vbat = %5.3f\n" % (vbat))
print("# Vbat = %5.3f" % (vbat))
while not done:
t_elapsed = watch.time()/1000 - tstart
mot_angle = mB.angle()-mC.angle() # motor encoder reading
a1 = gy1.angle() # first gyro reading
a2 = gy2.angle() # second gyro reading
a3 = gy3.angle() # third gyro reading
am = (a1+a2+a3)/3 # average of gyro readings
i += 1 # ye olde loop counter
if (i%5 == 0) :
logfile.write("%6.3f, %d, %d, %d, %d, %d\n" %
(t_elapsed,mot_angle,am,am-a1,am-a2,am-a3) )
if (i%400 == 0):
vbat = brick.battery.voltage()/1000.0 # convert millivolts to V
logfile.write("# %6.3f , Vbat = %5.3f\n" % (t_elapsed,vbat))
if (DTFlag):
logfile.write("# Drive Time: %5.3f\n" % (driveTime))
DTFlag = False
sleep(0.01) # yield some time to overworked task scheduler
# Here: (done==True) so finish up and close out
vbat = brick.battery.voltage()/1000.0 # in millivolts
logfile.write("# END RUN Vbat = %5.3f\n" % (vbat))
logfile.close() # all done writing to log file
def connect():
global client_skt
client_skt = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
con_result = client_skt.connect((SERVER_IP, SERVER_PORT))
except:
print('connect exception')
def record_mdp(self, state, action, reward, next_state):
"""
Use the state/action/reward/next_state pairs from real-life experience to populate the MDP
:param state: Current state
:param action: action
:param reward: Immediate reward of doing an "action" at state "state"
:param next_state: Next state after doing an "action" at state "state"
:return: None
"""
if self.running_on_lego:
if (state, action) in self.sampled_reward_function:
old_reward = self.sampled_reward_function[state, action]
if old_reward != reward:
print(
'\t\t****Reward change for same state/action pair: (OLD)',
old_reward, '(NEW)', reward)
self.sampled_reward_function[state, action] = reward
else:
self.sampled_reward_function[state, action] = reward
if (state, action) in self.sampled_transition_function:
previous_next_state = self.sampled_transition_function[state,
action]
if previous_next_state != next_state:
raise ValueError(
'This should never happen on this robot ...')
self.sampled_transition_function[state, action] = next_state
def update_state():
global state
color_left = color_sensor_left.color()
color_right = color_sensor_right.color()
distance = ultrasonic_sensor.distance()
reflection_left = color_sensor_left.reflection()
reflection_right = color_sensor_right.reflection()
print("Reflection"+ str(reflection_left))
if color_left == Color.RED or color_right == Color.RED:
state = GOAL_REACHED_STATE
# elif distance<130 or color_left == Color.BLACK or color_right == Color.BLACK:
# state = 3
elif reflection_left < 2:
state = ESCAPE_RIGHT_STATE
elif reflection_right < 2:
state = ESCAPE_LEFT_STATE
elif distance<130 :
if random.random()>0.5:
state = ESCAPE_LEFT_STATE
else:
state = ESCAPE_RIGHT_STATE
elif distance<450:
state = AVOID_STATE
else:
state = CRUISE_STATE
def soundAbspielen():
'''
Spielt alle Töne ab
'''
# Ist zu kompliziert im Moment
import inspect
members = inspect.getmembers(SoundFile)
members = [(name, data) for (name, data) in members
if not name.startswith('_')]
i = 0
while True:
name, data = members[i]
print('SoundFile.{}'.format(name))
brick.display.clear()
brick.display.text("")
brick.display.text('SoundFile.{}'.format(name))
brick.sound.file(data)
wait(50)
buttons = brick.buttons()
while not any(buttons):
buttons = brick.buttons()
wait(10)
if Button.RIGHT in buttons:
i += 1
if Button.LEFT in buttons:
i -= 1
if Button.CENTER in buttons:
pass
if Button.UP in buttons:
i = 0
if Button.DOWN in buttons:
break
print("i={}".format(i))
if not i < len(members):
i = 0
def inchWorm(lineCrossingModule, lineFollowingModule, direction):
brick.light(Color.GREEN)
lineFollowingModuleAmbience = lineFollowingModule.ambient()
lineCrossingModuleColor = lineCrossingModule.color()
lineCrossingModuleAmbience = lineCrossingModule.ambient()
lineCrossingModuleSeesLine = False
if lineFollowingModuleAmbience > lineFollowingModule.getEdgeOfLineAmbience(
): # lighter
lineFollowingModule.run(speedFactor=(1 / 2 * direction))
lineCrossingModule.run(speedFactor=direction)
elif lineFollowingModuleAmbience <= lineFollowingModule.getEdgeOfLineAmbience(
): # darker
if lineCrossingModuleColor == Color.BLACK:
print("line cross sees line is black")
lineCrossingModuleSeesLine = True
brick.light(Color.RED)
lineCrossingModule.run(speedFactor=(1 / 2 * direction))
lineFollowingModule.run(speedFactor=(1 / 2 * direction))
if lineCrossingModuleAmbience < lineCrossingModule.getEdgeOfLineAmbience(
):
print("line cross sees darker than average")
lineCrossingModuleSeesLine = True
brick.light(Color.RED)
lineCrossingModule.run(speedFactor=(1 / 2 * direction))
lineFollowingModule.run(speedFactor=(1 / 2 * direction))
else:
lineCrossingModule.run(speedFactor=(1 / 2 * direction))
lineFollowingModule.run(speedFactor=direction)
return lineCrossingModuleSeesLine
def turn_pointer(): """ Again, we can't do this, but we can use the other file to give us the necessary info # This is how you do a get call using urequests.get. It returns a string in json format ret = urequests.get(url).json() # ret is a long json string. You can parse the temperature data only by using the following code temp_in_kelvin = ret['main']['temp'] # the temperature in kelvin city_name = ret['name'] # you can access other information in 'ret' by changing the 'main' , 'temp' or 'name' # to see what other information is contained in ret, remove the comment '#' sign from the code below # print(ret) """ temp_in_kelvin = 300 # TODO: GET THIS VALUE FROM OTHER PYTHON FILE city_name = "tester" # TODO: GET THIS VALUE FROM OTHER PYTHON FILE temp_in_celsius = round(temp_in_kelvin -273.15) # temperature conversion to celsius print(temp_in_celsius) # prints the data # each degree of temperature should turn the pointer by one unit_degree unit_degree = angle_bw_blueandred / (red_temp-blue_temp) # the pointer should turn turn_angle degrees to represent the temp_in_celsius degree temperature turn_angle = unit_degree * temp_in_celsius # move the pointer to turn_angle degrees pointer.run_target(20,turn_angle) # TRANSLATION --> pointer.run_for_degrees(20, turn_angle) Note: Could be switched, haven't tested yet return temp_in_celsius, city_name
def drive_until_bumped():
if not bump_sensor.pressed():
drive_robot_at_angular_speed(200)
return False
stop_robot()
print(str("bumped"))
return True
def Create_SL(Tag, Type):
urlBase, headers = SL_setup()
urlTag = urlBase + Tag
propName={"type":Type,"path":Tag}
try:
urequests.put(urlTag,headers=headers,json=propName).text
except Exception as e:
print(e)
def _espeak(self, msg):
lang = ""
if self._german:
lang = "-vde"
print(msg)
os.system(
"/usr/bin/espeak -a 200 --stdout '{}' {} | /usr/bin/aplay -i".
format(msg, lang))
def CallService(thing, servicename):
urlValue = urlBase + 'Things/' + thing + '/Services/' + servicename
try:
response = urequests.put(urlValue, headers=headers)
print(response.text)
response.close()
except RuntimeError as e:
print('error with AddtoTable', e)
def WaitForButton():
print("Please unplug the EV3 then press any button to continue.")
while not any(brick.buttons()):
NewColor()
wait(100)
while any(brick.buttons()):
wait(10)
def take_object(self):
self.grabber.open()
self.run_forward()
while (self.front_sensor.distance() > 40):
print("Distance to object: ", self.front_sensor.distance())
wait(1)
self.stop()
self.grabber.close()
def goforward(left, right, times):
print("проехать вперёд:" + str(time))
motorRule(left, right)
oldTime = time.time()
while (True):
newTime = time.time()
if (newTime - oldTime >= times):
break
def printThread(num):
while 1:
with lock:
for i in range(0, num):
print("thread: (" + str(i + 1) + "/" + str(num) + ")")
time.sleep(2)
def accelerate(
start_power, final_power, delay, distance, speed_increment, stop: bool,
correct: bool
): # Function to accelerate/decelerate from x vel to y vel at z mm/s^2
resetAngles() # Resetting the angles
current_power = start_power # Assigning the current power to the start power given as a parameter.
if speed_increment == 0: # If no speed increment is given or speed increment is 0
speed_increment = abs(
final_power - start_power
) / distance * 8 # Assigning the speed increment to be the different of powers divided by the distance.
resetAngles() # reset angle rotations
# get desired direction
try:
direction_coefficient = final_power / abs(final_power)
except ZeroDivisionError:
direction_coefficient = start_power / abs(start_power)
# while power is less than final power
while (abs(current_power) < abs(final_power)
if abs(current_power) <= abs(final_power) else
abs(current_power) > abs(final_power)):
# move in desired direction at increasing power
robot.drive(current_power, 0)
# increment speed
if ((abs(start_power) <= abs(final_power)) or
(abs(start_power) > abs(final_power) and abs(current_power) > 20)):
print(
"speed increment:", str(speed_increment), "increase:",
speed_increment * direction_coefficient *
(1 if abs(current_power) <= abs(final_power) else -1))
current_power += (
speed_increment * direction_coefficient *
(1 if abs(current_power) <= abs(final_power) else -1))
# display current movement stats
print("moving, power", str(current_power), "left rotations",
str(left_motor.angle()), "right rotations",
str(right_motor.angle()), "left distance",
str(angleToDistance(left_motor.angle())), "right distance",
str(angleToDistance(right_motor.angle())), "continue?",
(angleToDistance(abs(left_motor.angle())) < distance))
# wait for specified delay before changing power again
wait(delay)
# if desired distance reached, stop increasing power
if ((abs(angleToDistance(left_motor.angle())) >= distance)
or (angleToDistance(abs(right_motor.angle())) >= distance)):
break
# keep moving until desired distance reached
while not ((angleToDistance(abs(left_motor.angle())) >= distance) or
(angleToDistance(abs(right_motor.angle())) >= distance)):
wait(delay)
# if braking desired
if stop:
# brake
robot.stop(Stop.HOLD)
# if angle correction required
if correct:
# correct angle (making robot face correct direction)
correctRotation(distance)
def Get_Joke():
try:
value = urequests.get("http://api.icndb.com/jokes/random").text
data = ujson.loads(value)
result = data.get("value").get("joke")
except Exception as e:
print(e)
reply = 'failed'
return result
