def test_motors():
            # Generate random speeds for motors
            speeds = [intbv(randrange(-2**10, 2**10), min = -2**10, max = 2**10) for i in range(9)]

            # Set motor speeds one at a time
            for i in range(1, 9):
                yield set_motor_speed(i, speeds[i])

            # Check actual duty cycles
            yield join(check_motor_duty_cycle(1, speeds[1]),
                       check_motor_duty_cycle(2, speeds[2]),
                       check_motor_duty_cycle(3, speeds[3]),
                       check_motor_duty_cycle(4, speeds[4]),
                       check_motor_duty_cycle(5, speeds[5]),
                       check_motor_duty_cycle(6, speeds[6]),
                       check_motor_duty_cycle(7, speeds[7]),
                       check_motor_duty_cycle(8, speeds[8]))

            # Regen random speeds
            speeds = [intbv(randrange(-2**10, 2**10), min = -2**10, max = 2**10) for i in range(9)]

            # Set all motor speeds together
            yield set_motors_speeds(speeds)

            # Check actual duty cycles
            yield join(check_motor_duty_cycle(1, speeds[1]),
                       check_motor_duty_cycle(2, speeds[2]),
                       check_motor_duty_cycle(3, speeds[3]),
                       check_motor_duty_cycle(4, speeds[4]),
                       check_motor_duty_cycle(5, speeds[5]),
                       check_motor_duty_cycle(6, speeds[6]),
                       check_motor_duty_cycle(7, speeds[7]),
                       check_motor_duty_cycle(8, speeds[8]))

        def test_servos():
            # Generate random consigns for servos
            # respect min/max useful consigns
            consigns = [intbv(randrange(12500, 62500))[16:] for i in range(9)]

            # Set servo consigns one at a time
            for i in range(1, 9):
                yield set_servo_consign(i, consigns[i])

            # Check actual duty cycles
            yield join(check_servo_duty_cycle(1, consigns[1]),
                       check_servo_duty_cycle(2, consigns[2]),
                       check_servo_duty_cycle(3, consigns[3]),
                       check_servo_duty_cycle(4, consigns[4]),
                       check_servo_duty_cycle(5, consigns[5]),
                       check_servo_duty_cycle(6, consigns[6]),
                       check_servo_duty_cycle(7, consigns[7]),
                       check_servo_duty_cycle(8, consigns[8]))

            # Regen random consigns
            # respect min/max useful consigns
            consigns = [intbv(randrange(12500, 62500))[16:] for i in range(9)]

            # Set all servo consigns together
            yield set_servos_consigns(consigns)

            # Check actual duty cycles
            yield join(check_servo_duty_cycle(1, consigns[1]),
                       check_servo_duty_cycle(2, consigns[2]),
                       check_servo_duty_cycle(3, consigns[3]),
                       check_servo_duty_cycle(4, consigns[4]),
                       check_servo_duty_cycle(5, consigns[5]),
                       check_servo_duty_cycle(6, consigns[6]),
                       check_servo_duty_cycle(7, consigns[7]),
                       check_servo_duty_cycle(8, consigns[8]))

 def set_rc_ports(forward_steps_array, backward_steps_array):
     """ Make all rc roll """
     print 'roll all rc'
     yield join(set_rc1_port(forward_steps_array[1], backward_steps_array[1]),
                set_rc2_port(forward_steps_array[2], backward_steps_array[2]),
                set_rc3_port(forward_steps_array[3], backward_steps_array[3]),
                set_rc4_port(forward_steps_array[4], backward_steps_array[4]))

