def __init__(self):
super(Sorter, self).__init__(self.state_push)
print "[Sorter.__init__]"
self.white_piston = Piston(**config.devices[self.ORANGE_PISTON_ID]) #C'EST VOULU QUE CA SOIT INVERSE
self.orange_piston = Piston(**config.devices[self.WHITE_PISTON_ID]) #C'EST VOULU QUE CA SOIT INVERSE
self.color_sensor = ColorSensor(**config.devices[self.COLOR_SENSOR_ID])
self.active_piston = self.white_piston
self.ball_count = 0
self.white_count = 0
self.orange_count = 0
self.last_ball_time = time.time();
self.cycle_time = time.time();
def __init__(self, collector):
super(CollectorController, self).__init__(self.state_wait_init)
print "[CollectorController.__init__]"
self.sorter = collector.sorter
self.rail = collector.rail
self.vacuum_shaker = collector.vacuum_shaker
self.start_collect_switch = Switch(**config.devices[self.START_COLLECT])
self.foot_up_switch = MagneticSwitch(**config.devices[self.FOOT_UP])
self.foot_down_switch = MagneticSwitch(**config.devices[self.FOOT_DOWN])
self.gate = Piston(**config.devices[self.GATE])
class Sorter(Component):
"""Control the sorting function of the collector.
Implementation use a simple state machine generator. Each state yield None
or the next state function. The state machine end when a state return instead
of yielding
The states go in this order:
1. state_push: Push the white piston
2. state_pushed: Check if there a ball
3. state_pull: Pull the currently pushed piston
4. state_push: Push the good piston
5. Go back to 2.
"""
PULL_DELAY = 0.15
PUSH_DELAY = 0.15
WHITE_PISTON_ID = "white_piston"
ORANGE_PISTON_ID = "orange_piston"
COLOR_SENSOR_ID = "color_sensor"
MAX_BALL = 12
COLOR_TIMEOUT = 2.0
BLACK_TIMEOUT = 5.0
def __init__(self):
super(Sorter, self).__init__(self.state_push)
print "[Sorter.__init__]"
self.white_piston = Piston(**config.devices[self.ORANGE_PISTON_ID]) #C'EST VOULU QUE CA SOIT INVERSE
self.orange_piston = Piston(**config.devices[self.WHITE_PISTON_ID]) #C'EST VOULU QUE CA SOIT INVERSE
self.color_sensor = ColorSensor(**config.devices[self.COLOR_SENSOR_ID])
self.active_piston = self.white_piston
self.ball_count = 0
self.white_count = 0
self.orange_count = 0
self.last_ball_time = time.time();
self.cycle_time = time.time();
def stop(self):
print "[Sorter.stop] Stop pistons"
self.white_piston.pull()
time.sleep(self.PULL_DELAY)
self.white_piston.stop()
self.orange_piston.pull()
time.sleep(self.PULL_DELAY)
self.orange_piston.stop()
def reset_ball_count(self):
self.ball_count = 0
self.white_count = 0
self.orange_count = 0
self.cycle_time = time.time()
def get_ball_count(self):
return self.ball_count
def get_last_ball_time(self):
return self.last_ball_time
def get_cycle_time(self):
return self.cycle_time
def get_ball_count(self):
return self.ball_count
def state_push(self):
"""State that move one piston to push a ball
change to 'state_pushed' after a small delay"""
self.active_piston.push()
yield self.wait(self.PUSH_DELAY, self.state_pushed)
def state_pushed(self):
"""State that wait for a balls. Read ball's color and
change to 'state_recall' after setting the good piston"""
self.active_piston.standby()
print "[Sorter.state_pushed] Piston in standby, w:%d,o:%d,t:%d" % (self.white_count, self.orange_count, self.ball_count)
ball_color = self.color_sensor.get_color()
unknown_timeout = time.time() + self.COLOR_TIMEOUT
black_timeout = time.time() + self.BLACK_TIMEOUT
while ball_color in [ColorSensor.UNKNOWN, ColorSensor.BLACK] and \
time.time() < unknown_timeout and time.time() < black_timeout:
if ball_color == ColorSensor.BLACK:
unknown_timeout = time.time() + self.COLOR_TIMEOUT
yield
ball_color = self.color_sensor.get_color()
current_piston_pushed = self.active_piston
if ball_color == ColorSensor.UNKNOWN:
print '[Sorter.state_pushed] Unknown ball detected'
if ball_color == ColorSensor.BLACK:
print '[Sorter.state_pushed] Sorter.state_pushed timed out'
elif self.orange_count > self.MAX_BALL or \
(ball_color == ColorSensor.WHITE and self.white_count <= self.MAX_BALL):
print "[Sorter.state_pushed] White ball detected"
self.active_piston = self.white_piston
self.white_count += 1
self.ball_count += 1
else:
print "[Sorter.state_pushed] Orange ball detected"
self.active_piston = self.orange_piston
self.orange_count += 1
self.ball_count += 1
self.last_ball_time = time.time();
yield partial(self.state_pull, current_piston_pushed)
def state_pull(self, piston_to_recall):
"""move back piston_to_recall. Change to 'state_push' after the PULL_DELAY"""
piston_to_recall.pull()
yield self.wait(self.PULL_DELAY, self.state_push)
class CollectorController(Component):
WAIT_TIME_GATE = 1.5
WAIT_TIME_FOOT = 2.0
WAIT_TIME_DUMP_BALLS = 1.5
MAX_BALLS_PER_ROUND = 20
MAX_DELAY_BETWEEN_BALLS = 2.0
MAX_DELAY_CYCLE = 30.0
START_COLLECT = "start_collect_switch"
GATE = "gate_servo"
FOOT_UP = "collector_foot_up"
FOOT_DOWN = "collector_foot_down"
def __init__(self, collector):
super(CollectorController, self).__init__(self.state_wait_init)
print "[CollectorController.__init__]"
self.sorter = collector.sorter
self.rail = collector.rail
self.vacuum_shaker = collector.vacuum_shaker
self.start_collect_switch = Switch(**config.devices[self.START_COLLECT])
self.foot_up_switch = MagneticSwitch(**config.devices[self.FOOT_UP])
self.foot_down_switch = MagneticSwitch(**config.devices[self.FOOT_DOWN])
self.gate = Piston(**config.devices[self.GATE])
def stop(self):
print "[CollectorController.stop] Stop controller"
self.gate.stop()
def state_wait_init(self):
print "[CollectorController.state_wait_init]"
# Initialiser la position du collecteur,
self.rail.slide_to_home()
while self.rail.is_moving():
yield
self.rail.slide_to_wait_for_sorting_position()
while self.rail.is_moving():
yield
# Wait for start switch from collector
if not self.vacuum_shaker.load_tank_switch.is_pressed():
self.vacuum_shaker.load_tank_switch.wait_pressed()
print "[CollectorController.state_wait_init] load_tank_switch pressed"
# Wait for start switch from truck
if not self.start_collect_switch.is_pressed():
self.start_collect_switch.wait_pressed()
print "[CollectorController.state_wait_init] start_collect_switch pressed"
self.vacuum_shaker.is_init = True
Logger().start_new_cycle() # Start first cycle here
self.gate.push()
yield self.state_push_truck_home
def state_push_truck_home(self):
print "[CollectorController.state_push_truck_home]"
self.rail.slide_to_home()
while self.rail.is_moving():
yield
yield self.state_wait_sorter
def state_push_truck_away(self):
print "[CollectorController.state_push_truck_away]"
self.rail.slide_to_away()
while self.rail.is_moving():
yield
Logger().end_current_cycle()
self.vacuum_shaker.wait_balls()
yield self.wait(self.WAIT_TIME_DUMP_BALLS, self.state_push_truck_home)
def state_push_truck_standby(self):
print "[CollectorController.state_push_truck_standby]"
self.rail.slide_to_wait_for_sorting_position()
while self.rail.is_moving():
yield
yield self.state_wait_truck_foot
def ready_to_drop_balls(self):
balls = self.sorter.get_ball_count()
last_ball_time = self.sorter.get_last_ball_time()
cycle_time = self.vacuum_shaker.last_button_push
done = balls >= self.MAX_BALLS_PER_ROUND
halfway_done = balls >= self.MAX_BALLS_PER_ROUND / 2
timed_out_ball = int(time.time() - last_ball_time) > self.MAX_DELAY_BETWEEN_BALLS
timed_out_cycle = int(time.time() - cycle_time) > self.MAX_DELAY_CYCLE
foot_down = self.foot_down_switch.is_pressed()
is_home = self.rail.is_home()
return is_home and foot_down and (done or (halfway_done and timed_out_ball) or (timed_out_cycle and balls > 0))
def state_wait_sorter(self):
print "[CollectorController.state_wait_sorter]"
self.sorter.last_ball_time = time.time()
self.vacuum_shaker.last_button_push = time.time()
while not self.ready_to_drop_balls():
yield
if self.sorter.get_ball_count() < self.MAX_BALLS_PER_ROUND:
print "[CollectorController.state_wait_sorter] Timed out. Dumping " + str(
self.sorter.get_ball_count()
) + " balls!"
Logger().set_current_cycle_ball_count(self.sorter.get_ball_count())
self.sorter.reset_ball_count()
yield self.state_open_gate
def state_open_gate(self):
print "[CollectorController.state_open_gate]"
self.gate.pull()
yield self.wait(self.WAIT_TIME_GATE, self.state_close_gate)
def state_close_gate(self):
print "[CollectorController.state_close_gate]"
self.gate.push()
yield self.state_push_truck_standby
def state_wait_truck_foot(self):
print "[CollectorController.state_wait_truck_foot]"
while not self.foot_up_switch.is_pressed():
yield
yield self.state_push_truck_away
from collector.rail import Rail
from device import Piston, Switch
from lib import config
print "The correct starting state for the collector is: Truck on sort_position. Gate closed."
r = Rail()
gate = Piston(**config.devices["gate_servo"])
while True:
print "TOTAL DISTANCE: "+str(r.HOME_POSITION)
print "SORTING POSITION: "+str(r.WAIT_FOR_SORTING_POSITION)+" or "+str(r.HOME_POSITION - r.WAIT_FOR_SORTING_POSITION)
print "1 : Goto X right"
print "2 : Goto X left"
print "3 : Close gate"
print "4 : Open gate"
c = int(raw_input("your choice?"))
if c == 1:
c = int(raw_input("how much? "))
r.stepper.move(0,abs(c), r.is_away)
elif c == 2:
c = int(raw_input("how much? "))
r.stepper.move(1,abs(c), r.is_home)
elif c == 3:
print "closing gate"
gate.push()
elif c == 4:
print "opening gate"
gate.pull()
