def ReadValue():
while 1:
forward()
GPIO.output("P9_41",GPIO.HIGH)
time.sleep(1)
value = ADC.read("P9_40")
rawv = ADC.read_raw("P9_40")
voltage = value * 1.8 #1.8V
D = voltage/0.00699 #.00699
value2 = ADC.read("P9_36")
rawv2 = ADC.read_raw("P9_36")
voltage2 = value * 1.8 #1.8V
D2 = voltage2/0.00699
value3 = ADC.read("P9_38")
rawv3 = ADC.read_raw("P9_38")
voltage3 = value3 * 1.8 #1.8V
D3 = voltage3/0.00699
GPIO.output("P9_41",GPIO.LOW)
time.sleep(1)
if D and D3 >= 25:
stop()
print "Done!" #change this to turn on LED or buzzer
break
def scanread():
for count in range(5):
GPIO.output("P9_41",GPIO.HIGH)
time.sleep(1)
value = ADC.read("P9_40")
rawv = ADC.read_raw("P9_40")
voltage = value * 1.8 #1.8V
D = voltage/0.00699 #.00699
#41
value2 = ADC.read("P9_36")
rawv2 = ADC.read_raw("P9_36")
voltage2 = value * 1.8 #1.8V
D2 = voltage2/0.00699
#27
value3 = ADC.read("P9_38")
rawv3 = ADC.read_raw("P9_38")
voltage3 = value3 * 1.8 #1.8V
D3 = voltage3/0.00699
#15
print(D,D3)
time.sleep(1)
GPIO.output("P9_41",GPIO.LOW)
time.sleep(1)
def calibrate(self):
"""Calibrating the joystick module to correct sensitivity and movement"""
print("Calibrating Joystick Module, DO NOT MOVE JOYSTICK!")
time.sleep(2.0) # Time for user to read instructions..
self.x_origin = ADC.read_raw(self.xmove)
self.y_origin = ADC.read_raw(self.ymove)
print("Calibration Finished")
def do_sensor_read():
print 'sensor read'
global readings
readings = {}
# value = ADC.read("AIN1")
# adc returns value from 0 to 1.
# use read_raw(pin) to get V values
# tank = adc.read(tankPin)
tank = adc.read(tankPin) # have to read twice due to bbio bug
print 'tank is %s' % tank
time.sleep(1)
# photo = adc.read(photoPin) # have to read twice due to bbio bug
photo = 1.0-adc.read(photoPin) # reverse range so that 0 is darkest
print 'photo is %s' % photo
time.sleep(1)
# temp1 = adc.read_raw(thermistor1)
temp1 = adc.read_raw(thermistor1)
time.sleep(1)
print 'temp1 raw %s' % temp1
temp1 = convert_thermistor_special(temp1)
readings['bedTemp'] = temp1
print 'converted bed_temp is %s' % temp1
# # do conversion per
# # http://learn.adafruit.com/thermistor/using-a-thermistor
# temp2 = adc.read_raw(thermistor2)
temp2 = adc.read_raw(thermistor2)
time.sleep(1)
print 'temp2 raw %s' % temp2
print temp2
temp2 = convert_thermistor(temp2)
readings['tankTemp'] = temp2
print 'converted reservoir_temp is %s' % temp2
# do conversion per
# http://learn.adafruit.com/thermistor/using-a-thermistor
# tmp36reading = adc.read_raw(tmp36Pin)
# tmp36reading = adc.read_raw(tmp36Pin) # have to read twice due to bbio bug
# millivolts = tmp36reading * 1800 # 1.8V reference = 1800 mV
# temp_c = (millivolts - 500) / 10
# print temp_c
# ph_val = get_ph()
# print 'ph_val was thoght to be %s' % ph_val
readings['tankLevel'] = tank # tank level
readings['photocell'] = photo # photocell
def read_adc(self):
t = time.time()
l_value = ADC.read_raw(self.encoderPin[LEFT])
r_value = ADC.read_raw(self.encoderPin[RIGHT])
self.results[self.results_ind,0] = self.results_ind
self.results[self.results_ind,1] = t
self.results[self.results_ind,2] = l_value
self.results[self.results_ind,3] = r_value
self.results_ind = self.results_ind + 1
def readFlames(Flame1, Flame2, Flame3, rangeval):
sample1 = []
sample2 = []
sample3 = []
full = [
1799.0, 1799.0, 1799.0, 1799.0, 1799.0, 1799.0, 1799.0, 1799.0, 1799.0,
1799.0
]
for x in range(0, rangeval):
#collect values of sensor
Flame1reading = ADC.read_raw(Flame1)
Flame2reading = ADC.read_raw(Flame2)
Flame3reading = ADC.read_raw(Flame3)
if Flame1reading < 1780.0:
sample1 = sample1 + [Flame1reading]
if Flame2reading < 1780.0:
sample2 = sample2 + [Flame2reading]
if Flame3reading < 1780.0:
sample3 = sample3 + [Flame3reading]
#eliminate outliers if found
sample1 = [
e for e in sample1
if (np.median(sample1) - 2 * np.std(sample1) < e < np.median(sample1) +
2 * np.std(sample1))
]
if len(sample1) == 0:
sample1 = full
sample2 = [
e for e in sample2
if (np.median(sample2) - 2 * np.std(sample2) < e < np.median(sample2) +
2 * np.std(sample2))
]
if len(sample2) == 0:
sample2 = full
sample3 = [
e for e in sample3
if (np.median(sample3) - 2 * np.std(sample3) < e < np.median(sample3) +
2 * np.std(sample3))
]
if len(sample3) == 0:
sample3 = full
#takes average of values and stores for future use.
read1 = np.mean(sample1)
read2 = np.mean(sample2)
read3 = np.mean(sample3)
return {'FlameRead1': read1, 'FlameRead2': read2, 'FlameRead3': read3}
def readPotentiometerVal():
val = 3000
# Read raw adc value from P9_36 and save its value to "val" variable
val = ADC.read_raw("P9_36") ## Solution
return val
def run(self):
"""
Continuously monitor the A/D
:return:
"""
value = None
while True:
try:
for entry in self.pin_states:
if entry['enabled']:
if entry['mode'] == 'analog':
value = ADC.read(entry['pin'])
value = round(value, 4)
elif entry['mode'] == 'sonar':
value = ADC.read_raw(entry['pin'])
value = self.convert_to_distance(value)
digital_reply_msg = umsgpack.packb({u"command": "analog_read", u"pin": entry['pin'],
u"value": str(value)})
envelope = ("B" + self.board_num).encode()
self.publisher.send_multipart([envelope, digital_reply_msg])
time.sleep(0.05)
except KeyboardInterrupt:
sys.exit(0)
def IR_call():
#Sets up the ADC pins which we will need to read the raw voltage of the IR sensor
ADC.setup()
pub = rospy.Publisher('/range', Range, queue_size=10)
rospy.init_node('Range_Finder')
r = rospy.Rate(5) #Sets the code to be executed 5 times per second
IR.header.frame.id = "inertial_link"
# Radiation value is set to 1 because the SHARP sensor is an infared sensor
IR.radiation_type = 1
# the field of view was set to 20 degrees but the value ha to be provided in
IR.field_of_view = 3.14/9.0
# Minimum distance is set to 15 cm in order to avoid confusion where voltage could have two corresponding distances
IR.min_range = 0.15
# Maximum distance is 150 cm as sepcified by the spec sheet
IR.Max_range = 1.50
#Initalize IR sensor to have no value
IR.range = None
while not rospy.is_shutdown():
# We are going to use pin P9_39 because it worked and was used in the last assignment
# Want to store the data as a float so it can be used to find the distance
value = float(ADC.read_raw("P9_39"))
# Converts the voltage to distance by using an equation derived from the IR spec sheet
# Distance to voltage was best modeled as a 4th degree polynomial
distance = (20.065 * (value ** 4) - 155.71 * (value ** 3) + 443.24 * (value ** 2) - 570.96 * value + 324.71) * 100
# Stores the distance conversion as the range
IR.range = distance
rospy.loginfo(IR)
# Publishes the data of the IR sensor
pub.publish(IR)
r.sleep()
def task(self):
ADC.setup()
while not self.stop.isSet():
try:
value = ADC.read_raw(self.pin)
now = datetime.datetime.now()
print self.protocol + "%f : %d : %d : %d "%(value,now.minute,now.second,now.microsecond)
val = str(value)
date = str(now.minute) + ":" + str(now.second) + ":" + str(now.microsecond)
data = {'data':val,'sensor_code':self.sensor_code, 'date':date}
string_data = json.dumps(data)
if global_module.mode == 0:
global_module.wifi_namespace_reference.on_send(data)
else:
#print "usb is sending"
global_module.ep_out.write(string_data.encode('utf-8'),timeout=0)
if self.isLogging:
self.log_file.write(val + " : " + date + "\n")
sleep(self.rate);
except Exception as e:
print e;
self.log_file.write(e);
self.log_file.close();
e_msg = "Sorry! Unable to read Analog Data"
print e_msg
if global_module.mode == 0:
global_module.wifi_namespace_reference.on_error(e_msg)
else:
global_module.ep_out.write(e_msg.encode('utf-8'),timeout=0)
def do_work():
D = []
V = ADC.read_raw(sensor_pin)
#D.append(V)
T = V * TMP36_gain + TMP36_offset
D.append(T)
return D
def checkDarkBrightChange(self,_sv):
_genIsChanged = 0
value = ADC.read("P9_40")
value = ADC.read_raw("P9_40")
if _sv != self.old_static_light_val:
self.old_static_light_val = _sv
else:
if abs(self.old_val - value) < 20:
self.old_val = value
return _genIsChanged, value
if value < 80:
self.state = 0
else:
self.state = 1
#print value
if self.old_state != self.state:
strings = ('state has changed %s to %s Value = %d' % (self.str_state[self.old_state], self.str_state[self.state], value))
print ('state has changed %s to %s ' % (self.str_state[self.old_state], self.str_state[self.state]))
writeDataToFile('light_state.txt',strings)
_genIsChanged = [1, self.state]
self.old_state = self.state
self.old_val = value
return _genIsChanged, value
def do_work(): D = [] V = ADC.read_raw(sensor_pin) #D.append(V) T = V * TMP36_gain + TMP36_offset D.append(T) return D
def read_ir_thread_fcn(self):
""" Thread function for reading IR sensor values """
while self.run_flag:
for i in range(0, self.n_ir):
ADC_LOCK.acquire()
self.ir_val[i] = ADC.read_raw(config.IR[i])
time.sleep(ADCTIME)
ADC_LOCK.release()
def run():
cnt = 0
while cnt < size:
t = time.time() - t0
valList[cnt] = ADC.read_raw(encoderPin)
timeList[cnt] = t
cnt = cnt + 1
time.sleep(0.001)
def roasting(state):
blinkCount = 0
timePass = 0
timeStart = time.time()
while state == 8:
if (blinkCount <= 700):
GPIO.output(Status4, GPIO.HIGH)
blinkCount = blinkCount + 1
elif (700 < blinkCount <= 1400):
GPIO.output(Status4, GPIO.LOW)
blinkCount = blinkCount + 1
else:
blinkCount = 0
reading = ADC.read_raw(probe)
if (reading > 1393) and timePass > 30:
# Position servo in up position
state = 10 #allows for immediate state change to return state
servoCom = "Y" #set servo to start position
ser.write(servoCom)
#stop the mallow!
GPIO.output(motorA, GPIO.LOW)
GPIO.output(motorB, GPIO.LOW)
PWM.set_duty_cycle(motorPWM, (0))
GPIO.output(Status4, GPIO.LOW)
if (GPIO.input(SButton) == 1):
state = 9 #brings to pause state for manual advance
# timeNow=time.time()
# timePass=timeNow-timeStart
# if timeStart > 500 and reading<1370:
# while(state==8):
# GPIO.output(Status1, GPIO.HIGH)
# GPIO.output(Status2, GPIO.LOW) #LED pattern for failed roast
# GPIO.output(Status3, GPIO.HIGH)
# GPIO.output(Status4, GPIO.LOW)
# GPIO.output(Status5, GPIO.HIGH)
# if (GPIO.input(SButton) == 1):
# state=7 #brings to pause state for manual advance
#for demo day
timeNow = time.time()
timePass = timeNow - timeStart
if timePass > 60:
#stop the mallow!
GPIO.output(motorA, GPIO.LOW)
GPIO.output(motorB, GPIO.LOW)
PWM.set_duty_cycle(motorPWM, (0))
GPIO.output(Status4, GPIO.LOW)
state = 10 #allows for immediate state change to return state
servoCom = "Y" #set servo to start position
ser.write(servoCom)
return state
def read_air():
value = ADC.read_raw(AIR_SENSOR)
if value > 700:
level = "High pollution"
elif value > 300:
level = "Low pollution"
else:
level = "Air fresh"
streamer.log(":wind_blowing_face:Air Quality", level)
def position(state, Status3, minTherm):
blinkCount = 0
startCount = 0
servoCount = 0
thermistor = "P9_39"
#spin the mallow!
potVal = ADC.read(pot)
GPIO.output(motorA, GPIO.HIGH)
GPIO.output(motorB, GPIO.LOW)
PWM.set_duty_cycle(motorPWM, (100 * .56))
time.sleep(.2)
servoCom = "Y" #set servo to start position
ser.write(servoCom)
while state == 6:
if (GPIO.input(SButton) == 1):
state = 7 #return a state of 7 so manual state control can occur
if (blinkCount <= 1):
GPIO.output(Status3, GPIO.HIGH)
blinkCount = blinkCount + 1
elif (1 < blinkCount <= 3):
GPIO.output(Status3, GPIO.LOW)
blinkCount = blinkCount + 1
else:
blinkCount = 0
servoCom = "W" #move servo down (6) degrees
ser.write(servoCom)
#reads thermistor to check quality of position
time.sleep(.35)
thermistor = ADC.read_raw("P9_39")
servoCount = servoCount + 1
#if thermistor angle is where we want it
if (thermistor > (80 + minTherm) and startCount > 3):
servoCom = "W" #move servo down (6) degrees
ser.write(servoCom)
state = 8 #allows for immediate state change to roasting
elif (servoCount >= 29):
servoCount = 0
startCount = startCount + 1 #try again?
if startCount == 1: #go back to search
#GPIO.output(Status3, GPIO.LOW)
#state=4
state = 8
else:
servoCom = "Y" #set servo to start position
ser.write(servoCom)
return state
def Move():
while 1:
value = ADC.read("P9_40")
rawv = ADC.read_raw("P9_40")
voltage = value * 1.8 #1.8V
D = voltage/0.00699 #.00699
if D <= 50:
speak('Move bitch get out the way')
print('Move')
def _get_temp(self):
temps=[]
for j in range(len(self.fan_definitions)):
if self.fan_definitions[j]['has_temp'] == False :
temps.append('undefined')
else :
value = BADC.read_raw(self.fan_definitions[j]['temp_pin'])*1.0/4
temps.append(round(self._cal_temp(value, self.fan_definitions[j]['temp_type']),1))
#tempp.append(value)
return temps
def loop(self):
start = time.time()
v = list()
ua = list()
ca = list()
for x in range(0,10):
v.append(ADC.read_raw('AIN1'))
time.sleep(0.05)
ua.append(ADC.read_raw('AIN2'))
time.sleep(0.05)
ca.append(ADC.read_raw('AIN3'))
time.sleep(0.05)
finish = time.time()
d = finish - start
t = start + d/2
m = Message(to=['storage'],msg=['put', [t, d,
self.mapVoltage(sum(v)/10), self.mapUsed(sum(ua)/10),
self.mapCharged(sum(ca)/10)]])
self.MessageBox.put(m)
def read_light():
value = ADC.read_raw(LIGHT_SENSOR)
if value <= 300:
level = "dim"
if 301 <= value <= 500:
level = "average"
if 501 <= value <= 800:
level = "bright"
if value > 800:
level = "very bright"
streamer.log(":bulb:Brightness", level)
def readIRValues(self):
prevVal = self.irVal[self.ithIR]
ADC_LOCK.acquire()
self.irVal[self.ithIR] = ADC.read_raw(self.irPin[self.ithIR])
time.sleep(ADCTIME)
ADC_LOCK.release()
if self.irVal[self.ithIR] >= 1100:
self.irVal[self.ithIR] = prevVal
self.ithIR = ((self.ithIR+1) % 5)
def talker():
pub = rospy.Publisher('adc_val', UInt16, queue_size=10)
rospy.init_node('ADCIntTalker', anonymous=False)
rate = rospy.Rate(10)
ADC.setup()
print('ADC setup...')
while not rospy.is_shutdown():
adc_val = int(ADC.read_raw("P9_39"))
#rospy.loginfo('Talker: ADC value = ' + str(adc_val))
pub.publish(adc_val)
rate.sleep()
def gp2y0a12():
ADC.setup()
pub = rospy.Publisher('ir_sharp_80cm', String)
rospy.init_node('gp2y0a21yk0f')
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
measure = str(ADC.read_raw("P9_40"))
cad = "gp2y0a12: %s" % measure
rospy.loginfo(cad)
pub.publish(String(measure))
#rospy.sleep(1.0)
r.sleep()
def task(self): ADC.setup() while not self.stop.isSet(): value = ADC.read_raw(self.pin) now = datetime.datetime.now() print self.protocol + "%f : %d : %d : %d "%(value,now.minute,now.second,now.microsecond) val = str(value) + " : " + str(now.minute) + " : " + str(now.second) + " : " + str(now.microsecond)+"\n" global_module.wifi_namespace_reference.on_send(val); if self.isLogging: self.log_file.write(val) sleep(1);
def _get_raw_adc_data(cls, sensor):
raw_data = {
'value': None,
'scaled': None,
'raw': None,
}
pin = cls.SENSORS[sensor]['pin']
raw_data['value'] = ADC.read(pin) # ADC value between 0 - 1.0
raw_data[
'scaled'] = raw_data['value'] * 1.8 # ADC values scaled to 1.8V
raw_data['raw'] = ADC.read_raw(pin) # raw ADC value
return raw_data
def Value():
while 1:
GPIO.output("P9_41",GPIO.HIGH)
time.sleep(1)
value = ADC.read("P9_40")
rawv = ADC.read_raw("P9_40")
voltage = value * 1.8 #1.8V
D = voltage/0.00699 #.00699
#41
value2 = ADC.read("P9_36")
rawv2 = ADC.read_raw("P9_36")
voltage2 = value * 1.8 #1.8V
D2 = voltage2/0.00699
#27
value3 = ADC.read("P9_38")
rawv3 = ADC.read_raw("P9_38")
voltage3 = value3 * 1.8 #1.8V
D3 = voltage3/0.00699
def task(self): print "offline log started" ADC.setup() while not self.stop.isSet(): print type(self.log_file) value = ADC.read_raw(self.pin) now = datetime.datetime.now() print self.protocol + "%f : %d : %d : %d "%(value,now.minute,now.second,now.microsecond) val = str(value) + " : " + str(now.minute) + " : " + str(now.second) + " : " + str(now.microsecond)+"\n" if self.isLogging: self.log_file.write(val) sleep(1);
def ignite(debuglevel):
threshold=340 #150mv threshold
threshigh=1024 #450mv upper threshold
ADC.setup()
ADC.read_raw("AIN4") #Flush this
baseline=ADC.read_raw("AIN4")
GPIO.setup("P8_14",GPIO.OUT) #This pin fires the ignition
GPIO.output("P8_14",GPIO.LOW)
PWM.start("P8_19",15,49500) #works best with an 11 turn primary
time.sleep(0.05) #50ms Settling time for the analogue front end
ADC.read_raw("AIN4")
selftest=ADC.read_raw("AIN4")
if selftest>threshold and selftest<threshigh and baseline<128:
GPIO.output("P8_14",GPIO.HIGH)
if debuglevel:
print "Igniting"
time.sleep(2) #plenty of time for ignition
failure=0
else:
if debuglevel:
print "Failed"
failure=1
GPIO.output("P8_14",GPIO.LOW)
PWM.stop("P8_19")
PWM.cleanup()
#Debug output
if debuglevel:
print baseline
print selftest
return {'failure':failure, 'baseline':baseline ,'selftest':selftest }
def center(driveCom, minAll, max1, max2, max3):
blinkCount = 0
count = 0
centered = 0 #if program called, Jacques currently not centered
while (centered == 0): #while not centered
time.sleep(.3)
Flame1reading = ADC.read_raw(Flame1)
Flame2reading = ADC.read_raw(Flame2)
Flame3reading = ADC.read_raw(Flame3)
ScaledFlame1 = translate(Flame1reading, minAll, max1, 5, 100)
ScaledFlame2 = translate(Flame2reading, minAll, max2, 5, 100)
ScaledFlame3 = translate(Flame3reading, minAll, max3, 5, 100)
lowestFlame = min(ScaledFlame1, ScaledFlame2, ScaledFlame3)
#Check to be sure flame is, in fact, not centered
if (lowestFlame != ScaledFlame2):
if (ScaledFlame3 == lowestFlame):
GPIO.output(Status1, GPIO.HIGH)
driveCom = "d" #turn left
ser.write(driveCom)
else:
driveCom = "e" #turn right
GPIO.output(Status5, GPIO.HIGH)
ser.write(driveCom)
elif (lowestFlame == ScaledFlame2 and driveCom != "G"):
driveCom = "G"
ser.write(driveCom)
#print str(driveCom)
centered = 1 #flame centered
elif (lowestFlame == ScaledFlame2 and driveCom == "G"):
centered = 1
return centered # centered?
GPIO.cleanup()
def main():
while True:
#read returns values
value = ADC.read_raw("P9_40")
print(value)
# Turns LED on when value is greater than 2.5
if value > 1600:
onLED("P8_14")
else:
offLED("P8_14")
def check_threshold(self, pin):
"""
Checks if an input has hit under threshold voltage (ie. it is toggled on when
voltage is low enough
Input: analog input pin to check
Output: true if input has hit threshold voltage
"""
measured = []
# read 3 times to average out noise in pin reading
for i in range(3):
measured.append(ADC.read_raw(pin))
return max(measured) <= THRESHOLD
def task(self):
ADC.setup()
while not self.stop.isSet():
value = ADC.read_raw(self.pin)
now = datetime.datetime.now()
print self.protocol + "%f : %d : %d : %d " % (
value, now.minute, now.second, now.microsecond)
val = str(value) + " : " + str(now.minute) + " : " + str(
now.second) + " : " + str(now.microsecond) + "\n"
global_module.wifi_namespace_reference.on_send(val)
if self.isLogging:
self.log_file.write(val)
sleep(1)
def main():
last= {} # remember sensor values in a dict
while True:
for sensor in analog_sensors:
val= int(ADC.read_raw(sensor)) # 0-1799
if last.get(sensor, None)!=val:
sendOSC("/ana", sensor, val)
last[sensor]= val # store sensor value
for sensor in digital_sensors:
val= GPIO.input(sensor) # 0-1
if last.get(sensor, None)!=val:
sendOSC("/dig", sensor, val)
last[sensor]= val # store sensor value
time.sleep(update_rate)
def task(self):
print "offline log started"
ADC.setup()
while not self.stop.isSet():
print type(self.log_file)
value = ADC.read_raw(self.pin)
now = datetime.datetime.now()
print self.protocol + "%f : %d : %d : %d " % (
value, now.minute, now.second, now.microsecond)
val = str(value) + " : " + str(now.minute) + " : " + str(
now.second) + " : " + str(now.microsecond) + "\n"
if self.isLogging:
self.log_file.write(val)
sleep(1)
def main(): ## Defined test condition
# a= n.linspace(0,10,1000)
i=0
y=[0]
while True:
v= ADC.read_raw("P9_40")
sleep(.1)
y.append(v)
#print(y)
winsize=5
avg=boxcar(y,winsize,i)
i=i+1
print(avg)
def run(self):
self.t0 = time.time()
while base.RUN_FLAG:
global ENC_IND
global ENC_TIME
global ENC_VAL
for side in range(0, 2):
ENC_TIME[side][ENC_IND[side]] = time.time() - self.t0
ADC_LOCK.acquire()
ENC_VAL[side][ENC_IND[side]] = ADC.read_raw(self.encPin[side])
time.sleep(ADCTIME)
ADC_LOCK.release()
ENC_IND[side] = (ENC_IND[side] + 1) % ENC_BUF_SIZE
def run(self):
self.t0 = time.time()
while base.RUN_FLAG:
global ENC_IND
global ENC_TIME
global ENC_VAL
for side in range(0, 2):
ENC_TIME[side][ENC_IND[side]] = time.time() - self.t0
ADC_LOCK.acquire()
ENC_VAL[side][ENC_IND[side]] = ADC.read_raw(self.encPin[side])
time.sleep(ADCTIME)
ADC_LOCK.release()
ENC_IND[side] = (ENC_IND[side] + 1) % ENC_BUF_SIZE
def checkDarkBrightChange(self):
value = ADC.read("P9_40")
value = ADC.read_raw("P9_40")
if value < 80:
self.state = 0
else:
self.state = 1
print value
if self.old_state != self.state:
strings = (
'state has changed %s to %s ' %
(self.str_state[self.old_state], self.str_state[self.state]))
print('state has changed %s to %s ' %
(self.str_state[self.old_state], self.str_state[self.state]))
writeDataToFile('light_state.txt', strings)
self.old_state = self.state
def read_pressure(pin_name): """Reads the pressure sensor analog input and converts to a pressure differential Args: pin_name: BeagleBone Analog Pin to read from Returns: __diff: The calculated pressure difference [psi] """ #Analog read voltage of the pressure differential sensor __diff = ADC.read_raw(pin_name) #TODO: Map voltage value to pressure differential return __diff
def main():
last= {} # remember sensor values in a dict
while True:
for sensor in analog_sensors:
val= int(ADC.read_raw(sensor)) # 0-1799
if last.get(sensor, None)!=val:
sendOSC("/ana", sensor, val)
last[sensor]= val # store sensor value
for sensor in digital_sensors:
val= GPIO.input(sensor) # 0-1
if last.get(sensor, None)!=val:
sendOSC("/dig", sensor, val)
last[sensor]= val # store sensor value
if sensor=="P9_23" and val==1: # a bit of a hack
os.system("sudo halt") # turn off the bbb
exit() # exit this python program
time.sleep(update_rate)
def calib(pin,led):
print("Please hold while device calibrates")
GPIO.output(led,GPIO.HIGH) # Turns on calibration LED
sampsize= 10000
calibtime= 2 # seconds
vol= n.zeros(sampsize)
for i in xrange(0,sampsize-1): #for loop that defines indices for array
vol[i]=(abs(ADC.read_raw(pin))) ## read in the resulting voltage response
sleep(calibtime/sampsize) ## calibration time per sample (seconds)
offset= n.mean(vol)## finds the mean of voltages collected during the sample time (equal to the offset)
GPIO.output(led,GPIO.LOW) # Turns calibration LED off
return offset
#if __name__== "__main__":
# main()
#
def show_analog_value(self):
"""Show the analog value on the screen:
- Read raw analog value
- Divide by 4 (remove two LSBs)
- Display value
- Return value
"""
# Read raw value from ADC
value = ADC.read_raw(self.analog_in)
# Divide value by 8
value = int(value // 8)
# Update display (must be an integer)
self.display.update(value)
# Return value
return value
def auto_pwm(self):
self._mylogger("BBBC auto_pwm called")
dutys=[]
for j in range(len(self.fan_definitions)):
if self.fan_definitions[j]['fan_pin'] != 'None' and self.fan_definitions[j]['manual_fan'] == False and self.fan_definitions[j]['has_temp'] == True :
value = BADC.read_raw(self.fan_definitions[j]['temp_pin'])*1.0/4
temp = round(self._cal_temp(value, self.fan_definitions[j]['temp_type']),1)
if self._settings.get_float(['MinTemp']) < temp and temp <= self._settings.get_float(['MaxTemp']):
duty = int(self._settings.get_float(['C_factor']) * temp)
if duty > 100 : duty = 100
dutys.append([j,duty])
BPWM.set_duty_cycle(self.fan_definitions[j]['fan_pin'], duty)
elif temp <= self._settings.get_float(['MinTemp']):
dutys.append([j,0])
BPWM.set_duty_cycle(self.fan_definitions[j]['fan_pin'], 0)
elif temp > self._settings.get_float(['MaxTemp']):
dutys.append([j,100])
BPWM.set_duty_cycle(self.fan_definitions[j]['fan_pin'], 100)
# else: dutys.append([j,self.fan_definitions[j]['PWM']])
#self._mylogger("Here my dutys %s" % dutys, forceinfo=True)
self._plugin_manager.send_plugin_message(self._identifier, dict(msg="dutyupdate", field2=dutys))
def sample(self):
t = time.time() - self.t0
self.val = ADC.read_raw(self.pin)
print "Pin " + self.pin + ": " + str(self.val)
cogPrev = self.cog
if self.val >= self.threshold:
self.cog = 1
else:
self.cog = 0
if cogPrev == 0 and self.cog == 1:
# Tick
self.tick = 1
self.pos = self.pos + self.dir
if self.tPrev != -1:
self.vel = self.dir * (t - self.t)**(-1.0) / (self.tickPerRev)
self.tPrev = self.t
self.t = t
else:
# No Tick
self.tick = 0
def show_analog_value(self):
"""Show the analog value on the screen:
- Read raw analog value
- Divide by 4 (remove two LSBs)
- Display value
- Return value
"""
# Read raw value from ADC
value = ADC.read_raw(self.analog_in)
# Divide value by 8
value = int(value // 8)
# Update display (must be an integer)
self.display.update(value)
# Play the sound of the value
if self.sound is not None:
self.sound.play_notes(Note(value, 0.2))
# Return value
return value
def function():
#thread function
global state
global outputs
record = []
left = ADC.read_raw(left_sensor)
right= ADC.read_raw(right_sensor)
print("Phase Search mode")
for i in range(0,40):
left = ADC.read_raw(left_sensor)
right= ADC.read_raw(right_sensor)
record.append(left+right)
stepRight()
time.sleep(0.1)
lowest = record[0]
index =0
for i in range(1,len(record)):
if(lowest>record[i]):
index = i
lowest = record[i]
print("Hunting Dog Mode")
print "Moving back by {} degrees".format((len(record)-index)*9)
for i in range(0,len(record)-index):
stepLeft()
time.sleep(0.1)
print "Real Time Tracking Mode"
time.sleep(1)
while running:
left = ADC.read_raw(left_sensor)
right= ADC.read_raw(right_sensor)
time.sleep(0.1)
if(left<right):
stepLeft()
else:
stepRight()
for out in outputs:
GPIO.output(out,GPIO.LOW)
import Adafruit_BBIO.ADC as ADC
ADC.setup()
#read returns values 0-1.0
value = ADC.read("P9_40")
#read_raw returns non-normalized value
value = ADC.read_raw("P9_40")
def print_temp(): # === Display Results ===
print '<i> ADC = ',ADC.read(ADC_PIN),' raw = ', ADC.read_raw(ADC_PIN)
print 'T(C)= ', temp_C , ' T(F)= ', temp_F, '\n'
def read_raw(self):
return ADC.read_raw(self.pin)
raw_input("Press enter to activate")
GPIO.output("P8_8", GPIO.HIGH)
GPIO.output("P8_9", GPIO.LOW)
raw_input("Press enter to change direction")
GPIO.output("P8_8", GPIO.LOW)
GPIO.output("P8_9", GPIO.HIGH)
raw_input("Press enter to continue")
PWM.stop("P8_13")
print "Test Motor 4 - Left Front"
PWM.set_duty_cycle("P8_19", 50)
raw_input("Press enter to activate")
GPIO.output("P8_10", GPIO.HIGH)
GPIO.output("P8_11", GPIO.LOW)
raw_input("Press enter to change direction")
GPIO.output("P8_10", GPIO.LOW)
GPIO.output("P8_11", GPIO.HIGH)
raw_input("Press enter to continue")
PWM.stop("P8_19")
GPIO.cleanup()
PWM.cleanup()
# Setup the ADC
ADC.setup()
print "Test IR Sensor"
i = 0
while i <= 0:
print ("Raw ", ADC.read_raw("P9_39"))
i = raw_input("Press 0 to re-read 1 to end")
import Adafruit_BBIO.ADC as ADC
import time
ADC.setup()
f = open("test1.dat", "w")
f.write("The Voltage Monitor")
start = time.time()
while True:
try:
voltage = ADC.read_raw("P9_40")
elapsed_time = time.time() - start
f.write(str(voltage))
f.write(str(elapsed_time))
f.write('\n')
print voltage
except KeyboardInterrupt:
f.close()
break
def run(self):
global RUN_FLAG
self.t0 = time.time()
while RUN_FLAG:
global ENC_IND
global ENC_TIME
global ENC_VAL
for side in range(0, 2):
ENC_TIME[side][ENC_IND[side]] = time.time() - self.t0
ADC_LOCK.acquire()
ENC_VAL[side][ENC_IND[side]] = ADC.read_raw(self.encPin[side])
time.sleep(ADCTIME)
ADC_LOCK.release()
class QuickBot_zxy():
"""The QuickBot_zxy Class"""
# === Class Properties ===
# Parameters
sampleTime = 20.0 / 1000.0
# Pins
ledPin = 'USR1'
# Motor Pins -- (LEFT, RIGHT)
dir1Pin = ('P8_14', 'P8_12')
dir2Pin = ('P8_16', 'P8_10')
pwmPin = ('P9_16', 'P9_14')
# ADC Pins
irPin = ('P9_38', 'P9_40', 'P9_36', 'P9_35', 'P9_33')
encoderPin = ('P9_39', 'P9_37')
# Encoder counting parameter and variables
ticksPerTurn = 16 # Number of ticks on encoder disc
encWinSize = 2**5 # Should be power of 2
minPWMThreshold = [45, 45] # Threshold on the minimum value to turn wheel
encTPrev = [0.0, 0.0]
encThreshold = [0.0, 0.0]
encTickState = [0, 0]
encTickStateVec = np.zeros((2, encWinSize))
# Constraints
pwmLimits = [-100, 100] # [min, max]
# State PWM -- (LEFT, RIGHT)
pwm = [0, 0]
# State IR
irVal = [0.0, 0.0, 0.0, 0.0, 0.0]
ithIR = 0
# State Encoder
encTime = [0.0, 0.0] # Last time encoders were read
encPos = [0.0, 0.0] # Last encoder tick position
encVel = [0.0, 0.0] # Last encoder tick velocity
# Encoder counting parameters
encCnt = 0 # Count of number times encoders have been read
encSumN = [0, 0] # Sum of total encoder samples
encBufInd0 = [0, 0] # Index of beginning of new samples in buffer
encBufInd1 = [0, 0] # Index of end of new samples in buffer
encTimeWin = np.zeros((2, encWinSize)) # Moving window of encoder sample times
encValWin = np.zeros((2, encWinSize)) # Moving window of encoder raw sample values
encPWMWin = np.zeros((2, encWinSize)) # Moving window corresponding PWM input values
encTau = [0.0, 0.0] # Average sampling time of encoders
## Stats of encoder values while input = 0 and vel = 0
encZeroCntMin = 2**4 # Min number of recorded values to start calculating stats
encZeroMean = [0.0, 0.0]
encZeroVar = [0.0, 0.0]
encZeroCnt = [0, 0]
encHighCnt = [0, 0]
encLowCnt = [0, 0]
encLowCntMin = 2
# Variables
ledFlag = True
cmdBuffer = ''
# UDP
baseIP = '192.168.7.1'
robotIP = '192.168.7.2'
port = 5005
robotSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
robotSocket.setblocking(False)
# === Class Methods ===
# Constructor
def __init__(self, baseIP, robotIP):
# Initialize GPIO pins
GPIO.setup(self.dir1Pin[LEFT], GPIO.OUT)
GPIO.setup(self.dir2Pin[LEFT], GPIO.OUT)
GPIO.setup(self.dir1Pin[RIGHT], GPIO.OUT)
GPIO.setup(self.dir2Pin[RIGHT], GPIO.OUT)
GPIO.setup(self.ledPin, GPIO.OUT)
# Initialize PWM pins: PWM.start(channel, duty, freq=2000, polarity=0)
PWM.start(self.pwmPin[LEFT], 0)
PWM.start(self.pwmPin[RIGHT], 0)
# Set motor speed to 0
self.setPWM([0, 0])
# Initialize ADC
ADC.setup()
self.encoderRead = encoderRead(self.encoderPin)
# Set IP addresses
self.baseIP = baseIP
self.robotIP = robotIP
self.robotSocket.bind((self.robotIP, self.port))
if DEBUG:
## Stats of encoder values while moving -- high, low, and all tick state
self.encHighLowCntMin = 2**5 # Min number of recorded values to start calculating stats
self.encHighMean = [0.0, 0.0]
self.encHighVar = [0.0, 0.0]
self.encHighTotalCnt = [0, 0]
self.encLowMean = [0.0, 0.0]
self.encLowVar = [0.0, 0.0]
self.encLowTotalCnt = [0, 0]
self.encNonZeroCntMin = 2**5
self.encNonZeroMean = [0.0, 0.0]
self.encNonZeroVar = [0.0, 0.0]
self.encNonZeroCnt = [0, 0]
# Record variables
self.encRecSize = 2**13
self.encRecInd = [0, 0]
self.encTimeRec = np.zeros((2, self.encRecSize))
self.encValRec = np.zeros((2, self.encRecSize))
self.encPWMRec = np.zeros((2, self.encRecSize))
self.encNNewRec = np.zeros((2, self.encRecSize))
self.encPosRec = np.zeros((2, self.encRecSize))
self.encVelRec = np.zeros((2, self.encRecSize))
self.encTickStateRec = np.zeros((2, self.encRecSize))
self.encThresholdRec = np.zeros((2, self.encRecSize))
# Getters and Setters
def setPWM(self, pwm):
# [leftSpeed, rightSpeed]: 0 is off, caps at min and max values
self.pwm[LEFT] = min(
max(pwm[LEFT], self.pwmLimits[MIN]), self.pwmLimits[MAX])
self.pwm[RIGHT] = min(
max(pwm[RIGHT], self.pwmLimits[MIN]), self.pwmLimits[MAX])
# Left motor
if self.pwm[LEFT] > 0:
GPIO.output(self.dir1Pin[LEFT], GPIO.LOW)
GPIO.output(self.dir2Pin[LEFT], GPIO.HIGH)
PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT]))
elif self.pwm[LEFT] < 0:
GPIO.output(self.dir1Pin[LEFT], GPIO.HIGH)
GPIO.output(self.dir2Pin[LEFT], GPIO.LOW)
PWM.set_duty_cycle(self.pwmPin[LEFT], abs(self.pwm[LEFT]))
else:
GPIO.output(self.dir1Pin[LEFT], GPIO.LOW)
GPIO.output(self.dir2Pin[LEFT], GPIO.LOW)
PWM.set_duty_cycle(self.pwmPin[LEFT], 0)
# Right motor
if self.pwm[RIGHT] > 0:
GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW)
GPIO.output(self.dir2Pin[RIGHT], GPIO.HIGH)
PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT]))
elif self.pwm[RIGHT] < 0:
GPIO.output(self.dir1Pin[RIGHT], GPIO.HIGH)
GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW)
PWM.set_duty_cycle(self.pwmPin[RIGHT], abs(self.pwm[RIGHT]))
else:
GPIO.output(self.dir1Pin[RIGHT], GPIO.LOW)
GPIO.output(self.dir2Pin[RIGHT], GPIO.LOW)
PWM.set_duty_cycle(self.pwmPin[RIGHT], 0)
# Methods
def run(self):
global RUN_FLAG
self.encoderRead.start()
try:
while RUN_FLAG is True:
self.update()
# Flash BBB LED
if self.ledFlag is True:
self.ledFlag = False
GPIO.output(self.ledPin, GPIO.HIGH)
else:
self.ledFlag = True
GPIO.output(self.ledPin, GPIO.LOW)
time.sleep(self.sampleTime)
except:
RUN_FLAG_LOCK.acquire()
RUN_FLAG = False
RUN_FLAG_LOCK.release()
raise
self.cleanup()
return
def cleanup(self):
sys.stdout.write("Shutting down...")
self.setPWM([0, 0])
self.robotSocket.close()
GPIO.cleanup()
PWM.cleanup()
if DEBUG:
# tictocPrint()
self.writeBuffersToFile()
sys.stdout.write("Done\n")
def update(self):
self.readIRValues()
self.readEncoderValues()
self.parseCmdBuffer()
def parseCmdBuffer(self):
global RUN_FLAG
try:
line = self.robotSocket.recv(1024)
except socket.error as msg:
return
self.cmdBuffer += line
bufferPattern = r'\$[^\$\*]*?\*' # String contained within $ and * symbols with no $ or * symbols in it
bufferRegex = re.compile(bufferPattern)
bufferResult = bufferRegex.search(self.cmdBuffer)
if bufferResult:
msg = bufferResult.group()
print msg
self.cmdBuffer = ''
msgPattern = r'\$(?P<CMD>[A-Z]{3,})(?P<SET>=?)(?P<QUERY>\??)(?(2)(?P<ARGS>.*)).*\*'
msgRegex = re.compile(msgPattern)
msgResult = msgRegex.search(msg)
if msgResult.group('CMD') == 'CHECK':
self.robotSocket.sendto('Hello from QuickBot\n',(self.baseIP, self.port))
elif msgResult.group('CMD') == 'PWM':
if msgResult.group('QUERY'):
self.robotSocket.sendto(str(self.pwm) + '\n',(self.baseIP, self.port))
elif msgResult.group('SET') and msgResult.group('ARGS'):
args = msgResult.group('ARGS')
pwmArgPattern = r'(?P<LEFT>[-]?\d+),(?P<RIGHT>[-]?\d+)'
pwmRegex = re.compile(pwmArgPattern)
pwmResult = pwmRegex.match(args)
if pwmResult:
pwm = [int(pwmRegex.match(args).group('LEFT')),
int(pwmRegex.match(args).group('RIGHT'))]
self.setPWM(pwm)
elif msgResult.group('CMD') == 'IRVAL':
if msgResult.group('QUERY'):
reply = '[' + ', '.join(map(str, self.irVal)) + ']'
print 'Sending: ' + reply
self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port))
elif msgResult.group('CMD') == 'ENVAL':
if msgResult.group('QUERY'):
reply = '[' + ', '.join(map(str, self.encPos)) + ']'
print 'Sending: ' + reply
self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port))
elif msgResult.group('CMD') == 'ENVEL':
if msgResult.group('QUERY'):
reply = '[' + ', '.join(map(str, self.encVel)) + ']'
print 'Sending: ' + reply
self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port))
elif msgResult.group('CMD') == 'RESET':
self.encPos[LEFT] = 0.0
self.encPos[RIGHT] = 0.0
print 'Encoder values reset to [' + ', '.join(map(str, self.encVel)) + ']'
elif msgResult.group('CMD') == 'UPDATE':
if msgResult.group('SET') and msgResult.group('ARGS'):
args = msgResult.group('ARGS')
pwmArgPattern = r'(?P<LEFT>[-]?\d+),(?P<RIGHT>[-]?\d+)'
pwmRegex = re.compile(pwmArgPattern)
pwmResult = pwmRegex.match(args)
if pwmResult:
pwm = [int(pwmRegex.match(args).group('LEFT')),
int(pwmRegex.match(args).group('RIGHT'))]
self.setPWM(pwm)
reply = '[' + ', '.join(map(str, self.encPos)) + ', ' \
+ ', '.join(map(str, self.encVel)) + ']'
print 'Sending: ' + reply
self.robotSocket.sendto(reply + '\n', (self.baseIP, self.port))
elif msgResult.group('CMD') == 'END':
RUN_FLAG_LOCK.acquire()
RUN_FLAG = False
RUN_FLAG_LOCK.release()
def readIRValues(self):
prevVal = self.irVal[self.ithIR]
ADC_LOCK.acquire()
self.irVal[self.ithIR] = ADC.read_raw(self.irPin[self.ithIR])
time.sleep(ADCTIME)
ADC_LOCK.release()
if self.irVal[self.ithIR] >= 1100:
self.irVal[self.ithIR] = prevVal
self.ithIR = ((self.ithIR+1) % 5)
def readEncoderValues(self):
if DEBUG and (self.encCnt % 10) == 0:
print "------------------------------------"
print "EncPos: " + str(self.encPos)
print "EncVel: " + str(self.encVel)
print "***"
print "Threshold: " + str(self.encThreshold)
print "***"
print "Zero Cnt: " + str(self.encZeroCnt)
print "Zero Mean: " + str(self.encZeroMean)
print "Zero Var: " + str(self.encZeroVar)
print "***"
print "NonZero Cnt: " + str(self.encNonZeroCnt)
print "NonZero Mean: " + str(self.encNonZeroMean)
print "NonZero Var: " + str(self.encNonZeroVar)
print "***"
print "High Cnt: " + str(self.encHighTotalCnt)
print "High Mean: " + str(self.encHighMean)
print "High Var: " + str(self.encHighVar)
print "***"
print "Low Cnt: " + str(self.encLowTotalCnt)
print "Low Mean: " + str(self.encLowMean)
print "Low Var: " + str(self.encLowVar)
self.encCnt = self.encCnt + 1
# Fill window
for side in range(0, 2):
self.encTime[side] = self.encTimeWin[side][-1]
self.encBufInd0[side] = self.encBufInd1[side]
self.encBufInd1[side] = ENC_IND[side]
ind0 = self.encBufInd0[side] # starting index
ind1 = self.encBufInd1[side] # ending index (this element is not included until the next update)
if ind0 < ind1:
N = ind1 - ind0 # number of new elements
self.encSumN[side] = self.encSumN[side] + N
self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N)
self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:ind1]
self.encValWin[side] = np.roll(self.encValWin[side], -N)
self.encValWin[side, -N:] = ENC_VAL[side][ind0:ind1]
self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N)
self.encPWMWin[side, -N:] = [self.pwm[side]]*N
elif ind0 > ind1:
N = ENC_BUF_SIZE - ind0 + ind1 # number of new elements
self.encSumN[side] = self.encSumN[side] + N
self.encTimeWin[side] = np.roll(self.encTimeWin[side], -N)
self.encValWin[side] = np.roll(self.encValWin[side], -N)
self.encPWMWin[side] = np.roll(self.encPWMWin[side], -N)
self.encPWMWin[side, -N:] = [self.pwm[side]]*N
if ind1 == 0:
self.encTimeWin[side, -N:] = ENC_TIME[side][ind0:]
self.encValWin[side, -N:] = ENC_VAL[side][ind0:]
else:
self.encTimeWin[side, -N:-ind1] = ENC_TIME[side][ind0:]
self.encValWin[side, -N:-ind1] = ENC_VAL[side][ind0:]
self.encTimeWin[side, -ind1:] = ENC_TIME[side][0:ind1]
self.encValWin[side, -ind1:] = ENC_VAL[side][0:ind1]
if ind0 != ind1:
tauNew = self.encTimeWin[side,-1] - self.encTimeWin[side,-N]
self.encTau[side] = tauNew / self.encCnt + self.encTau[side] * (self.encCnt-1)/self.encCnt # Running average
if self.encSumN[side] > self.encWinSize:
self.countEncoderTicks(side)
# Fill records
if DEBUG:
ind = self.encRecInd[side]
if ind+N < self.encRecSize:
self.encTimeRec[side, ind:ind+N] = self.encTimeWin[side, -N:]
self.encValRec[side, ind:ind+N] = self.encValWin[side, -N:]
self.encPWMRec[side, ind:ind+N] = self.encPWMWin[side, -N:]
self.encNNewRec[side, ind:ind+N] = [N]*N
self.encPosRec[side, ind:ind+N] = [self.encPos[side]]*N
self.encVelRec[side, ind:ind+N] = [self.encVel[side]]*N
self.encTickStateRec[side, ind:ind+N] = self.encTickStateVec[side, -N:]
self.encThresholdRec[side, ind:ind+N] = [self.encThreshold[side]]*N
self.encRecInd[side] = ind+N
def countEncoderTicks(self, side):
# Set variables
t = self.encTimeWin[side] # Time vector of data (non-consistent sampling time)
tPrev = self.encTPrev[side] # Previous read time
pwm = self.encPWMWin[side] # Vector of PWM data
pwmPrev = pwm[-1] # Last PWM value that was applied
tickStatePrev = self.encTickState[side] # Last state of tick (high (1), low (-1), or unsure (0))
tickCnt = self.encPos[side] # Current tick count
tickVel = self.encVel[side] # Current tick velocity
encValWin = self.encValWin[side] # Encoder raw value buffer window
threshold = self.encThreshold[side] # Encoder value threshold
minPWMThreshold = self.minPWMThreshold[side] # Minimum PWM to move wheel
N = np.sum(t > tPrev) # Number of new updates
tickStateVec = np.roll(self.encTickStateVec[side], -N)
# Determine wheel direction
if tickVel != 0:
wheelDir = np.sign(tickVel)
else:
wheelDir = np.sign(pwmPrev)
# Count ticks and record tick state
indTuple = np.where(t == tPrev) # Index of previous sample in window
if len(indTuple[0] > 0):
ind = indTuple[0][0]
newInds = ind + np.arange(1, N+1) # Indices of new samples
for i in newInds:
if encValWin[i] > threshold: # High tick state
tickState = 1
self.encHighCnt[side] = self.encHighCnt[side] + 1
self.encLowCnt[side] = 0
if tickStatePrev == -1: # Increment tick count on rising edge
tickCnt = tickCnt + wheelDir
else: # Low tick state
tickState = -1
self.encLowCnt[side] = self.encLowCnt[side] + 1
self.encHighCnt[side] = 0
tickStatePrev = tickState
tickStateVec[i] = tickState
# Measure tick speed
diffTickStateVec = np.diff(tickStateVec) # Tick state transition differences
fallingTimes = t[np.hstack((False,diffTickStateVec == -2))] # Times when tick state goes from high to low
risingTimes = t[np.hstack((False,diffTickStateVec == 2))] # Times when tick state goes from low to high
fallingPeriods = np.diff(fallingTimes) # Period times between falling edges
risingPeriods = np.diff(risingTimes) # Period times between rising edges
tickPeriods = np.hstack((fallingPeriods, risingPeriods)) # All period times
if len(tickPeriods) == 0:
if all(pwm[newInds] < minPWMThreshold): # If all inputs are less than min set velocity to 0
tickVel = 0
else:
tickVel = wheelDir * 1/np.mean(tickPeriods) # Average signed tick frequency
# Estimate new mean values
newEncRaw = encValWin[newInds]
if pwmPrev == 0 and tickVel == 0:
x = newEncRaw
l = self.encZeroCnt[side]
mu = self.encZeroMean[side]
sigma2 = self.encZeroVar[side]
(muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2)
self.encZeroMean[side] = muPlus
self.encZeroVar[side] = sigma2Plus
self.encZeroCnt[side] = n
elif tickVel != 0:
if DEBUG:
x = newEncRaw
l = self.encNonZeroCnt[side]
mu = self.encNonZeroMean[side]
sigma2 = self.encNonZeroVar[side]
(muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2)
self.encNonZeroMean[side] = muPlus
self.encNonZeroVar[side] = sigma2Plus
self.encNonZeroCnt[side] = n
NHigh = np.sum(tickStateVec[newInds] == 1)
if NHigh != 0:
indHighTuple = np.where(tickStateVec[newInds] == 1)
x = newEncRaw[indHighTuple[0]]
l = self.encHighTotalCnt[side]
mu = self.encHighMean[side]
sigma2 = self.encHighVar[side]
(muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2)
self.encHighMean[side] = muPlus
self.encHighVar[side] = sigma2Plus
self.encHighTotalCnt[side] = n
NLow = np.sum(tickStateVec[newInds] == -1)
if NLow != 0:
indLowTuple = np.where(tickStateVec[newInds] == -1)
x = newEncRaw[indLowTuple[0]]
l = self.encLowTotalCnt[side]
mu = self.encLowMean[side]
sigma2 = self.encLowVar[side]
(muPlus, sigma2Plus, n) = recursiveMeanVar(x, l, mu, sigma2)
self.encLowMean[side] = muPlus
self.encLowVar[side] = sigma2Plus
self.encLowTotalCnt[side] = n
# Set threshold value
if self.encZeroCnt[side] > self.encZeroCntMin:
self.encThreshold[side] = self.encZeroMean[side] - 3*np.sqrt(self.encZeroVar[side])
# elif self.encNonZeroCnt[side] > self.encNonZeroCntMin:
# self.encThreshold[side] = self.encNonZeroMean[side]
# elif self.encHighTotalCnt[side] > self.encHighLowCntMin and self.encLowTotalCnt > self.encHighLowCntMin:
# mu1 = self.encHighMean[side]
# sigma1 = self.encHighVar[side]
# mu2 = self.encLowMean[side]
# sigma2 = self.encLowVar[side]
# alpha = (sigma1 * np.log(sigma1)) / (sigma2 * np.log(sigma1))
# A = (1 - alpha)
# B = -2 * (mu1 - alpha*mu2)
# C = mu1**2 - alpha * mu2**2
# x1 = (-B + np.sqrt(B**2 - 4*A*C)) / (2*A)
# x2 = (-B - np.sqrt(B**2 - 4*A*C)) / (2*A)
# if x1 < mu1 and x1 > mu2:
# self.encThreshold[side] = x1
# else:
# self.encThreshold[side] = x2
# Update variables
self.encPos[side] = tickCnt # New tick count
self.encVel[side] = tickVel # New tick velocity
self.encTickStateVec[side] = tickStateVec # New tick state vector
self.encTPrev[side] = t[-1] # New latest update time
def writeBuffersToFile(self):
matrix = map(list, zip(*[self.encTimeRec[LEFT], self.encValRec[LEFT], self.encPWMRec[LEFT], self.encNNewRec[LEFT], \
self.encTickStateRec[LEFT], self.encPosRec[LEFT], self.encVelRec[LEFT], self.encThresholdRec[LEFT], \
self.encTimeRec[RIGHT], self.encValRec[RIGHT], self.encPWMRec[RIGHT], self.encNNewRec[RIGHT], \
self.encTickStateRec[RIGHT], self.encPosRec[RIGHT], self.encVelRec[RIGHT], self.encThresholdRec[RIGHT]]))
s = [[str(e) for e in row] for row in matrix]
lens = [len(max(col, key=len)) for col in zip(*s)]
fmt = '\t'.join('{{:{}}}'.format(x) for x in lens)
table = [fmt.format(*row) for row in s]
f = open('output.txt', 'w')
f.write('\n'.join(table))
f.close()
print "Wrote buffer to output.txt"
def recursiveMeanVar(x, l, mu, sigma2):
"""
This function calculates a new mean and variance given
the current mean "mu", current variance "sigma2", current
update count "l", and new samples "x"
"""
m = len(x)
n = l + m
muPlus = l / n * mu + m / n * np.mean(x)
if n > 1:
sigma2Plus = 1/(n-1) * ((l-1)*sigma2 + (m-1)*np.var(x) + l*(mu - muPlus)**2 + m*(np.mean(x) - muPlus)**2)
else:
sigma2Plus = 0
return (muPlus, sigma2Plus, n)
currentDate += str(currentDateTime.date().year)
currentTime = str(currentDateTime.time().hour) + ":"
if currentDateTime.time().minute < 10:
currentTime += "0" + str(currentDateTime.time().minute)
else:
currentTime += str(currentDateTime.time().minute)
global Energy
f = open(logFile, 'a')
f.write(currentDate + ' ' + currentTime + ' ' + str(Energy / 3600) + ' W*hr\n')
f.close()
print currentDate + ' ' + currentTime + ' ' + str(Energy / 3600) + ' W*hr'
Energy = 0
stopFlag = Event()
pwrThread = powerThread(stopFlag)
pwrThread.start()
strThread = storeThread(stopFlag)
strThread.start()
ADC.setup()
while True:
Varray.append(((ADC.read_raw(Vpin[0]) - ADC.read_raw(Vpin[1])) * 0.000439453125 / Vdivide))
Iarray.append(((ADC.read_raw(Ipin[0]) - ADC.read_raw(Ipin[1])) * 0.000439453125 / shunt))
time.sleep(.0002)
lastdig1= -1
sc= OSC.OSCClient()
sc.connect(('127.0.0.1', 57120)) #send locally to sc
def sendOSC(name, val):
msg= OSC.OSCMessage()
msg.setAddress(name)
msg.append(val)
try:
sc.send(msg)
except:
1+1 # dummy
#print msg
while True:
val= int(ADC.read_raw("P9_39")) # 0-1799
if val!=lastadc0:
sendOSC("/adc0", val)
lastadc0= val
val= int(ADC.read_raw("P9_40")) # 0-1799
if val!=lastadc1:
sendOSC("/adc1", val)
lastadc1= val
val= GPIO.input("P9_41")
if val!=lastdig0:
sendOSC("/dig0", val)
lastdig0= val
val= GPIO.input("P9_42")
if val!=lastdig1:
sendOSC("/dig1", val)
lastdig1= val
