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
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)
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()