예제 #1
0
def main():
    motors = Sensorimotor(number_of_motors = 2, verbose = False)

    try:
        # Check for motors
        N = motors.ping()
        print("Found {0} motors.".format(N))
        sleep(1.0)

        #TODO: set this according to your supply voltage and desired max. motor speed
        motors.set_voltage_limit([0.18, 0.18])

        # Start motor
        motors.start()

        #move left
        counter = 0
        while(counter < 5):
            motors.apply_impulse([0.5, 0])
            print("tak")
            counter = counter +1
            sleep(0.1)
        counter = 0
        #move right
        while(counter < 5):
            motors.apply_impulse([-0.5, 0])
            print("tak")
            counter = counter +1
            sleep(0.1)
        counter = 0
        #move down
        while(counter < 5):
            motors.apply_impulse([0, 0.5])
            print("tak")
            counter = counter +1
            sleep(0.1)
        counter = 0
        #move up
        while(counter < 5):
            motors.apply_impulse([0, -0.5])
            print("tak")
            counter = counter +1
            sleep(0.1)
        motors.stop()

    except (KeyboardInterrupt, SystemExit):
        # Stop motors
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()

    print("____\nDONE.")
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=False)

    try:
        # Check for motors
        N = motors.ping()
        print("Found {0} motors.".format(N))
        sleep(1.0)

        #TODO: set this according to your supply voltage and desired max. motor speed
        motors.set_voltage_limit([0.25, 0.18])
        # Start motors
        motors.start()

        #TODO: set this parameters according to your desired motor positon control behaviour
        motors.set_pos_ctrl_params(0,
                                   Kp=2.3,
                                   Ki=0.4,
                                   Kd=0.1,
                                   deadband=0.1,
                                   pulse_threshold=0.1)
        motors.set_pos_ctrl_params(1,
                                   Kp=0.4,
                                   Ki=0.2,
                                   Kd=0.01,
                                   deadband=0.1,
                                   pulse_threshold=0.1)

        # Set the motor position
        motors.set_position([0.6, 0])
        sleep(1.5)

        # Print the current position
        print_position(motors.get_position())

        # Set the motor position
        motors.set_position([-0.6, 0])
        sleep(1.5)

        # Print the current position
        print_position(motors.get_position())

        motors.stop()

    except (KeyboardInterrupt, SystemExit):
        # Stop motors
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()

    print("____\nDONE.")
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=False)

    try:
        # Check for motors
        N = motors.ping()
        print("Found {0} motors.".format(N))
        sleep(1.0)

        #TODO: set the supply voltage limit
        motors.set_voltage_limit([0.11, 0.11])

        # Start motors
        motors.start()

        motors.set_pos_ctrl_params(0,
                                   Kp=1.8,
                                   Ki=0.2,
                                   Kd=0.1,
                                   deadband=0.1,
                                   pulse_threshold=0.1)
        motors.set_pos_ctrl_params(1,
                                   Kp=1.8,
                                   Ki=0.2,
                                   Kd=0.1,
                                   deadband=0.1,
                                   pulse_threshold=0.1)

        motors.set_position([0.5, 0.5])
        sleep(1.5)

        # Print the current position
        print_position(motors.get_position())

        motors.set_position([-0.5, -0.5])
        sleep(1.5)

        # Print the current position
        print_position(motors.get_position())

        motors.stop()

    except (KeyboardInterrupt, SystemExit):
        # Stop motors
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()

    print("____\nDONE.")
예제 #4
0
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=False)

    try:
        # checking for motors
        N = motors.ping()
        print("Found {0} motors.".format(N))
        sleep(1.0)

        #TODO: set this according to your supply voltage and desired max. motor speed
        motors.set_voltage_limit([0.18, 0.18])
        # starting motorcord
        motors.start()

        #TODO: set this parameters according to your desired motor positon control behaviour
        motors.set_pos_ctrl_params(0,
                                   Kp=0.9,
                                   Ki=0.0,
                                   Kd=0.05,
                                   deadband=0.0,
                                   pulse_threshold=0.1)
        motors.set_pos_ctrl_params(1,
                                   Kp=0.9,
                                   Ki=0.0,
                                   Kd=0.05,
                                   deadband=0.0,
                                   pulse_threshold=0.1)

        while (True):
            print_position(motors.get_position())
            sleep(0.2)

#prints the current position

        motors.stop()

    except (KeyboardInterrupt, SystemExit):
        # stopping motor cord
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()
        raise

    print("____\nDONE.")
예제 #5
0
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=False)

    try:
        # Check for motors
        N = motors.ping()
        print("Found {0} motors.".format(N))
        sleep(1.0)

        #voltage limit for supply voltage and desired max. motor speed
        motors.set_voltage_limit([0.16, 0.16])

        # Start motors
        motors.start()

        counter = 0
        while (counter < 10):
            #TODO: set the impuse for each motor
            #moves left
            motors.apply_impulse([0.1, 0.1])
            print("tak")
            counter = counter + 1
            sleep(0.1)
        counter = 0

        while (counter < 10):
            #TODO: set the impuse for each motor
            #move right
            motors.apply_impulse([0.1, -0.1])
            print("tak")
            counter = counter + 1
            sleep(0.1)
        counter = 0
        motors.stop()

    except (KeyboardInterrupt, SystemExit):
        # Stop motors
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()

    print("____\nDONE.")
