def calibrate_motor(): for i in range(1,3+1) m = {1:"ground_layer",2:"vertical_altitude_layer",3:"horizontal_altitude_layer"}.get(motor_number) motor = Layer(m) turn_on_gpio(motor.pin_sleep) cmd_pos = 40000 flag = True turn_off_gpio(motor.pin_dir) while(flag): try: turn_off_gpio(motor.pin_step) turn_on_gpio(motor.pin_step) except KeyboardInterrupt: flag = False motor.cur_pos = motot.vr_end turn_on_gpio(motor.pin_dir) flag = True while(flag and motor.cur_pos>0) try: turn_off_gpio(motor.pin_step) turn_on_gpio(motor.pin_step) motor.cur_pos -= 1 except KeyboardInterrupt: flag = False turn_off_gpio(motor.pin_sleep)
def motor_to_init(motor, server): ''' Moves motor to init ''' m = Layer(motor) print(RED+'Moving until find a sensor OR PRESS CONTROL+C'+NO_COLOR) m.steps = MAX_NUM server.motor_move(m.name, m.pin_dir, m.pin_step, m.pin_sleep, m.pin_opto1, m.pin_opto2, m.simulated, m.direction, m.velocity, m.steps, m.vr_init, m.vr_end, m.cur_pos)
def layer_move_skip_sensor(self, layer_id): layer = Layer(layer_id) steps = self.client.motor_move_skip_sensor(layer.name, layer.direction, layer.velocity, layer.steps, layer.cur_pos, layer.cmd_pos) layer.cur_pos = layer.cur_pos + steps return steps
def set_to_zero(self, layer_id): layer = Layer(layer_id) steps = self.client.set_to_zero(layer.name, layer.direction, layer.velocity, layer.steps, layer.cur_pos, layer.cmd_pos) layer.cur_pos = steps return steps
def move_in_valid_range(motor, steps, server): ''' Movements in valid ranges ''' m = Layer(motor) print("cur_pos : %d" % m.cur_pos) cmd_pos = m.cur_pos + steps print("cmd_pos : %d" % cmd_pos) if cmd_pos < m.vr_init: print("cmd from %d --> %d" % (cmd_pos, m.vr_init)) cmd_pos = m.vr_init if cmd_pos > m.vr_end: print("cmd from %d --> %d" % (cmd_pos, m.vr_init)) cmd_pos = m.vr_init steps = cmd_pos - m.cur_pos if steps > 0: print("STEPS to cmd_pos: %d --> to %s" % (steps,m.direction)) m.steps = steps m.direction = "END_POSITION" server.motor_move(m.name, m.pin_dir, m.pin_step, m.pin_sleep, m.pin_opto1, m.pin_opto2, m.simulated, m.direction, m.velocity, m.steps, m.vr_init, m.vr_end, m.cur_pos) else: m.direction = "INIT_POSITION" print("STEPS to cmd_pos: %d --> to %s" % (steps,m.direction)) steps = abs(steps) m.steps = steps server.motor_move(m.name, m.pin_dir, m.pin_step, m.pin_sleep, m.pin_opto1, m.pin_opto2, m.simulated, m.direction, m.velocity, m.steps, m.vr_init, m.vr_end, m.cur_pos) m.cur_pos = cmd_pos return m.cur_pos, cmd_pos
def set_position(self, layer_id, cmd_pos, vel): ''' ''' layer = Layer(layer_id) layer.cmd_pos = cmd_pos layer.velocity = vel layer.cur_pos = self.client.get_motor_cur_pos(layer.name) steps = cmd_pos - layer.cur_pos if steps > 0: layer.direction = 'END_POSITION' else: layer.direction = 'INIT_POSITION' steps = abs(steps) layer.steps = steps layer.cur_pos = self.layer_move_skip_sensor(layer_id)
def set_to_zero(self, name, direction, velocity, steps, cur_pos, cmd_pos): motor = Layer(name) motor.steps = 0 motor.direction = direction motor.velocity = velocity dir_pin = 0 motor.cur_pos = 0 motor.cmd_pos = 0 #print "SET ALL TO ZERO" #print "name %s" % motor.name #print "direction %s" % motor.direction #print "velocity %d" % motor.velocity #print "steps %d" % motor.steps #print "cur_pos %d" % motor.cur_pos #print "cmd_pos %d" % motor.cmd_pos return 0
def motor_move_skip_sensor(self, name, direction, velocity, steps, cur_pos, cmd_pos): #print "name %s" % name #print "direction %s" % direction #print "velocity %d" % velocity #print "steps %d" % steps #print "cur_pos %d" % cur_pos #print "cmd_pos %d" % cmd_pos motor = Layer(name) motor.steps = steps motor.direction = direction motor.velocity = velocity dir_pin = 0 if('INIT_POSITION' in direction): dir_pin = 1 - motor.pos_dir else: dir_pin = motor.pos_dir # if(dir_pin): #print 'pin on' # else: #print 'pin off' if(dir_pin): self.turn_on_gpio(motor.pin_dir) else: self.turn_off_gpio(motor.pin_dir) self.turn_on_gpio(motor.pin_sleep) s = c_driver.move_motor(steps, motor.pin_step) self.turn_off_gpio(motor.pin_sleep) #print sys._getframe().f_code.co_name, #print ": %s -> %1.2f " % (name, s) motor.cur_pos = cmd_pos motor.cmd_pos = cmd_pos #print "\n\n" return motor.cur_pos
def save(self, widget): print "### SAVE ###" config_name = self.combobox_layer.get_active_text() layer = Layer(config_name) layer.pin_sleep = self.entry_pin_sleep.get_text() layer.pin_step = self.entry_pin_step.get_text() layer.pin_dir = self.entry_pin_dir.get_text() layer.pin_opto1 = self.entry_pin_opto1.get_text() layer.pin_opto2 = self.entry_pin_opto2.get_text() layer.name = self.entry_name.get_text() layer.sim = self.checkbutton_sim.get_active() layer.direction = self.combobox_direction.get_active_text() #layer.velocity = int(self.hscale_vel.get_value()) layer.steps = self.entry_steps.get_text() layer.vr_init = self.entry_vr_init.get_text() layer.vr_end = self.entry_vr_end.get_text() layer.image_prefix = self.entry_image_prefix.get_text() print layer.pin_sleep print layer.pin_step print layer.pin_dir print layer.name print layer.sim print layer.direction print layer.velocity print layer.steps print layer.vr_init print layer.vr_end print layer.image_prefix print "### SAVE ###"