Exemple #1
0
    def pb(self,who,unused=None,unusedA=None):
        if ((pyb.millis()  - self.pbTime) < State.pbBounceDelay):
            self.pbTime = pyb.millis()
            State.printT('PB BOUNCE!! delta millis:\t' +  str(pyb.millis()  - self.pbTime))
            return False
        State.printT('PB delta millis:\t' +  str(pyb.millis()  - self.pbTime))
        self.pbTime = pyb.millis()
        whoFuncs = (
            # this either steps the seq or toggles splitpot tracking if not sequencing
            (self.pb0Func,),  # pb 0 # either step sequence or toggle tracking if not sequencing
            # this is the one saves the preset,      
            (self.turnOnYellowLed, self.saveCurrentConfAsPreset),          # pb 1
            # Tremolo
            (self.toggleTrem,),                       # pb 2
            # Vibrato
            (self.toggleVib,),                        # pb 3
            (self.lcdMgr.onLeftButton,),              # pb 4
            (self.lcdMgr.onRightButton,))             # pb 5

        State.printT('PB:\t' + str(who))  
        res = False         
        for f in whoFuncs[who]:
            res = f() or res
        State.printT('pb returning: ' +str(res))
        return res # True if who in [2,3] else False
Exemple #2
0
    def send(self, buf, timeout=500):
        # power up
        self.reg_write(CONFIG, (self.reg_read(CONFIG) | PWR_UP) & ~PRIM_RX)
        pyb.udelay(150)

        # send the data
        self.cs.low()
        self.spi.send(W_TX_PAYLOAD)
        self.spi.send(buf)
        if len(buf) < self.payload_size:
            self.spi.send(b'\x00' * (self.payload_size - len(buf))) # pad out data
        self.cs.high()

        # enable the chip so it can send the data
        self.ce.high()
        pyb.udelay(15) # needs to be >10us
        self.ce.low()

        # blocking wait for tx complete
        start = pyb.millis()
        while pyb.millis() - start < timeout:
            status = self.reg_read_ret_status(OBSERVE_TX)
            if status & (TX_DS | MAX_RT):
                break

        # get and clear all status flags
        status = self.reg_write(STATUS, RX_DR | TX_DS | MAX_RT)
        if not (status & TX_DS):
            raise OSError("send failed")

        # power down
        self.reg_write(CONFIG, self.reg_read(CONFIG) & ~PWR_UP)
    def makegauge(self):
        '''
        Generator refreshing the raw measurments.
        '''
        delays = (5, 8, 14, 25)
        while True:
            self._bmp_i2c.mem_write(0x2E, self._bmp_addr, 0xF4)
            t_start = pyb.millis()
            while pyb.elapsed_millis(t_start) <= 5: # 5mS delay
                yield None
            try:
                self.UT_raw = self._bmp_i2c.mem_read(2, self._bmp_addr, 0xF6)
            except:
                yield None

            self._bmp_i2c.mem_write((0x34+(self.oversample_setting << 6)),
                                    self._bmp_addr,
                                    0xF4)

            t_pressure_ready = delays[self.oversample_setting]
            t_start = pyb.millis()
            while pyb.elapsed_millis(t_start) <= t_pressure_ready:
                yield None
            try:
                self.MSB_raw = self._bmp_i2c.mem_read(1, self._bmp_addr, 0xF6)
                self.LSB_raw = self._bmp_i2c.mem_read(1, self._bmp_addr, 0xF7)
                self.XLSB_raw = self._bmp_i2c.mem_read(1, self._bmp_addr, 0xF8)
            except:
                yield None
            yield True
Exemple #4
0
 def interpolateStep(self):
     if not self.interpolating:
         return
     complete = self.servoCount
     while (pyb.millis() - self.lastFrame < BIOLOID_FRAME_LENGTH):
         pass
     self.lastFrame = pyb.millis()
     for i in range(self.servoCount):
         diff = self.nextPose[i] - self.pose[i]
         if diff == 0:
             complete -= 1
         else:
             if diff > 0:
                 if diff < self.speed[i]:
                     self.pose[i] = self.nextPose[i]
                     complete -= 1
                 else:
                     self.pose[i] += self.speed[i]
             else:
                 if (-diff) < self.speed[i]:
                     self.pose[i] = self.nextPose[i]
                     complete -= 1
                 else:
                     self.pose[i] -= self.speed[i]
     if complete <= 0:
         self.interpolating = False
     self.writePose()