def testJoin():
    tx = Signal(1)
    rx = tx
    rxData = intbv(0)
    for val in testvals:
        txData = intbv(val)
        yield join(rs232_rx(rx, rxData), rs232_tx(tx, txData, duration=T_10200))

    def bench(self):
        clk = Signal(0)
        sig1 = Signal(0)
        sig2 = Signal(0)
        td = 10

        def gen(s, n):
            for i in range(n-1):
                yield delay(td)
            s.next = 1
            yield delay(td)

        for i in range(10):
            offset = now()
            n0 = randrange(1, 50)
            n1 = randrange(1, 50)
            n2 = randrange(1, 50)
            sig1.next = 0
            sig2.next = 0
            yield join(delay(n0*td), gen(sig1, n1), gen(sig2, n2))
            assert sig1.val == 1
            assert sig2.val == 1
            assert now() == offset + td * max(n0, n1, n2)

        raise StopSimulation("Joined concurrent generator yield")

    def LeftRightToAngleDistanceTester(self, left_val, right_val, angle_val, distance_val):

        self.assertEquals(left_val,  0)
        self.assertEquals(right_val, 0)
        self.assertEquals(angle_val,    0)
        self.assertEquals(distance_val, 0)

        def stimulus(left_rand, right_rand):
            # print 'set angle to:', left_rand
            left_val.next = left_rand

            # print 'set distance to:', right_rand
            right_val.next = right_rand            

        def check(left_rand, right_rand):
            # print 'waiting for angle_val or distance_val'
            yield angle_val, distance_val

            # print 'left should be:', right_rand - left_rand
            self.assertEquals(angle_val, (right_rand - left_rand) / 2)

            # print 'right should be:', right_rand + left_rand
            self.assertEquals(distance_val, (right_rand + left_rand) / 2)

        for i in range(NR_TESTS):
            left_rand  = randrange(left_val.min,  left_val.max)
            right_rand = randrange(right_val.min, right_val.max)

            yield join(stimulus(left_rand, right_rand), check(left_rand, right_rand))

        print 'DONE'

        raise StopSimulation()

    def AngleDistanceToLeftRightTester(self, angle_val, distance_val, left_val, right_val):

        self.assertEquals(angle_val,    0)
        self.assertEquals(distance_val, 0)
        self.assertEquals(left_val,  0)
        self.assertEquals(right_val, 0)

        def stimulus(angle_rand, distance_rand):
            # print 'set angle to:', angle_rand
            angle_val.next = angle_rand

            # print 'set distance to:', distance_rand
            distance_val.next = distance_rand            

        def check(angle_rand, distance_rand):
            # print 'waiting for left_val and right_val'
            yield left_val, right_val

            # print 'left should be:', distance_rand - angle_rand
            self.assertEquals(left_val, distance_rand - angle_rand)

            # print 'right should be:', distance_rand + angle_rand
            self.assertEquals(right_val, distance_rand + angle_rand)

        for i in range(NR_TESTS):
            angle_rand    = randrange(angle_val.min, angle_val.max)
            distance_rand = randrange(distance_val.min, distance_val.max)

            yield join(stimulus(angle_rand, distance_rand), check(angle_rand, distance_rand))

        print 'DONE'

        raise StopSimulation()

 def ParityError(self):
     tx = Signal(intbv(0))
     rx = tx
     actual = intbv(0)
     cfg_rx = Config(parity=ODD)
     cfg_tx = Config(parity=EVEN)
     data = intbv(randrange(256))
     yield join(rs232_tx(tx, data, cfg_tx), rs232_rx(rx, actual, cfg_rx))

 def oddParity(self):
     tx = Signal(intbv(0))
     rx = tx
     actual = intbv(0)
     cfg = Config(parity=ODD)
     for i in range(256):
         data = intbv(i)
         yield join(rs232_tx(tx, data, cfg), rs232_rx(rx, actual, cfg))
         self.assertEqual(data, actual)

 def sevenBitsEvenParity(self):
     tx = Signal(intbv(0))
     rx = tx
     actual = intbv(0)
     cfg = Config(parity=EVEN, n_bits=7)
     cfg_rx = Config(parity=EVEN, n_bits=7)
     for i in range(256):
         data = intbv(i)
         yield join(rs232_tx(tx, data, cfg), rs232_rx(rx, actual, cfg_rx))
         self.assertEqual(data, actual)

 def bench(self, tx_baud_rate):
     tx = Signal(intbv(0))
     rx = tx
     actual = intbv(0)
     cfg_tx = Config(baud_rate=tx_baud_rate)
     cfg_rx = Config()
     for i in range(256):
         data = intbv(i)
         yield join(rs232_tx(tx, data, cfg_tx), rs232_rx(rx, actual, cfg_rx))
         if not data == actual:
             raise Error

        def read_adc_channel(number):
            master_to_slave = get_read_adc_channel_command(number)
            slave_to_master = intbv(0)

            channel = intbv(0)[3:]
            values = [intbv(randrange(2**10))[10:] for i in range(8)]

            yield join(fake_mcp3008(channel, values), spi_transfer(master_to_slave, slave_to_master))
            self.assertEquals(channel, number)
            self.assertEquals(slave_to_master[16:], values[number])
            print 'done'

    def RXTester(self, miso, mosi, sclk, ss_n,
                 txdata, txrdy, rxdata, rxrdy,
                 rst_n, n):

        def check():
            yield rxrdy
            print 'master sent:', hex(self.master_to_slave)
            print 'slave read:', hex(rxdata)
            self.assertEqual(rxdata, self.master_to_slave)

        for i in range(NR_TESTS):
            print "\nmaster write test"
            self.master_to_slave = intbv(randrange(2**n))[n:]
            self.slave_to_master = intbv(0)[n:]
            yield join(spi_transfer(miso, mosi, sclk, ss_n, self.master_to_slave, self.slave_to_master), check())

 def response():
     yield join(a, delay(30)), join(c, d)
     assert now() == 20

 def response():
     yield join(a, a)
     self.assertEqual(now(), 5)

 def wait_for_clk_period():
     yield join(clk.posedge, clk.negedge)

 def gen():
     yield join(delay(10), delay(20))

 def stimulus():
     a = Signal(0)
     yield join(delay(10), delay(20)), delay(5)
     assert now() == 5
     yield a
     raise AssertionError("Incorrect run")  # should not get here