예제 #6
0
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=True)

    # checking for motors
    N = motors.ping()
    print("Found {0} sensorimotors.".format(N))
    sleep(1.0)

    # starting motorcord
    motors.set_voltage_limit([0.1, 0.1])
    motors.start()

    try:

        while (True):
            # USER CODE HERE BEGIN

            motors.set_position([0.2, 0.2])
            #print_position(motors.get_position())
            sleep(0.25)
            motors.set_position([0.2, -0.2])
            #print_position(motors.get_position())
            sleep(0.25)
            motors.set_position([-0.2, -0.2])
            #print_position(motors.get_position())
            sleep(0.25)
            motors.set_position([-0.2, +0.2])
            #print_position(motors.get_position())
            sleep(0.25)

            # USER CODE HERE END

    except (KeyboardInterrupt, SystemExit):
        # stopping motor cord
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()
        raise

    print("____\nDONE.")
예제 #7
0
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=False)

    try:
        # checking for motors
        N = motors.ping()
        print("Found {0} sensorimotors.".format(N))
        sleep(1.0)

        #TODO: set this according to your supply voltage and desired max. motor speed
        #motors.set_voltage_limit([0.25, 0.25])

        # starting motorcord
        motors.start()

        #TODO: set this parameters according to your desired motor positon control behaviour
        #motors.set_pos_ctrl_params(0, Kp = 1.0, Ki = 0.0, Kd = 0.0, deadband = 0.00, pulse_threshold = 0.00)

        while (True):
            # USER CODE HERE BEGIN

            motors.set_position([0.2, 0.2])
            for i in range(100):
                sleep(0.01)
                print_position(motors.get_position())

            motors.set_position([-0.2, 0.2])
            sleep(1.0)

            # USER CODE HERE END

    except (KeyboardInterrupt, SystemExit):
        # stopping motor cord
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()
        raise

    print("____\nDONE.")
def main():
    motors = Sensorimotor(number_of_motors=3, verbose=False)

    try:
        # checking for motors
        N = motors.ping()
        print("Found {0} sensorimotors.".format(N))
        sleep(1.0)

        #TODO: set this according to your supply voltage and desired max. motor speed
        #motors.set_voltage_limit([0.25, 0.25, 0.25])

        # starting motorcord
        motors.start()

        beat = tst0

        while (True):
            # USER CODE HERE BEGIN

            print("tak")
            for b in beat:
                motors.apply_impulse(b[0])
                sleep(b[1] * 1.2)

            # USER CODE HERE END

    except (KeyboardInterrupt, SystemExit):

        # stopping motor cord
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()

        raise

    print("____\nDONE.")
예제 #9
0
파일: lombi-hex-1.py 프로젝트: x75/lombi
def cord_init():
    # create sensorimotor communication bus
    cord = Sensorimotor(
        number_of_motors=6,  # how many motors
        verbose=False,  # print debug info
        update_rate_Hz=100)  # low-level update frequency

    try:
        # checking for motors
        N = cord.ping()
        print("Found {0} sensorimotors.".format(N))
        time.sleep(0.25)

        # starting motorcord
        cord.start()
        i = 0
    except:
        # Script crashed?
        #print("\rException thrown, stopping cord.")
        cord.stop()
        raise
    return cord