Exemple #5
0
 def enterObstacleAvoidanceScanState(self):
     # this function shuts down the FSM for a second or two
     self.log('Entering ObstacleAvoidanceScanState')
     self.ikEngine.travelX = 0
     self.ikEngine.travelRotZ = 0
     self.ikEngine.setupForWalk()
     self.ikEngine.bodyPosX = -50  # move the body forwards so the head clears the legs
     self.ikEngine.setupForWalk()
     self.ikEngine.bodyPosX = 0  # it will move back the next time the IK engine runs
     blue = pyb.LED(BLUE_LED)
     blue.on()
     openAngle = self.mapObstacleSpace()
     blue.off()
     if openAngle is None:
         # we didn't find any open areas, so switch to walking mode which will re-trigger obstacle mode again
         self.log("No openings found from scan")
         self.obstacleScanTurnTime = pyb.millis()
     else:
         # The IK Engine uses radians/s for rotation rate, so figure out the delta in radians and thus given a fixed
         # rotation rate figure out how long we need to turn in order to end up pointing in that direction
         openAngleRadians = math.radians(openAngle)
         self.obstacleScanTurnTime = pyb.millis() + int((abs(openAngleRadians) / FRONT_OBSTACLE_TURN_SPEED) * 1000)
         if openAngle > 0:
             self.log("Found opening at angle %d - turning left" % openAngle)
             self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
         else:
             self.log("Found opening at angle %d - turning right" % openAngle)
             self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
        def master():
            fut = Future()
            self.assertEqual((yield from x(10, 'happy')), 'happy')
            self.assertEqual((yield from w(x(10, 'pleased'), fut)), 'pleased')
            self.assertEqual(fut.result(), 'pleased')
            #logging.basicConfig(level=logging.DEBUG)
            #log.debug("Now %f", self.loop.time())

            # One that completes before the timeout
            t0 = pyb.millis()
            fut = Future()
            self.loop.call_soon(w(x(20, 'good'), fut))
            v = yield from wait_for(fut, 50)
            self.assertEqual(v, 'good')
            et = pyb.elapsed_millis(t0)
            self.assertTrue(20 <= et < 25,
                            'et was %rms (expected 20-25ms)' % et)

            # One that hits the timeout
            t0 = pyb.millis()
            fut = Future()
            self.loop.call_soon(w(x(20, 'fine'), fut))
            with self.assertRaises(TimeoutError):
                v = yield from wait_for(fut, 10)
            et = pyb.elapsed_millis(t0)
            self.assertTrue(10 <= et < 15,
                            'et was %rms (expected 10-15ms)' % et)
 def update(self):
     if pyb.millis() - self.lastDebounceTime > self.debounceDelay:
         reading = self.pin.value()
         if reading != self.lastReading:
             # we got a new value
             self.lastDebounceTime = pyb.millis()
             self.lastReading = reading
             if reading == self.onState and self.doOn:
                 self.doOn()
Exemple #8
0
def tone(f,t,b=0):
    global bz
    p = int(1000000/f)
    t1 = pyb.millis()+t
    while pyb.millis() < t1:
        bz.high()
        pyb.udelay(p)
        bz.low()
        pyb.udelay(p)
    if b!=0: pyb.delay(b)
Exemple #9
0
def stableValue(pin,stabilityPeriod=20):
    res = pin.value()
    if res:
        return res
    readTime = millis()
    while(millis()-readTime < stabilityPeriod):
        if res != pin.value():
            res = pin.value()
            readTime = millis()
        delay(1)
    return res
Exemple #10
0
 def update(self):
     ok,v = self.readA()
     if ok:  # read went ok
         curZone = self.detectionZone(v - self.baseVal)
         if (curZone): # we have passed a threshold,
             if curZone != self.prevZone: # changed zones!
                 self.tf() #tfunc must be not None
                 self.lastActionTime = millis()
                 self.prevZone = curZone
         if millis() - self.lastActionTime > self.timeOut:
             self.of() # oFunc must be not None
             self.lastActionTime = millis()
Exemple #11
0
    def _get_B5(self):
        '''
        Calculates and sets compensation value B5.
        '''
        if (self._B5 is None) or ((pyb.millis()-self._t_B5) > self._dt_B5):
            self.get_temperature()
            X1 = (self._UT-self._AC6)*self._AC5/2**15
            X2 = self._MC*2**11/(X1+self._MD)
            self._B5 = X1+X2
            self._t_B5 = pyb.millis()

        return
Exemple #12
0
def FLASH_WaitForLastOperation(timeout):
    tickstart = pyb.millis()
    while (__HAL_FLASH_GET_FLAG(FLASH_FLAG_BSY) != RESET):
        if (timeout != HAL_MAX_DELAY):
            if (timeout == 0 or (pyb.millis() - tickstart) > timeout):
                return False
    if __HAL_FLASH_GET_FLAG(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR |
                            FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR |
                            FLASH_FLAG_PGSERR | FLASH_FLAG_RDERR) != RESET:
        # FLASH_SetErrorCode()
        return False
    return True
  def run( self ) :
    self._display.fill(0)
    sw = pyb.Switch()
    lasttime = pyb.millis()
    while sw() == False :
      pyb.delay(DISPLAY_DELAY)
      distance = self._getdistance()

      thistime = pyb.millis()
      t = thistime - lasttime
      self.printdistance(distance)
      self._rangepoint.update(self._display, distance, t / 1000.0)
      lasttime = thistime
Exemple #14
0
def master():
    nrf = NRF24L01(SPI(2), Pin('Y5'), Pin('Y4'), payload_size=8)

    nrf.open_tx_pipe(pipes[0])
    nrf.open_rx_pipe(1, pipes[1])
    nrf.start_listening()

    num_needed = 16
    num_successes = 0
    num_failures = 0
    led_state = 0

    print('NRF24L01 master mode, sending %d packets...' % num_needed)

    while num_successes < num_needed and num_failures < num_needed:
        # stop listening and send packet
        nrf.stop_listening()
        millis = pyb.millis()
        led_state = max(1, (led_state << 1) & 0x0f)
        print('sending:', millis, led_state)
        try:
            nrf.send(struct.pack('ii', millis, led_state))
        except OSError:
            pass

        # start listening again
        nrf.start_listening()

        # wait for response, with 250ms timeout
        start_time = pyb.millis()
        timeout = False
        while not nrf.any() and not timeout:
            if pyb.elapsed_millis(start_time) > 250:
                timeout = True

        if timeout:
            print('failed, respones timed out')
            num_failures += 1

        else:
            # recv packet
            got_millis, = struct.unpack('i', nrf.recv())

            # print response and round-trip delay
            print('got response:', got_millis, '(delay', pyb.millis() - got_millis, 'ms)')
            num_successes += 1

        # delay then loop
        pyb.delay(250)

    print('master finished sending; succeses=%d, failures=%d' % (num_successes, num_failures))
