def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(
0, 0) # (forward velocity, rotational velocity)
#####################################
# ground = inp.prox_ground.reflected
# ground = inp.prox_ground.ambiant
ground = inp.prox_ground.delta
print(ground)
try:
left = ground[0]
right = ground[1]
except:
left = -1
right = -1
print(left, right)
print(state)
if left >= 255 and right >= 255:
if state == 0:
return 0, io.Action(fv=0.05, rv=0.0)
else:
return 3, io.Action(fv=0.0, rv=-1)
if left < 255:
return 1, io.Action(fv=0.0, rv=1)
if left >= 255 and right < 255:
return 2, io.Action(fv=0.05, rv=0.0)
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0,0)
#ground = inp.prox_ground.reflected
#ground1 = inp.prox_ground.ambiant
ground2 = inp.prox_ground.delta
#left = ground[0]
#right = ground[1]
#right1 = ground1[1]
left2 = ground2[0]
right2 = ground2[1]
if left2 > 500:
state = 'white'
forward = 0.2
else:
state = 'black'
forward = 0.0
#io.Action(fv=0.0, rv=0.0) if state == 'black' else io.Action(fv=0.2, rv=0.0)
return state, io.Action(fv=forward, rv=0.0)
def get_next_values(self, state, inp):
fv_, rv_, wall = state
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0, 0)
#####################################
#ground = inp.prox_ground.reflected
#ground = inp.prox_ground.ambiant
ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
print(left, right)
next_state = 0.1, 0.0, False
if wall:
if left > 200 and right > 200: # both white
return next_state, io.Action(fv=0.03, rv=1)
if left < 200 and right < 200: # both black
return next_state, io.Action(fv=0.03, rv=-1)
else:
return next_state, io.Action(fv=0.1, rv=rv_)
return next_state, io.Action(fv=0.1, rv=rv_)
def get_next_values(
self, state,
inp): #used to stop the robot by pressing the back button
if inp.button_backward:
return 'halt', io.Action(0, 0)
ground = inp.prox_ground.delta #IR readings from the sensors on the Thymio
left = ground[
0] #assign left to be the reading from the left IR sensor
right = ground[
1] #assign right to be the reading from the right IR sensor
check = firebase.get(
"/light2_state"
) #determine the state of the upcoming traffic light on the road the Thymio is on
print(left, right) #to see the IR readings as the program runs
print(check
) #to see the state of the firebase variable as the program runs
# to stop the car we need to make sure two thing:
# 1. the thymio have reached the black line at the crossing road
# 2. the light is red
if left < 200 and right < 200 and check == "red": #IR readings imply black line
return ("Stop", io.Action(fv=0.0, rv=0.0)
) #check if the traffic light is red, then stop
# to stop the car we need to make sure two thing:
# 1. the thymio have reached the black line at the crossing road
# 2. the light is green
elif left < 200 and right < 200 and check == "green": #IR readings imply black line
return ("Go", io.Action(fv=0.03, rv=0.0)
) #check if the traffic light is green, then go
return (None, io.Action(fv=0.03, rv=0))
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0,0)
#####################################
ground = inp.prox_ground.reflected
# ground = inp.prox_ground.ambiant
# ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
print(left,right)
# next_state = state
# return next_state, io.Action(fv=0.15, rv=0.2)
if state == 1 and left >300 and right >300:
nextstate = 1
return nextstate, io.Action(fv= 0.2, rv=0)
elif state==1 and left<100 and right <100:
nextstate=0
return nextstate, io.Action(0,0)
else:
return 'halt', io.Action(0,0)
def get_next_values(self, state, ground):
ground = ground.prox_ground.delta
left = ground[0]
right = ground[1]
print(left, right)
if state == 0 and ground[0] > 200 and ground[1] > 200:
return 0, io.Action(0, 0.2)
elif state == 0 and ground[0] < 200 and ground[1] < 200:
return 0, io.Action(0, 0.2)
elif state == 0 and ground[0] > 200 and ground[1] < 200:
return 1, io.Action(0.1, 0)
elif state == 0 and ground[0] < 200 and ground[1] > 200:
return 1, io.Action(0.1, 0)
elif state == 1 and ground[0] > 200 and ground[1] > 200:
return 0, io.Action(0.0, 0.2)
elif state == 1 and ground[0] < 200 and ground[1] < 200:
return 1, io.Action(0.0, -0.2)
elif state == 1 and ground[0] > 200 and ground[1] < 200:
return 1, io.Action(0.1, 0.0)
elif state == 1 and ground[0] < 200 and ground[1] > 200:
return 1, io.Action(0.1, 0.0)
#####################################
# ground = inp.prox_ground.reflected
# ground = inp.prox_ground.ambiant
next_state = state
return next_state, io.Action(fv=0.0, rv=0.0) #Edit this part
def get_next_values(self, state, inp):
if inp.button_backward:
return 'halt', io.Action(0, 0)
print(inp.prox_horizontal[2])
#print(inp.prox_ground.delta)
#print inp.temperature
#print inp.accelerometer
if inp.prox_horizontal[2] > 0:
state = 'halt'
forward = 0.0
else:
forward = 0.1
return state, io.Action(fv=forward, rv=0)
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0,0)
def get_next_values(self, state, inp):
ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
print(left, right)
next_state = state
return next_state, io.Action(fv=0.1, rv=-0.1)
def get_next_values(self, state, inp):
ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
print(left, right)
# print(inp.prox_ground.delta)
# print(inp.prox_ground.reflected)
# print(inp.prox_ground.ambiant)
return state, io.Action(fv=0.05, rv=0.1)
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0,0)
#####################################
#ground = inp.prox_ground.reflected
#ground = inp.prox_ground.ambiant
ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
print(left,right)
next_state = state + 0.01
return next_state, io.Action(fv=0, rv=0)
def get_next_values(self, state, inp):
if inp.button_backward:
return 'halt', io.Action(0, 0)
ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
diff = left - right
print(state)
print(left, right, diff)
if state == 'follow':
if diff > 500:
print('Follow, forward.')
fv = 0.05
rv = 0.0
elif diff < -500:
print('Follow, forward left.')
fv = 0.05
rv = 0.2
elif -100 < diff < 100 and left > 500 and right > 500:
print('Follow, forward right.')
fv = 0.01
rv = -0.2
elif -100 < diff < 100 and left < 500 and right < 500:
print('Follow, left.')
fv = 0
rv = 0.2
else:
print('Follow, others.')
fv = 0.05
rv = 0.0
next_state = 'follow'
elif state == 'find':
if -100 < diff < 100 and 0 < left < 500 and 0 < right < 500:
print('Change to follow.')
fv = 0.0
rv = 0.2
next_state = 'follow'
else:
print('Still in find, move forward.')
fv = 0.03
rv = 0.0
next_state = 'find'
return next_state, io.Action(fv, rv)
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0, 0)
#####################################
if state == None:
ground = inp.prox_ground.delta
next_state = get_state(ground)
return next_state, io.Action(fv=0.2, rv=0.0)
else:
ground = inp.prox_ground.delta
next_state = get_state(ground)
if next_state == state:
return next_state, io.Action(fv=0.2, rv=0.0)
else:
return "halt", io.Action(fv=0.0, rv=0.0)
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(
0, 0) # (forward velocity, rotational velocity)
#####################################
# ground = inp.prox_ground.reflected
# ground = inp.prox_ground.ambiant
# ground = inp.prox_ground.delta
try:
left = ground[0]
right = ground[1]
except:
left = -1
right = -1
# print(left,right)
next_state = state
return next_state, io.Action(fv=0.1, rv=0.2)
class MySMClass(sm.SM):
start_state=None
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0,0)
#ground = inp.prox_ground.reflected
#ground1 = inp.prox_ground.ambiant
ground2 = inp.prox_ground.delta
#left = ground[0]
#right = ground[1]
#right1 = ground1[1]
left = ground2[0]
right = ground2[1]
if state == None and left > 500 and right > 500:
state = None
forward = 0.1
rotate = 0
elif left < 500 and right < 500:
state = 'black'
forward = 0.0
rotate = 0.4
elif left > 500 and right < 500:
state = 'black'
forward = 0.1
rotate = 0.0
elif state == 'black' and left > 500 and right > 500:
state = 'black'
forward = 0.0
rotate = -0.4
print(state)
#io.Action(fv=0.0, rv=0.0) if state == 'black' else io.Action(fv=0.2, rv=0.0)
return state, io.Action(fv=forward, rv=rotate)
def get_next_values(self, state, inp):
wall = state
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0, 0)
#####################################
#ground = inp.prox_ground.reflected
#ground = inp.prox_ground.ambiant
ground = inp.prox_ground.delta
left = ground[0]
right = ground[1]
print(left, right, wall)
next_state = wall
if not wall:
if left > 200 and right > 200:
return next_state, io.Action(fv=0.1, rv=0)
else:
wall = True
next_state = wall
return next_state, io.Action(fv=0.03, rv=1)
print("wall")
if wall == True:
print("wall")
if left > 200 and right > 200: # both white
print("rv=1")
return next_state, io.Action(fv=0.03, rv=0.5)
if left < 200 and right < 200: # both black
return next_state, io.Action(fv=0.03, rv=-0.5)
else:
return next_state, io.Action(fv=0.09, rv=0)
return next_state, io.Action(fv=0.1, rv=0)
def halt(self): # thymio brake
return io.Action(0,0)
def inv_rv(self):
return io.Action(0,-0.5)
def rv_(self): # speed tested on = 150 --> 57.1429(deg/ s)
return io.Action(0,0.5)
def reverse(self): # speed tested on = 250 --> 7.82895(cm/ s)
return io.Action(-1,0)
def forward(self): # speed tested on = 250 --> 7.82895(cm/ s)
return io.Action(1,0)
def brake(self): # thymio brake
return io.Action(0,0)
def get_next_values(self, state, inp):
print('Delta:', inp.prox_ground.delta)
print('Reflected:', inp.prox_ground.reflected)
print('Ambient:', inp.prox_ground.ambiant)
return state, io.Action(fv=0.00, rv=0)
def get_next_values(self, state, inp):
if inp.button_backward:
return 'halt', io.Action(0, 0)
#####################################
ground = inp.prox_ground.delta
wall = inp.prox_horizontal
print(self.counter)
print(data)
#print(ground[0],ground[1])
if wall[0] > 2500: # stop if there is obstacle in front
forv = 0.0
rotv = 0.0
next_state = state
elif state == 0: # at initial position
data["bin1"] = [
firebase_sensor.get("/Bin 1"),
firebase_kivy.get("/Bin 1")
]
data["bin2"] = [
firebase_sensor.get("/Bin 2"),
firebase_kivy.get("/Bin 2")
]
data["bin3"] = [
firebase_sensor.get("/Bin 3"),
firebase_kivy.get("/Bin 3")
]
if 1 in data["bin2"] or 1 in data["bin3"]:
forv = 0.0
rotv = 0.0
next_state = 1
elif 1 in data["bin1"]:
forv = 0.0
rotv = 0.0
next_state = 2
else:
#sleep(0.5)
forv = 0.0
rotv = 0.0
next_state = 0
elif state == 1: # bin2 or bin3 is full (can also be used as boundary follower)
if ground[0] < 300 and ground[
1] > 920: # breaking point (2nd intersection on the way back)
sleep(7.5)
forv = 0.03
rotv = 0.0
next_state = 5 # go to transition state
elif ground[0] < 300: # first bit of barcode
forv = 0.03
rotv = 0.0
next_state = 3
elif ground[1] < 695: # black line (check value)
forv = 0.0
rotv = 0.05
next_state = 1
elif ground[1] < 894: # grey line (check value)
forv = 0.03
rotv = 0.0
next_state = 1
else:
forv = 0.0
rotv = -0.05
next_state = 1
elif state == 2: # bin1 is full
if ground[1] < 695:
forv = 0.0
rotv = 0.05
next_state = 2
elif ground[1] < 920:
forv = 0.03
rotv = 0.0
next_state = 2
else:
sleep(4.5)
forv = 0.03
rotv = 0.0
next_state = 1
elif state == 3: # barcode reading state
if self.counter > 7: # this line marks the end of barcode
forv = 0.0
rotv = 0.0
next_state = 4
elif ground[
0] > 700: # reading barcode in binary (white indicates 1 and black indicates 0)
if self.counter == 0:
self.reading1 = 1
elif self.counter == 2:
self.reading2 = 2
elif self.counter == 6:
self.reading3 = 4
forv = 0.03
rotv = 0.0
next_state = 3
else:
forv = 0.03
rotv = 0.0
next_state = 3
self.counter += 1
elif state == 4: # determining bin identity and next action
self.counter = -2
# print(barcode[self.barcode_reader(self.reading1, self.reading2, self.reading3)])
# barcode_reader method is called to interpret the binary numbers into order 10
# this value is then used to identify the bin
if barcode[self.barcode_reader(
self.reading1, self.reading2, self.reading3
)] == "stop": # stopping thymio when it is at the initial position
forv = 0.0
rotv = 0.0
next_state = 0
elif barcode[self.barcode_reader(
self.reading1, self.reading2, self.reading3
)] == "2nd decision": # This is to decide whether Thymio will return to initial position or continue to collect bin1
if 1 in data["bin1"]: # continue to bin1
sleep(4.2)
forv = 0.05
rotv = 0.0
next_state = 5
else: # return to initial position
forv = 0.0
rotv = 0.08
next_state = 6
elif 1 in data[barcode[self.barcode_reader(
self.reading1, self.reading2,
self.reading3)]]: # check whether current bin is full
forv = 0.0
rotv = 0.0
next_state = 5 # Thymio stops to collect trash
else: # bin is empty, return to boundary follower (state 1)
forv = 0.05
rotv = 0.0
next_state = 1
elif state == 5: # Transition state (to ignore commands for a fixed period of time)
print("trans")
forv = 0.05
rotv = 0.0
sleep(1.5)
next_state = 1
elif state == 6: # finding its way to the line to return to initial position
sleep(3.5)
forv = 0.05
rotv = 0.0
if self.counter_2 <= 3:
self.counter_2 += 1
next_state = 6
else:
self.counter_2 = 0
next_state = 1
return next_state, io.Action(fv=forv, rv=rotv)
def get_next_values(self, state, inp):
# These two lines is to stop the robot
# by pressing the backward button.
# This only works when using the real robot.
# It will not work in simulator.
if inp.button_backward:
return 'halt', io.Action(0, 0)
#####################################
next_state = self.get_state(inp)
if state == None:
return self.get_state(inp), io.Action(fv=0.1, rv=0.0)
elif state == "black":
if next_state == state:
return next_state, io.Action(fv=0.1, rv=0.0)
else:
return "align-right", io.Action(fv=0.0, rv=-0.2)
elif state == "white":
if next_state == state:
return next_state, io.Action(fv=0.1, rv=0.0)
else:
return "align-left", io.Action(fv=0.0, rv=0.2)
elif state == "align-left":
if next_state == "aligned":
return next_state, io.Action(fv=0.1, rv=0.0)
else:
return state, io.Action(fv=0.0, rv=0.2)
elif state == "align-right":
if next_state == "aligned":
return next_state, io.Action(fv=0.1, rv=0.0)
else:
return state, io.Action(fv=0.0, rv=-0.2)
elif state == "aligned":
if next_state == "black":
return "align-left", io.Action(fv=0.0, rv=0.2)
elif next_state == "white":
return "align-right", io.Action(fv=0.0, rv=-0.2)
else:
return state, io.Action(fv=0.1, rv=0.0)