예제 #10
0
def main(args):
    conf = conflib[args.conf]
    args_main = argparse.Namespace()
    for conf_key in conf:  # ['number_of_motors', 'update_rate_Hz', 'smnode_ids']:
        # update from command line
        if hasattr(args, conf_key) and getattr(args, conf_key) is not None:
            setattr(args_main, conf_key, getattr(args, conf_key))
        else:
            setattr(args_main, conf_key, conf[conf_key])

    # convenience
    args_main.update_period_s = 1. / args_main.update_rate_Hz

    # create queue
    qu = queue.Queue(maxsize=10)

    # def cb_hexagon_motors(path, args, types, target, unk):
    #     # def cb_hexagon_motors(*args, **kwargs):
    #     # i, f = args
    #     # print('args = {0}, kwargs = {1}'.format(args, kwargs))
    #     # print('qu = {0}'.format(qu))
    #     print("cb_hexagon_motors message {0} with arguments {1}".format(path, args))
    #     qu.put((path, args))
    #     # print('received args {0}'.format(args))

    osc_address_motors = f"/{args_main.name}_motors"
    osc_address_sensors = f"/{args_main.name}_sensors"

    # create server, listening on port 1234
    oscsrv = OSCsrv(port=args_main.port, queue=qu)
    oscsrv.add_method(
        path=osc_address_motors,
        # types='f'*number_of_motors,
        types='iiii',
        # use a partial here to bind the qu argument
        callback=partial(cb_hexagon_motors, qu)
        # callback=cb_hexagon_motors,
    )

    # # fix pd
    # target = liblo.Address(1337)
    # liblo.send(target, "/reconnect", 'bang')
    osc_target_hub = liblo.Address(1237)
    osc_target_trigrid = liblo.Address('localhost', 1235, liblo.UDP)

    # init motors
    if not args_main.dummy:
        cord = Sensorimotor(number_of_motors=args_main.number_of_motors,
                            verbose=DEBUG,
                            update_rate_Hz=args_main.update_rate_Hz)
    else:
        cord = SensorimotorDummy(number_of_motors=args_main.number_of_motors,
                                 verbose=DEBUG,
                                 update_rate_Hz=args_main.update_rate_Hz)

    try:
        # checking for motors
        N = cord.ping()
        print("osc2sensorimotor.main found {0} sensorimotors".format(N))
        sleep(0.2)

        # # TODO: set this according to your supply voltage and desired max. motor speed
        # cord.set_voltage_limit([0.8, 0.8, 0.8])

        # starting motorcord
        cord.start()

        # beat = tst0
    except (Exception):
        print('osc2sensorimotor.main failed to start motors')
        pass

    # start loop
    beats = [[0.6, 0.0, 0.0], [0.0, 0.6, 0.0], [0.0, 0.0, 0.6]]
    cnt = 0
    try:
        while True:
            # print('main loop cnt = {0}'.format(cnt))
            qud = None
            while qu.qsize() > 0:
                qud = qu.get()
                # qud[0] = 0
                # print('qu {0}'.format(qud))
                # QUD[0,:] = np.array(qud)
                # QUD = np.roll(QUD, shift=1, axis=0)

                # print('qud = {0}'.format(qud))

                # USER CODE HERE BEGIN
                # motors.apply_impulse(b[0])
                # b = beats[cnt%3]
                if qud is not None:
                    loopfunc_hexagon(qud[1], cord)
            # time.sleep(1e-3)

            cnt += 1
            # print('cnt={0}'.format(cnt))

            if cnt % 1000 == 0:
                # gng.stop_training()

                # f = open('gng.bin', 'wb')
                # pickle.dump(gng, f)
                # f.close()
                print('cnt = {0}'.format(cnt))
                # pass

            for i in args_main.smnode_ids:
                x = cord.get_raw_data_recv(i, 11)
                if i == 1:
                    print(i, x)
                # print(x[5], x[6])
                l_ = [i] + x
                oscsrv.server.send(osc_target_trigrid, osc_address_sensors,
                                   *l_)
            time.sleep(args_main.update_period_s)

    except (KeyboardInterrupt, SystemExit):
        print("key fin")
        oscsrv.isrunning = False
        # stopping motor cord
        print("\rAborted, stopping motors")
        cord.stop()
        # plt.ioff()
        # gng.stop_training()

    except:
        # Script crashed?
        print("\rException thrown, stopping cord.")
        cord.stop()
        raise

    print("____\nDONE.")
def main():
    motors = Sensorimotor(number_of_motors=2, verbose=False)

    try:
        # Check for motors
        N = motors.ping()
        print("Found {0} motors.".format(N))
        sleep(1.0)

        #voltage limit for supply voltage and desired max. motor speed
        motors.set_voltage_limit([0.16, 0.16])
        # Start motors
        motors.start()

        #these parameters influences the desired motor positon control behaviour
        motors.set_pos_ctrl_params(0,
                                   Kp=1.2,
                                   Ki=0.3,
                                   Kd=0.1,
                                   deadband=0,
                                   pulse_threshold=0)
        motors.set_pos_ctrl_params(1,
                                   Kp=1.2,
                                   Ki=0.3,
                                   Kd=0.1,
                                   deadband=0,
                                   pulse_threshold=0)

        motors.set_position([0.0, 0.0])
        sleep(2)

        # Print the current position
        print_position(motors.get_position())

        #TODO: set the position for the robot to move
        motors.set_position([])
        sleep(2)

        # Print the current position
        print_position(motors.get_position())

        #TODO: set the position for the robot to move
        motors.set_position([])
        sleep(2)

        # Print the current position
        print_position(motors.get_position())

        #TODO: set the position for the robot to move
        motors.set_position([])
        sleep(2)

        # Print the current position
        print_position(motors.get_position())

        #TODO: set the position for the robot to move
        motors.set_position([])
        sleep(2)

        # Print the current position
        print_position(motors.get_position())

        #TODO: set the position for the robot to move
        motors.set_position([])
        sleep(2)

        # Print the current position
        print_position(motors.get_position())

        motors.stop()

    except (KeyboardInterrupt, SystemExit):
        # Stop motors
        print("\rAborted, stopping motors")
        motors.stop()

    except:
        # Script crashed?
        print("\rException thrown, stopping motors")
        motors.stop()

    print("____\nDONE.")