Exemple #15
0
 def update(self):
     if self.autoOff:  # we look for timeout, otherwise keep going
         if millis() - self.lastActionTime > self.timeOut:
             self.of() # oFunc must be not None
             self.lastActionTime = millis()
             return  # we're done!
     ok,v = self.readA()
     if ok:  # read went ok
         curZone = self.detectionZone(v - self.baseVal)
         if (curZone): # we have passed a threshold,
             if curZone != self.prevZone: # changed zones!
                 self.tf() #tfunc must be not None
                 self.lastActionTime = millis()
                 self.prevZone = curZone
Exemple #16
0
 def update(self):
     # if there's been enouh time since last bounce, then take a reading
     if millis() - self.lastDebounceTime > self.debounceDelay:
         reading = self.pin.value()
         # if the reading is different from the last one,
         if reading != self.lastReading:
             # we got a new value:
             # * update bounce time
             # * update reading
             # * and if reading is HIGH, execute the onHigh action
             self.lastDebounceTime = millis()
             self.lastReading = reading
             if reading and self.onHigh:
                 self.onHigh()
Exemple #17
0
def setPWM(input):
    global trigcheck
    global trig_time

    for i,val in enumerate(input):
        out = int(val) * 10
        if out == 90: out = 100
        pin[i].pwm(out)

        # As its slow check for more triggers whilst writing pins
        if adc1.read() > 50 and (millis() - trig_time) >= 500:
            trigcheck = True
            print('blip')
            trig_time = millis()
            return
Exemple #18
0
def ps2int_read():
  
    global inhibiting,DataPin,head,bufferSize,tail,buff,bitcount,incoming,prev_ms 

    if (inhibiting):
        return # do nothing when clock manipulated by Arduino

    print('In: ps2int_read')
    
    val = DataPin.value()
    now_ms = pyb.millis()
    if (now_ms - prev_ms > 250):
        bitcount = incoming = 0
        print('\tIn: (now_ms - prev_ms > 250)')
    prev_ms = now_ms
    n = bitcount - 1
    if (n <= 7 and n >=0):
        incoming |= (val << n)
        print('\tIn: (n <= 7 and n >=0)')
    bitcount +=1
    if (bitcount == 11):
        print('\tIn: (bitcount == 11)')
        i = head + 1
        print('\tIn: (bitcount == 11), after head..')
        print('\tIn: bufferSize: ', str(bufferSize))
        if (i >= bufferSize):
            print('\t\tIn: (i >= bufferSize)')
            i = 0
        if (i != tail):
            print('\t\tIn: (i != tail)')
            buff[i] = incoming
            head = i
        bitcount = 0
        incoming = 0
    print('Leaving: ps2int_read')
Exemple #19
0
 def blink(self, relais):
     if pyb.elapsed_millis(self.blink_start) < self.blink_time:
         relais.low()
     else:
         relais.high()
         if pyb.elapsed_millis(self.blink_start) > 2 * self.blink_time:
             self.blink_start = pyb.millis()
Exemple #20
0
 def update(self):
     current = pyb.millis()
     if current - self._last_time >= 1000:
         print(self._count)
         self._last_time = current
         self._count = 0
     self._count += 1
