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