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
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
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()
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()
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)
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
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()
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
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
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))
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
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()
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
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')
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()
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()
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
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]
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))
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
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)
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()
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()
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
def __init__(self, logFilename):
self.file = open('/sd/%s' % logFilename, 'wt')
self.startCount = pyb.millis()
self.file.write("Logger Start\n")
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()
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()
def timestamp(self):
return pyb.millis() - self.startCount
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
# 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()
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()
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
def reset(self):
self.previous = pyb.millis()
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)
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()
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()
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()
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:
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