def test_main():
    """Test function for verifying basic functionality."""
    print("Running test_main")
    i2c = I2C(1, I2C.MASTER)
    lcd = I2cLcd(i2c, 0x27, 2, 16)
    lcd.putstr("It Works!\nSecond Line")
    delay(3000)
    lcd.clear()
    count = 0
    while True:
        lcd.move_to(0, 0)
        lcd.putstr("%7d" % (millis() // 1000))
        delay(1000)
        count += 1
        if count % 10 == 3:
            print("Turning backlight off")
            lcd.backlight_off()
        if count % 10 == 4:
            print("Turning backlight on")
            lcd.backlight_on()
        if count % 10 == 5:
            print("Turning display off")
            lcd.display_off()
        if count % 10 == 6:
            print("Turning display on")
            lcd.display_on()
        if count % 10 == 7:
            print("Turning display & backlight off")
            lcd.backlight_off()
            lcd.display_off()
        if count % 10 == 8:
            print("Turning display & backlight on")
            lcd.backlight_on()
            lcd.display_on()
Exemple #22
0
 def enterObstacleAvoidanceState(self):
     self.log('Entering ObstacleAvoidanceState')
     self.ikEngine.travelX = 0
     self.turnTimeoutTime = pyb.millis() + FRONT_OBSTACLE_TURN_TIMEOUT
     if (self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE) & (
         self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE):
         # stuff on both sides, pick a direction at random to turn
         if pyb.rng() & 1 == 0:
             self.log('Obstacles on both sides, turning right')
             self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
         else:
             self.log('Obstacles on both sides, turning left')
             self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
     elif self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
         self.log('Obstacle on left side, turning right')
         self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
     elif self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
         self.log('Obstacle on right side, turning left')
         self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
     else:  # nothing on either side, so pick a side at random
         if pyb.rng() & 1 == 0:
             self.log('Only front obstacle, turning right')
             self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
         else:
             self.log('Only front obstacle, turning left')
             self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
Exemple #23
0
def loop():
    global bitcount,incoming,prev_ms,valRead,buff,rCount,readBuff,lreadBuff,rIndex,oIndex,buffSize

    if (oIndex != rIndex):
        now_ms = pyb.millis()
        if (now_ms - prev_ms > 250):
            bitcount = incoming = 0
            buff=[]
            oIndex=rIndex=rCount=0
            print('\tIn: (now_ms - prev_ms > 250)')
        prev_ms = now_ms
        if (bitcount <= 7):
            incoming |= (readBuff[oIndex] << bitcount)
            oIndex = (1+oIndex) % buffSize
        if (bitcount ==7 ):
            #print('\tIn: (bitcount == 7)')
            #print ('incoming= %d'%incoming)
            buff = buff+[incoming]
            rCount+=1
            if (rCount==3):
                print('buff: ',str(buff))
                rCount=0
                buff=[]
            bitcount=0
            incoming=0
        else:
            bitcount+=1
    if (readBuff!=lreadBuff):
        print('rIndex: ',str(rIndex),'  oIndex: ', str(oIndex))
        print('readBuff= ', str(readBuff))
        for i in range(len(lreadBuff)):
            lreadBuff[i]=readBuff[i]
Exemple #24
0
def fp_speed(n = 80000):
	x = 1.1
	t1 = pyb.millis()
	while n != 0:
		x += 0.1
		n -= 1
	print ("Elapsed ", pyb.elapsed_millis(t1))
Exemple #25
0
    def update(self):
        # TODO: check for constants used
        # TODO: return x and y changes
        surface_quality = self.read_register(ADNS3080_SQUAL)
        # small delay
        pyb.udelay(50)

        # check for movement, update x,y values
        motion_reg = self.read_register(ADNS3080_MOTION)
        _overflow = ((motion_reg & 0x10) != 0)              # check if we've had an overflow # TODO: do something whit this info
        if( (motion_reg & 0x80) != 0 ):
            raw_dx = self.read_register(ADNS3080_DELTA_X, signed=True)
            # small delay
            pyb.udelay(50)
            raw_dy = self.read_register(ADNS3080_DELTA_Y, signed=True)
            self._motion = True
        else:
            raw_dx = 0
            raw_dy = 0

        last_update = pyb.millis()

        # Fix for orientation if needed
        #self.apply_orientation_matrix()

        self.dx = raw_dx
        self.dy = raw_dy

        self.x += raw_dx
        self.y += raw_dy

        return True
Exemple #26
0
 def _frame_fixed_timed(self, fixed_value, stage_time):
     t_start = pyb.millis()
     t_elapsed = -1
     while t_elapsed < stage_time: 
         for line in range(LINES_PER_DISPLAY -1, -1, -1): 
             self._line_fixed(line, fixed_value, set_voltage_limit = False)
         t_elapsed = pyb.elapsed_millis(t_start)
Exemple #27
0
 def new_fix_time(self):
     """Updates a high resolution counter with current time when fix is updated. Currently only triggered from
     GGA, GSA and RMC sentences"""
     try:
         self.fix_time = pyb.millis()
     except NameError:
         self.fix_time = time.time()
Exemple #28
0
    def __init__(self, Vpin, Afunc, AfuncArg='chan 0_1', initialcharge=100, batteryAH=110, Aoffset=0):

        # Setup PINS and functions
        self.voltage_adc = pyb.ADC(Vpin)
        self.Afunc = Afunc
        self.AfuncArg = AfuncArg

        # create and set variables
        self.Battery_AH_Capacity = batteryAH
        self.AH = batteryAH * initialcharge / 100
        self.V = 0
        self.A = 0
        self.AH_15 = 0
        self.AH_60 = 0
        self.AH_day = 0
        self.capacityV = 0
        self.capacityAH = 0
        self.Aoffset = Aoffset

        # start timer
        self.timer = pyb.millis()
        self.last_write = time.time()
        self.log_message = ''
        self.write_flag = False
        self.update()
Exemple #29
0
 def get_pid(self, error, scaler):
   tnow = millis()
   dt = tnow - self._last_t
   output = 0
   if self._last_t == 0 or dt > 1000:
     dt = 0
     self.reset_I()
   self._last_t = tnow
   delta_time = float(dt) / float(1000)
   output += error * self._kp
   if abs(self._kd) > 0 and dt > 0:
     if isnan(self._last_derivative):
       derivative = 0
       self._last_derivative = 0
     else:
       derivative = (error - self._last_error) / delta_time
     derivative = self._last_derivative + \
                  ((delta_time / (self._RC + delta_time)) * \
                   (derivative - self._last_derivative))
     self._last_error = error
     self._last_derivative = derivative
     output += self._kd * derivative
   output *= scaler
   if abs(self._ki) > 0 and dt > 0:
     self._integrator += (error * self._ki) * scaler * delta_time
     if self._integrator < -self._imax: self._integrator = -self._imax
     elif self._integrator > self._imax: self._integrator = self._imax
     output += self._integrator
   return output
    def play(self, pattern, kit=None):
        # channel volume
        self.midiout.control_change(10, self.volume, ch=self.channel)
        self.activate_drumkit(kit)
        # give MIDI instrument some time to load drumkit
        delay(200)

        try:
            while True:
                last_tick = millis()
                pattern.playstep(self.midiout, self.channel)
                timetowait = max(0, self.mpt - (millis() - last_tick))
                if timetowait > 0:
                    delay(int(timetowait))
        finally:
            # all sound off
            self.midiout.control_change(120, 0, ch=self.channel)
# -------- End of interrupt section ----------------

# Define constants for main program loop - shown in UPPERCASE
M = 50						# number of instantaneous energy epochs to sum
BEAT_THRESHOLD = 2.0		# threshold for c to indicate a beat
SILENCE_THRESHOLD = 1.3		# threshold for c to indicate silence

# initialise variables for main program loop
e_ptr = 0					# pointer to energy buffer
e_buf = array('L', 0 for i in range(M))	# reserve storage for energy buffer
sum_energy = 0				# total energy in last 50 epochs
oled.draw_text(0,20, 'Ready to GO')	# Useful to show what's happening?
oled.display()
pyb.delay(100)
tic = pyb.millis()			# mark time now in msec

beattime = pyb.millis()

while True:				# Main program loop
	if buffer_full:		# semaphore signal from ISR - set if buffer is full

		# Calculate instantaneous energy
		E = energy(s_buf)

		# compute moving sum of last 50 energy epochs
		sum_energy = sum_energy - e_buf[e_ptr] + E
		e_buf[e_ptr] = E		# over-write earlest energy with most recent
		e_ptr = (e_ptr + 1) % M	# increment e_ptr with wraparound - 0 to M-1

		# Compute ratio of instantaneous energy/average energy
 def tick(self, latest_millis=None):
     if latest_millis is None:
         latest_millis = pyb.millis()
     self.latest_millis = latest_millis
     pass
Exemple #33
0
 def __init__(self, logFilename):
     self.file = open('/sd/%s' % logFilename, 'wt')
     self.startCount = pyb.millis()
     self.file.write("Logger Start\n")
Exemple #34
0
    elif state['state'] == PUMP_ON:
        state['state'] = PUMP_ON
    state.update()
    return state


pid_controller = Controller(display=Display(pinout={
    'sda': 'Y10',
    'scl': 'Y9'
},
                                            height=64,
                                            external_vcc=False),
                            initial_state={
                                'state': PUMP_OFF,
                                'mode': SHOT,
                                'start_time': pyb.millis(),
                                'set_temp': DEFAULT_SET_TEMP,
                                'steam_temp': DEFAULT_STEAM_TEMP,
                                'max_temp': MAX_TEMP,
                                'boiler_temp': 0
                            },
                            view=view,
                            controller=controller,
                            up_pin='X9',
                            down_pin='X10',
                            shot_switch=Switch('X11'),
                            steam_switch=Switch('Y8'))

if __name__ == '__main__':
    pid_controller.run()
Exemple #35
0
sensor.set_hmirror(True)
sensor.set_windowing((int((sensor.width() / 2) - ((sensor.width() / 2) * FRAME_WIDE)), int(sensor.height() * (1.0 - FRAME_REGION)), \
                     int((sensor.width() / 2) + ((sensor.width() / 2) * FRAME_WIDE)), int(sensor.height() * FRAME_REGION)))
sensor.skip_frames(time = 200)
if COLOR_LINE_FOLLOWING: sensor.set_auto_gain(False)
if COLOR_LINE_FOLLOWING: sensor.set_auto_whitebal(False)
clock = time.clock()
#sensor.set_auto_exposure(False, \
#    exposure_us = 300)


###########
# Loop
###########

old_time = pyb.millis()

throttle_old_result = None
throttle_i_output = 0
throttle_output = THROTTLE_OFFSET

steering_old_result = None
steering_i_output = 0
steering_output = STEERING_OFFSET

while True:
    clock.tick()
    img = sensor.snapshot()
    img.binary(COLOR_HIGH_LIGHT_THRESHOLDS if COLOR_LINE_FOLLOWING else GRAYSCALE_HIGH_LIGHT_THRESHOLDS, zero = True)
    img.histeq()
Exemple #36
0
 def timestamp(self):
     return pyb.millis() - self.startCount
Exemple #37
0
def SensorHubDbg(itv=50):
    gui = SwimDbg()
    gui.set_font(gui.FONT6x13)
    gui.set_fill_color(pyb.SWIM.WHITE)
    fontH = 13
    h = fontH + 1
    lTitle = ['A=', 'G=', 'M=', 'E=', 'P=', 'T=']
    gui.update_display()
    # init sensor hub
    sh = sensors.frdm_fxs_multi2_b(2)
    sh.InitPressure()
    sh.InitAcc()
    sh.InitGyr()
    sh.InitMag()
    imu = pyb.IMU(0, 0, delta_ms=0.02)
    agm = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0.0], [0.0]]
    s = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [''], ['']]
    e = [0.0, 0.0, 0.0]
    l0 = bytearray(6)
    l1 = bytearray(6)
    l2 = bytearray(6)
    t1 = pyb.millis()
    x = 23
    y = 31
    dx = 5
    dy = 6
    isInflate = False
    amp = 0.7
    demo_st = 0

    btn_sw4 = pyb.Pin("04")
    btn_sw4_prev = btn_sw4.value()
    for cnt in range(9999999):
        if btn_sw4_prev == 1 and btn_sw4.value() == 0:
            demo_st += 1
            if demo_st >= 3:
                demo_st = 0
        btn_sw4_prev = btn_sw4.value()
        if demo_st != 2:
            if (cnt & 15) == 0:
                p = sh.ReadPressure()
                t = sh.ReadTemp()
            sh.ReadAcc(l0)
            sh.ReadGyr(l1)
            sh.ReadMag(l2)
            sh._prvDataProc(l0, 0, agm[0])
            sh._prvDataProc(l1, 1, agm[1])
            sh._prvDataProc(l2, 2, agm[2])
            t2 = pyb.millis()
            dt = t2 - t1
            t1 = t2
            e = imu.step(l0, l1, None, dt / 1000.0)
            agm[3][0] = e[0]
            agm[3][1] = e[1]
            agm[3][2] = e[2]
            # >>> format AGM string
            for j in range(4):
                for i in range(3):
                    if agm[j][i] >= 0:
                        s[j][i] = '+%.1f' % agm[j][i]
                    else:
                        s[j][i] = '%.1f' % agm[j][i]
            # <<<
            if (cnt & 7) == 0:
                print('P=', p, 'T=', t, 'A=', s[0][0], s[0][1], s[0][2], 'G=',
                      s[1], 'M=', s[2])
            agm[4] = [p]
            agm[5] = [t]
            s[4] = ['%.1f hPa' % p]
            s[5] = ['%.1f dgrC' % t]
            gui.clear_screen(0)
            for i in range(6):
                gui.put_text_xy(lTitle[i], 2, 2 + i * h)
                if i < 4:
                    str = "%s,%s,%s" % (s[i][0], s[i][1], s[i][2])
                else:
                    str = s[i][0]
                #gui.set_pen_color(pyb.SWIM.BLACK)
                #gui.put_box(16, y, 120, y + fontH)
                #gui.set_pen_color(pyb.SWIM.WHITE)
                gui.put_text_xy(str, 16, 2 + i * h)
            gui.put_text_xy('itv=%dms' % dt, 2, 2 + 6 * h)

        # >>> draw bouncing flower
        x += dx
        y += dy
        if x > 22 * 16:
            dx = -dx
        if y > 22 * 16:
            dy = -dy
        if x < 0:
            dx = -dx
        if y < 0:
            dy = -dy
        # <<<
        if isInflate == True:
            amp *= 1.01
            if amp >= 0.77:
                isInflate = False
        else:
            amp /= 1.01
            if amp < 0.45:
                isInflate = True
        #
        if demo_st == 1:
            draw_clock_1(gui,
                         cnt,
                         div=2,
                         cx=72 + (x >> 4),
                         cy=72 + (y >> 4),
                         amp=amp * 0.8,
                         rLen=6,
                         leaves=6)
        if demo_st == 2:
            gui.clear_screen(0)
            draw_clock_1(gui,
                         cnt,
                         div=3,
                         cx=64,
                         cy=64,
                         amp=amp * 1.22,
                         rLen=9,
                         leaves=9)
        gui.update_display()
        pyb.delay(itv)
    return sh
Exemple #38
0
# centroid of the largest blob in each roi. The x position of the centroids
# will then be averaged with different weights where the most weight is assigned
# to the roi near the bottom of the image and less to the next roi and so on.
ROIS = [  # [ROI, weight]
    (38, 1, 90, 38, 0.4), (35, 40, 109, 43, 0.6), (0, 79, 160, 41, 0.2)
]

blue_led = LED(3)

old_error = 0
measured_angle = 0
set_angle = 90  # this is the desired steering angle (straight ahead)
p_term = 0
i_term = 0
d_term = 0
old_time = pyb.millis()
radians_degrees = 57.3  # constant to convert from radians to degrees


def constrain(value, min, max):
    if value < min:
        return min
    if value > max:
        return max
    else:
        return value


ch1 = tim.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P7"))
ch2 = tim.channel(2, pyb.Timer.PWM, pin=pyb.Pin("P8"))
def step1(): #turning 1sec to the right
	B_forward(50)
	beattime = pyb.millis()
	while pyb.millis() < beattime + 1000:
		pass
	B_stop()
Exemple #40
0
    oled.clear()
    oled.draw_text(0, 0, "Testing Motors")

    oled.draw_text(0, 10, 'PWM:{:4d}'.format(speed))
    oled.draw_text(0, 20, 'SpeedA:{:5.2f} rps'.format(motor.speedA / 39))
    oled.draw_text(0, 30, 'SpeedB:{:5.2f} rps'.format(motor.speedB / 39))

    oled.display()
    try:
        send_uart("Motor Test")
    except OSError:
        print('UART send error')


tic = pyb.millis()
while True:
    #	y_LED.off()
    g_LED.off()
    r_LED.off()
    b_LED.toggle()
    if (read_sw() == 0):
        motor.stop()
        test_mic()
    elif (read_sw() == 1):
        motor.stop()
        toc = pyb.millis()
        test_imu(toc - tic)
        tic = pyb.millis()
    elif (read_sw() == 2):
        test_motor()
Exemple #41
0
    min_max = min(min_max, sim.max())
    max_max = max(max_max, sim.max())
    i += 1

print("============= Press throttle now =============")

for led_count in range(0, 20):
    set_servos(prev_throttle - 80)
    pyb.LED(1).toggle()
    pyb.delay(200 - (led_count * 10))

pyb.LED(1).off()

movement_found = False
throttle = prev_throttle - 20
start = pyb.millis()

while (not movement_found):
    clock.tick()
    print("throttle=", throttle)
    img = sensor.snapshot()

    sim = img.get_similarity(extra_fb)
    if (sim.min() < min_min - .3 or sim.min() > max_min + .09
            or sim.max() > max_max + .01 or sim.max() < min_max - .01):
        print("min_min=", min_min, ", max_min=", max_min)
        print("min_max=", min_max, ", max_max=", max_max)
        print("difference", sim)
        print("prev min throttle=", prev_throttle)
        print("new min throttle=", throttle)
        movement_found = True
Exemple #42
0
 def reset(self):
     self.previous = pyb.millis()
Exemple #43
0
def run_uav_test(i2c_bus=2):
    global SIGNALS
    global SIGNAL_USR

    SIGNALS[0] = 0
    serial_port = USB_VCP()
    nmea_line = None
    # serial_port.setinterrupt(-1)
    disp = display.create_spi_display(SSD1322, 256, 64)
    i2c = I2C(i2c_bus, freq=400000)
    devices = i2c.scan()
    lsm303 = LSM303D(i2c)
    switch = Switch()
    speed_pid = PID(target=500, kp=.4, ki=.2, kd=.1)
    uav['pid'] = speed_pid
    timestamp = None
    w = 0

    screen_renderers = [
        render_gps_screen, render_hud_screen, render_imu_screen
    ]
    renderer_idx = 0
    render_screen = screen_renderers[renderer_idx]

    switch.callback(switch_cb)

    while True:
        # retrieving data
        accel, mag = lsm303.read()

        if serial_port.any():
            nmea_line = str(serial_port.readline(), 'utf-8')

        # processing data
        x, y, z = accel
        x, z, y = mag
        uav['imu']['north'] = atan2(y, x) * 180.0 / pi

        if nmea_line:
            update_gps_data(nmea_line)
            nmea_line = None

        # sending orders
        if renderer_idx % 2:
            if timestamp:
                pid_value = speed_pid.update(
                    uav['speed'],
                    elapsed_millis(timestamp) / 1000.0)
                adjust_throttle(serial_port, pid_value)

            timestamp = millis()

        if SIGNALS[0] & SIGNAL_USR:
            renderer_idx = renderer_idx + 1
            render_screen = screen_renderers[renderer_idx %
                                             len(screen_renderers)]
            SIGNALS[0] = SIGNALS[0] & ~SIGNAL_USR

        render_screen(disp.framebuf, uav)

        disp.send_buffer()
        delay(50)
Exemple #44
0
def face_recog(calc_time):
    pin = pyb.millis()
    print(pin)
    print(calc_time)
    cc = 0
    #print(pyb.elapsed_millis(pin))
    while (pyb.elapsed_millis(pin)) < calc_time:
        print("top of face recog function")
        #snapshot on face detection
        RED_LED_PIN = 1
        BLUE_LED_PIN = 3
        sensor.reset()  # Initialize the camera sensor.
        sensor.set_contrast(3)
        sensor.set_gainceiling(16)
        sensor.set_pixformat(sensor.GRAYSCALE)
        sensor.set_framesize(sensor.HQVGA)  # or sensor.QQVGA (or others)
        sensor.skip_frames(time=2000)  # Let new settings take affect.
        face_cascade = image.HaarCascade("frontalface", stages=25)
        uos.chdir("/")
        pyb.LED(RED_LED_PIN).on()
        print("About to start detecting faces...")
        sensor.skip_frames(time=2000)  # Give the user time to get ready.
        pyb.LED(RED_LED_PIN).off()
        print("Now detecting faces!")
        pyb.LED(BLUE_LED_PIN).on()
        diff = 10  # We'll say we detected a face after 10 frames.
        #if (pyb.elapsed_millis(pin)) > calc_time:
        #continue
        try:
            while (diff):
                img = sensor.snapshot()
                faces = img.find_features(face_cascade,
                                          threshold=0.5,
                                          scale_factor=1.5)
                if faces:
                    diff -= 1
                    for r in faces:
                        img.draw_rectangle(r)
                elif (pyb.elapsed_millis(pin)) > calc_time:
                    raise Exception
        except Exception as e:
            print("Get Out Exception called")

        pyb.LED(BLUE_LED_PIN).off()
        print("Face detected! Saving image...")
        pic_name = "snapshot-person.pgm"
        sensor.snapshot().save(
            pic_name)  # Save Pic. to root of SD card -- uos.chdir("/")
        pyb.delay(100)
        snap_img = image.Image(pic_name, copy_to_fb=True).mask_ellipse()
        d0 = snap_img.find_lbp((0, 0, snap_img.width(), snap_img.height()))
        # face recognition
        pyb.LED(2).on()
        name_lbp_list = []
        uos.chdir(
            "/Faces"
        )  # change directory to where all the webex photos from tcp are stored
        for filename in uos.listdir("/Faces"):
            if filename.endswith(".pgm"):
                try:
                    img = None
                    img = image.Image(filename, copy_to_fb=True).mask_ellipse()
                    d1 = img.find_lbp((0, 0, img.width(), img.height()))
                    dist = image.match_descriptor(d0, d1, 50)
                    #print("weve matched")
                    word = filename
                    #print(filename)
                    und_loc = word.index('_')
                    word = word[0:(und_loc)]
                    name_lbp_list.append(word)
                    name_lbp_list.append(dist)
                    continue
                except Exception as e:
                    print(e)
                    print("error reading file")
            else:
                print("ERROR")
        print(name_lbp_list)
        #print(len(name_lbp_list))
        end = 0
        name_avg = []
        i = 0
        start = 0
        while i < len(name_lbp_list):
            if ((i + 2) < len(name_lbp_list)) and (name_lbp_list[i] !=
                                                   name_lbp_list[i + 2]):
                end = i + 2
                #print(start)
                #print(end)
                face = []
                face = name_lbp_list[start:end]
                print(face)
                j = 1
                sum_lbp = 0
                while j < len(face):
                    sum_lbp += face[j]
                    j += 2
                name_avg.append(face[0])
                name_avg.append(sum_lbp / (len(face) / 2))
                start = i + 2
            i += 2
        face = []
        face = name_lbp_list[(end):(len(name_lbp_list))]
        print(face)
        j = 1
        sum_lbp = 0
        while j < len(face):
            sum_lbp += face[j]
            j += 2
        name_avg.append(face[0])
        name_avg.append(sum_lbp / (len(face) / 2))
        print(name_avg)
        lbps = []
        k = 1
        while k < len(name_avg):
            lbps.append(name_avg[k])
            k += 2
        print(lbps)
        #print(len(lbps))
        min_lbp = min(lbps)
        print(min_lbp)
        ind = lbps.index(min(lbps))
        #print(ind)
        ind += 1
        found_person = name_avg[2 * ind - 2]
        id_name = "The person you are looking at is: " + found_person
        print(id_name)
        #delete snapshot of person
        uos.remove("/snapshot-person.pgm")
        pyb.LED(2).off()
Exemple #45
0
 def __init__(self, interval_millis):
     self.interval = interval_millis
     self.previous = pyb.millis()
def step4():
	A_back(50)
	B_back(50)
	if pyb.millis()> beattime + 1000:
		A_stop()
		B_stop()
Exemple #47
0
front_face = image.HaarCascade("frontalface", stages=25)
print("Front Face Cascade:", front_face)

TOP_CHAN = 0
BOT_CHAN = 1

ROT_SPEED = 15  # Make this speed a function of the distance to the object.
LIMIT_MIN = 992
LIMIT_MAX = 2000

FACE_TIMEOUT = 500
X_MAG = 80
Y_MAG = 80

c = pyb.millis()
saved = []

while (True):
    img = sensor.snapshot()  # Take a picture and return the image.

    blobs = list(
        img.find_features(front_face, threshold=0.95, scale_factor=1.25))

    invalidated = (pyb.millis() - c > FACE_TIMEOUT) or \
        len(blobs) != len(saved) or \
        any([abs(f[0] - l[0]) > X_MAG or abs(f[1] - l[1]) > Y_MAG  for f, l in zip(blobs, saved)])

    if invalidated:
        c = pyb.millis()
def step3():
	A_forward(50)
	B_forward(50)
	if pyb.millis()> beattime + 1000:
		A_stop()
		B_stop()
def step2(): #turning 1sec to the left
	A_forward(50)
	if pyb.millis()> beattime + 1000:
		A_stop()
Exemple #50
0
def calculate_bottom_bounding_box(top_line):
    if (top_line):
        theta2 = theta(top_line)
        #print("theta=", theta2)
        x = top_line.x2()
        y = top_line.y2()
        height = img.height() >> 1
        width = abs(int(math.tan(math.radians(theta2)) * height))
        #print(x, y, height, width)
        return (x - width, y, max(width * 2, 1), height)
    return (0, img.height() >> 1, img.width(), img.height() >> 1)


clock = time.clock()

start_time = pyb.millis()
adjust_exposure(60)
print("time=", pyb.elapsed_millis(start_time))

i = 0
threshold = COLOR_THRESHOLDS[0]
while (True):
    clock.tick()
    start_us = pyb.micros()
    img = sensor.snapshot()
    snap_us = pyb.elapsed_micros(start_us)

    start_us = pyb.micros()
    line_all = img.get_regression(threshold, robust=True)
    all_us = pyb.elapsed_micros(start_us)
    if line_all:
Exemple #51
0
def recognition(timeout=500):
    face = None
    img = None

    matchMin = 999999
    matchUser = ''
    matchArr = []

    basePath = "photo"
    #basePath = "desc"
    users = os.listdir(basePath)

    time_start = pyb.millis()
    while not face:
        if pyb.elapsed_millis(time_start) > timeout:
            break
        img = sensor.snapshot()
        face = facsTest(img)
        checkDisplay(img)
    if not face:
        return matchUser, face, img

    if not len(users):
        # pyb.delay(timeout)
        return matchUser, face, img

    nowDesc = img.find_lbp(face)

    photoFpath = ""
    try:
        for user in users:
            userDescArr = []
            baseDpath = "%s/%s" % (basePath, user)
            files = os.listdir(baseDpath)
            for file_ in files:
                # descFpath = baseDpath+"/"+file_
                # oldDesc = image.load_descriptor(descFpath)

                photoFpath = baseDpath + "/" + file_
                oldImg = image.Image(photoFpath)
                oldDesc = oldImg.find_lbp(
                    (0, 0, oldImg.width(), oldImg.height()))

                match = image.match_descriptor(nowDesc, oldDesc,
                                               DESC_THRESHOLD,
                                               DESC_FILTER_OUTLIERS)
                userDescArr.append(match)
            userDescArr.sort()
            sliceCnt = CHECK_MIN_CNT or len(userDescArr)
            matchResult = sum(userDescArr[:sliceCnt]) / sliceCnt
            matchArr.append(matchResult)
            # print("sliceCnt,userDescArr: ",sliceCnt,userDescArr)
            if matchResult < matchMin:
                matchMin = matchResult
                if matchResult < MATCH_THRESHOLD:
                    matchUser = user
    except:  # OSError,err:
        print("recognition error:", photoFpath)  # ,err)

    print(matchMin, matchUser, matchArr,
          len(matchArr) >= 2 and (matchArr[0] - matchArr[1]))
    return matchUser, face, img