def start(self, speed, direction):
""" method to start a motor
Arguments:
speed : speed of the motor 0-100 (as percentage of max)
direction : CW or CCW, for clockwise or counterclockwise
"""
# Standby pin should go high to enable motion
GPIO.output(self.STBYpin, GPIO.HIGH)
# x01 and x02 have to be opposite to move, toggle to change direction
if direction in ('cw','CW','clockwise'):
GPIO.output(self.ControlPin1, GPIO.LOW)
GPIO.output(self.ControlPin2, GPIO.HIGH)
elif direction in ('ccw','CCW','counterclockwise'):
GPIO.output(self.ControlPin1, GPIO.HIGH)
GPIO.output(self.ControlPin2, GPIO.LOW)
else:
raise ValueError('Please enter CW or CCW for direction.')
# Start the motor
# PWM.start(channel, duty, freq=2000, polarity=0)
if 0 <= speed <= 100:
PWM.start(self.PWMpin, speed, self.PWMfreq)
else:
raise ValueError("Please enter speed between 0 and 100, \
representing a percentage of the maximum \
motor speed.")
# set the status attributes
self.isRunning = True
self.currentDirection = direction
self.currentSpeed = speed
def on_success(self, data):
if 'text' in data:
print data['text'].encode('utf-8')
print
GPIO.output(LED, GPIO.HIGH)
time.sleep(0.5)
GPIO.output(LED, GPIO.LOW)
def turn_on(self):
self._state['lastchange'] = str(datetime.datetime.today())
print 'vars pin on', self._pin,self._on
GPIO.output(self._pin, self._on)
log.info('Turning {pin} on...',pin=self._pin)
self._event_bus.publish_command(self.state)
def __init__(self):
pines = ["P8_8","P8_10","P8_12","P8_14","P8_16", "P8_18"]
for i in pines:
gpio.setup(i, gpio.OUT)
gpio.output(i, gpio.LOW)
print i, " inicializado"
self.reset(True)
def main():
GPIO.setup(LED_PIN, GPIO.OUT)
GPIO.output(LED_PIN, GPIO.LOW)
radio = rfm69.RFM69()
radio.initialize(FREQUENCY,NODEID,NETWORKID)
radio.readAllRegs();
if IS_RFM69HW:
radio.setHighPower(IS_RFM69HW) #uncomment only for RFM69HW!
radio.setEncryptionKey(ENCRYPTKEY)
print "Transmitting at {} Mhz...".format(433 if FREQUENCY==rfm69.RF69_433MHZ else (868 if FREQUENCY==rfm69.RF69_868MHZ else 915))
lastPeriod = 0
sendSize = 0
while True:
#check for any received packets
if (radio.receiveDone()):
print "[{}] {} [RX_RSSI: {}]".format(radio.SENDERID, radio.DATA, radio.RSSI)
if (radio.ACKRequested()):
radio.sendACK()
print " - ACK sent"
Blink(LED_PIN, .003)
currPeriod = radio._millis()/TRANSMITPERIOD
if (currPeriod != lastPeriod):
lastPeriod = currPeriod
s = "Sending[{}]: {} ".format(sendSize, payload[:sendSize])
if (radio.sendWithRetry(GATEWAYID, payload, sendSize, retries=RETRIES, retryWaitTime=ACK_TIME)):
s += "ok!"
else:
s += "nothing..."
print s
sendSize = (sendSize + 1) % 31
Blink(LED_PIN, .003)
def display_char(char, font=FONT):
try:
GPIO.output(DC, GPIO.HIGH)
writebytes(font[char]+[0])
except KeyError:
pass # Ignore undefined characters.
def selftest(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:
if debuglevel:
print "Self test ok"
failure=0
else:
if debuglevel:
print "Failed"
failure=1
PWM.stop("P8_19")
time.sleep(0.2)
PWM.cleanup()
GPIO.cleanup()
#Debug output
if debuglevel:
print baseline
print selftest
return {'failure':failure, 'baseline':baseline ,'selftest':selftest }
def displayDigit(segmentArr, display):
for i in range(0, len(segmentArr)):
pinId = display[0][i]
if segmentArr[i] == 1:
GPIO.output(pinId, GPIO.HIGH)
else:
GPIO.output(pinId, GPIO.LOW)
def selectDigit(digitArr, display):
for i in range(0, len(digitArr)):
comPinId = display[1][i]
if digitArr[i] == 1:
GPIO.output(comPinId, GPIO.LOW)
else:
GPIO.output(comPinId, GPIO.HIGH)
def tx_start(self):
if not self.ptt_pin:
return
GPIO.setup(self.ptt_pin, GPIO.OUT)
GPIO.output(self.ptt_pin, GPIO.HIGH)
time.sleep(self.ptt_high)
def set_state(self, state): self._On = state _state = 0 if state: _state = 1 if self.verbose: print str(self.ssr.name) + " digitalWrite: " + str(self.ssr.pin) + " " + str(_state) #save the state if self.ssr.state != _state: self.ssr.state = _state self.ssr.save() #reverse if needed if self.ssr.reverse_polarity and self.enabled: _state = not _state if wiringpi2_available: wiringpi2.digitalWrite(int(self.ssr.pin), _state) elif bbb_available: if _state: GPIO.output(self.ssr.pin, GPIO.HIGH) else: GPIO.output(self.ssr.pin, GPIO.LOW)
def initFluidMotor(self):
self.fluidLeft = "P8_16"
self.fluidRight = "P8_18"
GPIO.setup(self.fluidLeft, GPIO.OUT)
GPIO.setup(self.fluidRight, GPIO.OUT)
GPIO.output(self.fluidLeft, GPIO.LOW)
GPIO.output(self.fluidRight, GPIO.LOW)
def enable(parameters = None):
"""
Name: enable
Desc: This enables the Main Power for the board.
The board is powered on with AUX enabled.
Params: parameters (list)
None
"""
global EN
for key in PINS.keys():
GPIO.output(PINS[key], GPIO.LOW)
GPIO.output(PINS["MAINPW_EN"], GPIO.HIGH)
with open("EN", "wb") as ENFile:
ENFile.write("1")
EN = True
time.sleep(1)
aux([1])
i2c = Adafruit_I2C(address, busNum)
pll = si5338POST(i2c = i2c, regs = VCOREGS)
pll._init()
print "Main Power Enabled"
print "Note: When finished use 'disable' or 'quit'"
def hold():
GPIO.output(ledPin, GPIO.HIGH)
printer.printImage(Image.open('gfx/goodbye.png'), True)
printer.feed(3)
subprocess.call("sync")
subprocess.call(["shutdown", "-h", "now"])
GPIO.output(ledPin, GPIO.LOW)
def testSwitch():
t0 = time.time()
GPIO.output(O_TEST, GPIO.HIGH)
GPIO.output(O_TEST, GPIO.LOW)
t1 = time.time()
t2 = time.time()
print(t1-t0, t2-t1)
def screenInit():
#GPIO.output(SCE, GPIO.LOW)
time.sleep(0.050)
# Toggle RST low to reset.
GPIO.output(RST, GPIO.LOW)
time.sleep(0.100)
GPIO.output(RST, GPIO.HIGH)
def ButtonFunction(void):
global alertFlag
print "ButtonFunction"
if alertFlag == 0:
alertFlag = 1
alertTime = time.time()+60
GPIO.output(led,GPIO.LOW)
def moveUP(self):
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.upDIR, GPIO.HIGH)
status = 1
print "Moving motor UP"
time.sleep(1.5)
return status
def moveDOWN(self):
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.downDIR, GPIO.HIGH)
status = 0
print "Moving motor DOWN"
time.sleep(1.5)
return status
def init_LEDs():
PWM.start(RED, 100) # red
PWM.start(GREEN, 100) # green
PWM.start(BLUE, 100) # blue
GPIO.setup(DOORPORT, GPIO.OUT) # LockPort
GPIO.output(DOORPORT, GPIO.LOW) # Default low: locked
def set_beeper(self, value):
if value:
if gpio_available:
GPIO.output(config.gpio_beeper, GPIO.HIGH)
else:
if gpio_available:
GPIO.output(config.gpio_beeper, GPIO.LOW)
def moveDOWN(self, device):
PWM.start(self.pwmPIN, self.duty, self.freq, 0)
GPIO.output(self.downDIR, GPIO.HIGH)
timer = time.time()
print "Moving motor %r DOWN" %device
#time.sleep(1.5)
return 0, timer # status
def setDB1(self, state):
#self.mcp23017.writePin("B", 6, state)
if int(state) == 0:
GPIO.output("P8_31", GPIO.LOW)
# digitalWrite(GPIO0_10, LOW)
else:
GPIO.output("P8_31", GPIO.HIGH)
def GPIOlightWriter(num):
GPIO.setup("P8_12", GPIO.OUT)
if num == 1:
GPIO.output("P8_12", GPIO.HIGH)
else:
GPIO.output("P8_12", GPIO.LOW)
def setRS(self, state):
#self.mcp23017.writePin("A", 0, state)
if state == 0:
GPIO.output("P8_27", GPIO.LOW)
#digitalWrite(GPIO2_22, LOW)
else:
GPIO.output("P8_27", GPIO.HIGH)
def stop(self):
"""
stop: Stop the conveyor motor.
"""
logging.debug("conveyor: stopped")
GPIO.output(self.pin, GPIO.LOW)
def _measure_ultra(self, trigger, echo):
GPIO.output(trigger, True)
time.sleep(DECPULSETRIGGER)
GPIO.output(trigger, False)
# Wait for echo to go high (or timeout)
intcountdown = INTTIMEOUT
while (GPIO.input(echo) == 0 and intcountdown > 0):
intcountdown = intcountdown - 1
# If echo is high
if intcountdown > 0:
# Start timer and init timeout countdown
echostart = time.time()
intcountdown = INTTIMEOUT
# Wait for echo to go low (or timeout)
while (GPIO.input(echo) == 1 and intcountdown > 0):
intcountdown = intcountdown - 1
# Stop timer
echoend = time.time()
# Echo duration
echoduration = echoend - echostart
# Display distance
if intcountdown > 0:
intdistance = (echoduration*1000000)/58
#print "Distance = " + str(intdistance) + "cm"
return intdistance
def read_temperature():
GPIO.output("P9_15", GPIO.LOW)
res = spi.xfer2([0x50, 0x00, 0x00])
print res
temp = (res[1] << 5 | res[2] >> 3) / 16.0
print "temp: %f deg C" % temp
def start(self):
"""
start: Start the conveyor motor.
"""
logging.debug("conveyor: started")
GPIO.output(self.pin, GPIO.HIGH)
def lecture_son(signal):
for i in range(33984/Fe):
if signal[i]>0:
GPIO.output("P9_22", GPIO.HIGH)
else:
GPIO.output("P9_22", GPIO.LOW)
time.sleep(1/Fe)
def green_on():
try:
GPIO.output(green_pin, GPIO.HIGH)
except:
pass
def nav_th():
#activate the cutter
GPIO.output(CUTTER_PIN, GPIO.HIGH)
sleep(1)
GPIO.output(CUTTER_PIN, GPIO.LOW)
#wait, and then start navigating the thing!
sleep(2)
#initialize servos
servo_r = TGY6114MD.TGY6114MD_SERVO(SERVO_PIN_R)
servo_l = TGY6114MD.TGY6114MD_SERVO(SERVO_PIN_L)
#state and direction variables
state = STRAIGHT
#servo angle set to midde
r_angle = 1080
l_angle = 1080
#holds last 20 gps hits
left_flag = 0
right_flag = 0
for i in range(40):
lat_array.append(0)
long_array.append(0)
#print 'starting nav'
#dict['direction'] = direction
dict['state'] = state
while True:
if dict['gps_fix'] == 1:
#print "We have a fix!"
while True:
#GPS UPDATING
dict['state'] = state
if dict['agl_avg'] < 45:
state = LANDING
l_angle = 1080
r_angle = 1080
long_sub = dict['long']-DEST_LONG
lat_sub = dict['lat']-DEST_LAT
angle_to_dest = math.tan(lat_sub/long_sub)
if (state != LANDING)
#QUAD 1
if(lat_sub > 0 and long_sub > 0):
if(dict['track'] < angle_to_dest or dict['track'] > (angle_to_dest + 180)):
state = TURNLEFT
else:
state = TURNRIGHT
#QUAD 2
elif(lat_sub < 0 and long_sub > 0):
if(dict['track'] < (180 - angle_to_dest) or dict['track'] > (360 - angle_to_dest)):
state = TURNRIGHT
else:
state = TURNLEFT
#QUAD 3
elif(lat_sub < 0 and long_sub < 0):
if(dict['track'] < angle_to_dest or dict['track'] > (angle_to_dest + 180)):
state = TURNRIGHT
else:
state = TURNLEFT
#QUAD 4
elif(lat_sub > 0 and long_sub < 0):
if(dict['track'] < (180 - angle_to_dest) or dict['track'] > (360 - angle_to_dest)):
state = TURNLEFT
else:
state = TURNRIGHT
if state == TURNLEFT:
if left_flag == 0:
right_flag = 0
left_flag = 1
l_angle += 30
r_angle -= 30
end = time.time() + 2
if time.time() > end:
end = time.time() + 2
l_angle += 30
r_angle -= 30
#print "more left"
elif state == TURNRIGHT:
if right_flag == 0:
right_flag = 1
left_flag = 0
r_angle += 30
l_angle -= 30
end = time.time() + 2
if time.time() > end:
end = time.time() + 2
r_angle += 30
l_angle -= 30
#print "MORE RIGHT"
elif state == STRAIGHT:
r_angle = 1080
l_angle = 1080
elif state == LANDING:
if dict['agl'] < 15:
servo_l.set_angle(2160)
servo_r.set_angle(2160)
if r_angle > 1260:
r_angle = 1260
if l_angle > 1260:
l_angle = 1260
if r_angle < 1080:
r_angle = 1080
if l_angle < 1080:
l_angle = 1080
servo_l.set_angle(l_angle)
servo_r.set_angle(r_angle)
NORTH = 0
WEST = 1
SOUTH = 2
EAST = 3
NONE = 4
logging.basicConfig(filename=ERROR_LOG,level=logging.WARNING,)
#setup the gps and transmitter uart ports
UART.setup("UART1")
UART.setup("UART2")
#initialize the cutter pin, it is triggered on a high signal
GPIO.setup(CUTTER_PIN, GPIO.OUT)
GPIO.output(CUTTER_PIN, GPIO.LOW)
#Sorry Curtis
#I cant think of a more elegant way to add this flag
dict = {'time': 0, 'agl': 0, 'temp': 0, 'a_x': 0, 'a_y': 0, 'a_z': 0, 'g_x': 0, 'g_y': 0, 'g_z': 0, 'gps_fix': 0, 'lat': 0, 'long': 0, 'arm_cut': 0, 'start_cut': 0, 'xbee_errors': 0, 'm_x': 0, 'm_y': 0, 'm_z': 0, 'state': 0, 'track': 0, 'speed': 0, 'agl_avg': 0}
error_trace = {'error': ' '}
global gps_report
gps_report = 0
err_lock = allocate_lock()
subprocess.call("/home/osu_rocketry/gpsd_setup.sh", shell=True)
def updateLED(channel):
print("wet detected")
state = GPIO.input(channel)
GPIO.output(LED1, state)
#!/usr/bin/env python3
import time
import Adafruit_BBIO.GPIO as GPIO
button1 = "GP0_3" # GP0 for buttons
LED1 = "GREEN" #GP1 for leds
# Set the GPIO pins:
GPIO.setup(LED1, GPIO.OUT)
GPIO.setup(button1, GPIO.IN)
# Turn on LEDs to default state
GPIO.output(LED1, 0)
def updateLED(channel):
print("wet detected")
state = GPIO.input(channel)
GPIO.output(LED1, state)
print("Running...")
GPIO.add_event_detect(button1, GPIO.BOTH,
callback=updateLED) # RISING, FALLING or BOTH
try:
while True:
time.sleep(100)
def red_on():
try:
GPIO.output(red_pin, GPIO.HIGH)
except:
pass
def green_off():
try:
GPIO.output(green_pin, GPIO.LOW)
except:
pass
def yellow_on():
try:
GPIO.output(yellow_pin, GPIO.HIGH)
except:
pass
def my_write_handler(pin, value):
print('Current V{} value: {}'.format(pin, value))
GPIO.output(LED, int(value[0]))
def yellow_off():
try:
GPIO.output(yellow_pin, GPIO.LOW)
except:
pass
def displayled(curr):
if curr == 0:
GPIO.output(red0, GPIO.LOW)
GPIO.output(green0, GPIO.HIGH)
time.sleep(4)
GPIO.output(red0, GPIO.HIGH)
GPIO.output(green0, GPIO.LOW)
if curr == 1:
GPIO.output(red1, GPIO.LOW)
GPIO.output(green1, GPIO.HIGH)
time.sleep(4)
GPIO.output(red1, GPIO.HIGH)
GPIO.output(green1, GPIO.LOW)
if curr == 2:
GPIO.output(red2, GPIO.LOW)
GPIO.output(green2, GPIO.HIGH)
time.sleep(4)
GPIO.output(red2, GPIO.HIGH)
GPIO.output(green2, GPIO.LOW)
if curr == 3:
GPIO.output(red3, GPIO.LOW)
GPIO.output(green3, GPIO.HIGH)
time.sleep(4)
GPIO.output(red3, GPIO.HIGH)
GPIO.output(green3, GPIO.LOW)
def red_off():
try:
GPIO.output(red_pin, GPIO.LOW)
except:
pass
def listener():
# anonymous=False flag means that rospy will choose exaclty the
# given name for the 'listener' node. If two nodes with the same
# name are launched, the previous one is kicked off.
rospy.init_node('motor_listener_int', anonymous=False)
rospy.Subscriber('range_val', UInt16, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
try:
PWM.start(pin_motL, 0, FREQ, 0)
PWM.start(pin_motR, 0, FREQ, 0)
GPIO.setup(pin_in1, GPIO.OUT)
GPIO.setup(pin_in2, GPIO.OUT)
print('PWM outpunt enabled...')
GPIO.output(pin_in1, GPIO.HIGH)
GPIO.output(pin_in2, GPIO.LOW)
listener()
print('')
finally:
PWM.set_duty_cycle(pin_motL, 0)
PWM.set_duty_cycle(pin_motR, 0)
PWM.cleanup()
print('...Pin disabled.')
print('Byebye...')
def display(num):
if num == 0:
GPIO.output(a, GPIO.HIGH)
GPIO.output(b, GPIO.HIGH)
GPIO.output(c, GPIO.HIGH)
GPIO.output(d, GPIO.HIGH)
GPIO.output(e, GPIO.HIGH)
GPIO.output(f, GPIO.HIGH)
GPIO.output(g, GPIO.LOW)
if num == 1:
GPIO.output(a, GPIO.LOW)
GPIO.output(b, GPIO.HIGH)
GPIO.output(c, GPIO.HIGH)
GPIO.output(d, GPIO.LOW)
GPIO.output(e, GPIO.LOW)
GPIO.output(f, GPIO.LOW)
GPIO.output(g, GPIO.LOW)
if num == 2:
GPIO.output(a, GPIO.HIGH)
GPIO.output(b, GPIO.HIGH)
GPIO.output(c, GPIO.LOW)
GPIO.output(d, GPIO.HIGH)
GPIO.output(e, GPIO.HIGH)
GPIO.output(f, GPIO.LOW)
GPIO.output(g, GPIO.HIGH)
if num == 3:
GPIO.output(a, GPIO.HIGH)
GPIO.output(b, GPIO.HIGH)
GPIO.output(c, GPIO.HIGH)
GPIO.output(d, GPIO.HIGH)
GPIO.output(e, GPIO.LOW)
GPIO.output(f, GPIO.LOW)
GPIO.output(g, GPIO.HIGH)
import time
import Adafruit_BBIO.GPIO as GPIO
#UltraE="P9_25"
#UltraT="P9_27"
#UltraE="P9_15"
#UltraT="P9_13"
#UltraE="P9_16"
#UltraT="P9_11"
UltraE = "P8_16"
UltraT = "P8_15"
GPIO.setup(UltraE, GPIO.IN)
GPIO.setup(UltraT, GPIO.OUT)
GPIO.output(UltraT, GPIO.LOW)
def readSonar(sensorTrig, sensorEcho):
signaloff = 0
status = 1
while (signaloff == 0):
time.sleep(.01)
GPIO.output(sensorTrig, True)
time.sleep(0.00001)
GPIO.output(sensorTrig, False)
signalstart = time.time()
while GPIO.input(sensorEcho) == 0 and status == 1:
signaloff = time.time()
def movement():
global dest
global src
global curr
GPIO.output(red0, GPIO.HIGH)
GPIO.output(red1, GPIO.HIGH)
GPIO.output(red2, GPIO.HIGH)
GPIO.output(red3, GPIO.HIGH)
while 1:
if len(dest) > 0:
print "in if"
z = dest[0]
dest.pop(0)
if z < curr:
while (curr != z):
display(curr)
if curr in src or curr in dest:
src = remove_values_from_list(src, curr)
dest = remove_values_from_list(dest, curr)
displayled(curr)
curr -= 1
display(curr)
time.sleep(1)
displayled(curr)
elif z > curr:
while (curr != z):
display(curr)
if curr in src or curr in dest:
src = remove_values_from_list(src, curr)
dest = remove_values_from_list(dest, curr)
displayled(curr)
curr += 1
display(curr)
time.sleep(1)
displayled(curr)
elif len(src) > 0:
print "in else"
z = src[0]
src.pop(0)
if z < curr:
while (curr != z):
display(curr)
if curr in src or curr in dest:
src = remove_values_from_list(src, curr)
dest = remove_values_from_list(dest, curr)
displayled(curr)
curr -= 1
display(curr)
time.sleep(1)
displayled(curr)
elif z > curr:
print "in elif"
while (curr != z):
display(curr)
time.sleep(2)
if curr in src or curr in dest:
src = remove_values_from_list(src, curr)
dest = remove_values_from_list(dest, curr)
displayled(curr)
curr += 1
display(curr)
time.sleep(1)
displayled(curr)
def output(channel):
# print("channel =" + channel)
state = GPIO.input(channel) # get input value
GPIO.output(map[channel], state) # outputs input value
def blinkLED(channel):
GPIO.output(channel, GPIO.HIGH)
time.sleep(0.25)
GPIO.output(channel, GPIO.LOW)
def nav_th():
#activate the cutter
GPIO.output(CUTTER_PIN, GPIO.HIGH)
sleep(1)
GPIO.output(CUTTER_PIN, GPIO.LOW)
#wait, and then start navigating the thing!
sleep(2)
#initialize servos
servo_r = TGY6114MD.TGY6114MD_SERVO(SERVO_PIN_R)
servo_l = TGY6114MD.TGY6114MD_SERVO(SERVO_PIN_L)
#state and direction variables
state = ORIENT
#servo angle set to midde
r_angle = 1080
l_angle = 1080
turn_init = 0
#holds last 20 gps hits
lat_array = []
long_array = []
long_dir = NONE
lat_dir = NONE
go_lat = 0
go_long = 0
for i in range(40):
lat_array.append(0)
long_array.append(0)
#print 'starting nav'
#dict['direction'] = direction
dict['state'] = state
while True:
if dict['gps_fix'] == 1:
#print "We have a fix!"
while True:
#GPS UPDATING
dict['state'] = state
if dict['agl'] < 30:
state = LANDING
l_angle = 1080
r_angle = 1080
if lat_array[0] != dict['lat'] or long_array[0] != dict['long']:
sleep(2)
for i in range(39):
lat_array[i + 1] = lat_array[i]
long_array[i + 1] = long_array[i]
lat_array[0] = dict['lat']
long_array[0] = dict['long']
sum_lat = 0
sum_long = 0
sum_lat2 = 0
sum_long2 = 0
for i in range(20):
sum_lat += lat_array[i]
sum_long += long_array[i]
sum_lat2 += lat_array[i + 10]
sum_long2 += long_array[i + 10]
if lat_array[0] <= lat_array[10] and long_array[
0] <= long_array[10]:
lat_dir = SOUTH
long_dir = WEST
#print "southwest"
elif lat_array[0] >= lat_array[10] and long_array[
0] >= long_array[10]:
lat_dir = NORTH
long_dir = EAST
#print "northeast"
elif lat_array[0] > lat_array[10] and long_array[
0] < long_array[10]:
lat_dir = NORTH
long_dir = WEST
#print "northwest"
elif lat_array[0] < lat_array[10] and long_array[
0] > long_array[10]:
lat_dir = SOUTH
long_dir = EAST
#print "southeast"
if lat_array[0] > DEST_LAT:
go_lat = SOUTH
else:
go_lat = NORTH
if long_array[0] > DEST_LONG:
go_long = WEST
else:
go_long = EAST
if lat_array[39] != [0] and state == WAIT:
state = ORIENT
if state == ORIENT:
if go_lat == lat_dir and go_long == long_dir:
state = STRAIGHT
#print "Straight!"
elif go_lat != lat_dir and go_long != long_dir:
state = TURNAROUND
#print "Turn around"
elif (lat_dir == NORTH and long_dir == WEST
and go_lat == NORTH and go_long == EAST) or (
lat_dir == SOUTH and long_dir == WEST
and go_lat == NORTH and go_long == WEST) or (
lat_dir == SOUTH and long_dir == EAST
and go_lat == SOUTH and go_long
== WEST) or (lat_dir == NORTH
and long_dir == EAST
and go_lat == SOUTH
and go_long == EAST):
state = TURNRIGHT
#print "right"
cur_lat = lat_dir
cur_long = long_dir
else:
state = TURNLEFT
#print "left"
cur_long = long_dir
cur_lat = lat_dir
elif state == STRAIGHT:
r_angle = 1080
l_angle = 1080
if lat_dir != go_lat or long_dir != go_long:
state = ORIENT
elif state == TURNLEFT:
if turn_init == 0:
turn_init = 1
l_angle = 1170
r_angle = 1080
end = time.time() + 5
if time.time() > end:
end = time.time() + 5
if lat_dir != go_lat or long_dir != go_long:
l_angle += 15
#print "more left"
if (cur_lat != lat_dir or cur_long != long_dir) and (
lat_dir != go_lat and long_dir != go_long):
state = ORIENT
turn_init = 0
elif state == TURNRIGHT:
if turn_init == 0:
turn_init = 1
r_angle = 1170
l_angle = 1080
end = time.time() + 5
if time.time() > end:
end = time.time() + 5
if lat_dir != go_lat or long_dir != go_long:
r_angle += 15
#print "MORE RIGHT"
if (cur_lat != lat_dir or cur_long != long_dir) and (
lat_dir != go_lat and long_dir != go_long):
state = ORIENT
turn_init = 0
elif state == TURNAROUND:
if turn_init == 0:
turn_init = 1
r_angle = 1180
l_angle = 1080
end = time.time() + 5
if time.time() > end:
end = time.time() + 5
r_angle += 20
#print "turn around faster"
if lat_dir == go_lat or long_dir == go_long:
state = ORIENT
turn_init = 0
elif state == LANDING:
if dict['agl'] < 15:
servo_l.set_angle(2160)
servo_r.set_angle(2160)
if r_angle > 1260:
r_angle = 1260
if l_angle > 1260:
l_angle = 1260
if r_angle < 1080:
r_angle = 1080
if l_angle < 1080:
l_angle = 1080
servo_l.set_angle(l_angle)
servo_r.set_angle(r_angle)
#!/usr/bin/env python3
import Adafruit_BBIO.GPIO as GPIO
import time
LED_1 = "P9_11"
flag = 1
GPIO.setup(LED_1, GPIO.OUT)
try:
while True:
if (flag):
GPIO.output(LED_1, 1)
else:
GPIO.output(LED_1, 0)
flag = not flag
time.sleep(0.000001)
except KeyboardInterrupt:
GPIO.cleanup()
def setrx():
open("/sys/class/gpio/gpio" + str(UART2_TXIO) + "/value",
"w").write("%d" % 0)
open("/sys/class/gpio/gpio" + str(UART2_RXIO) + "/value",
"w").write("%d" % 1)
GPIO.output("P8_10", GPIO.HIGH)
#!/usr/bin/env python3
#Douglas Wise
import Adafruit_BBIO.GPIO as GPIO
import time
GP_in = "GP1_3" # input gpio
GP_out = "GP1_4" # output gpio
# set pins
GPIO.setup(GP_in, GPIO.IN)
GPIO.setup(GP_out, GPIO.OUT)
# initial out is off
GPIO.output(GP_out, 0)
# map input to output
map = {GP_in: GP_out}
# output function
def output(channel):
# print("channel =" + channel)
state = GPIO.input(channel) # get input value
GPIO.output(map[channel], state) # outputs input value
print("Running...")
# detects input calls output function
GPIO.add_event_detect(GP_in, GPIO.BOTH, callback=output)
def pwmStartport(self):
if __debug__: print
"pwm port started"
else:
PWM.start("P9_22", 30, self.PWM_const, 1)
QtTest.QTest.qWait(3000)
PWM.start("P8_13", 30, self.PWM_const, 1)
QtTest.QTest.qWait(3000)
PWM.start("P8_19", 30, self.PWM_const, 1)
QtTest.QTest.qWait(3000)
GPIO.setup("P8_12", GPIO.OUT)
QtTest.QTest.qWait(3000)
GPIO.setup("P8_14", GPIO.OUT)
QtTest.QTest.qWait(3000)
GPIO.output("P8_12", GPIO.HIGH) # HIGH/LOW or 1/0
QtTest.QTest.qWait(3000)
GPIO.output("P8_14", GPIO.LOW) # HIGH/LOW or 1/0
def pwmStopport(self):
if __debug__: print
"pwm port stop"
else:
PWM.stop("P9_22")
QtTest.QTest.qWait(3000)
PWM.stop("P8_13")
QtTest.QTest.qWait(3000)
PWM.stop("P8_19")
QtTest.QTest.qWait(3000)
PWM.cleanup()
def detect_distance():
'''
Take a distance reading from the ultrasonic sensor
:returns: Distance if object detected, -1 otherwise
'''
# Ensure proper function usage (can be removed/refactored later)
assert (_g_initialized), 'Distance sensor was not initialized'
distance = -1
obj_detected = True
# Execute sensor trigger (10us pulse)
GPIO.output(g_sensor.trigger_pin, GPIO.HIGH)
time.sleep(_g_TRIGGER_PULSE_S)
GPIO.output(g_sensor.trigger_pin, GPIO.LOW)
# Listen for echo
sense_start = time.time()
while GPIO.input(g_sensor.echo_pin) == 0:
sense_current = time.time()
elapsed = sense_current - sense_start
# Check for timeout
if elapsed > _g_ECHO_START_TIMEOUT_S:
obj_detected = False
break
if obj_detected:
# Start of echo pulse
echo_pulse_start = time.time()
sense_start = time.time()
while GPIO.input(g_sensor.echo_pin) == 1:
sense_current = time.time()
elapsed = sense_current - sense_start
# Check for timeout
if elapsed > _g_ECHO_END_TIMEOUT_S:
break
# Calculate echo pulse width
echo_pulse_end = time.time()
pulse_width = echo_pulse_end - echo_pulse_start
# Calculate the distance in centimeters (divide by 2 due to round trip)
distance = (pulse_width * _g_SOUND_SPEED) / 2
# TODO wait_for_edge function is better way to do this but
# it doesn't seem to be working and needs testing
# end_edge_detect = None
# start_edge_detect = GPIO.wait_for_edge(g_sensor.echo_pin,
# GPIO.RISING,
# _g_ECHO_START_TIMEOUT_MS)
# if start_edge_detect:
# # Record start time of echo (in seconds)
# echo_pulse_start = time.time()
# # Wait for end of echo pulse
# end_edge_detect = GPIO.wait_for_edge(g_sensor.echo_pin,
# GPIO.FALLING,
# _g_ECHO_END_TIMEOUT_MS)
# if end_edge_detect:
# # Calculate the time echo pulse spent high
# echo_pulse_end = time.time()
# pulse_width = echo_pulse_end - echo_pulse_start # Pulse duration
# # Calculate the distance in centimeters (divide by 2 due to rtt)
# distance = (pulse_width * _g_SOUND_SPEED) / 2
return distance
import Adafruit_BBIO.GPIO as GPIO
import CountingPRU
# Variable for timebase counting
global TimeBase
TimeBase = 1
# Variable for counting values [LNLS1, LNLS2, LNLS3, LNLS4, LNLS5, LNLS6, Bergoz1, Bergoz2]
global Counting
# Inhibit pins - Bergoz
Inhibit = {"A1": "P9_14", "B1": "P9_16", "A2": "P9_13", "B2": "P9_15"}
for pin in Inhibit:
GPIO.setup(Inhibit[pin], GPIO.OUT)
GPIO.output(Inhibit[pin], GPIO.LOW)
# Error Codes - bsmp
COMMAND_OK = 0xE0
ERROR_READ_ONLY = 0xE6
# Datetime string
def time_string():
return (datetime.datetime.now().strftime("%d/%m/%Y %H:%M:%S.%f")[:-4] +
" - ")
def includeChecksum(list_values):
counter = 0
i = 0
#!/usr/bin/python
import serial
import time
import Adafruit_BBIO.UART as UART
import Adafruit_BBIO.GPIO as GPIO
UART.setup("UART2")
GPIO.setup("P8_10", GPIO.OUT)
GPIO.output("P8_10", GPIO.LOW)
UART2_PORT = "/dev/ttyO2"
UART2_BAUD = 115200
UART2_TX = "spi0_d0"
UART2_RX = "spi0_sclk"
UART2_TXMUX = 1
UART2_RXMUX = 33
UART2_TXIO = 117
UART2_RXIO = 49
AX_ID = 2
AX_LENGTH = 3
AX_INSTRUCTION = 4
AX_PARAMETER = 5
AX_WRITE = 3
AX_READ = 2
open("/sys/class/gpio/unexport", "w").write("%d" % UART2_TXIO)
open("/sys/class/gpio/export", "w").write("%d" % UART2_TXIO)
open("/sys/class/gpio/gpio" + str(UART2_TXIO) + "/direction", "w").write("out")
#!/usr/bin/python
# -*- coding: utf-8 -*-
import Adafruit_BBIO.GPIO as GPIO
import converters
import threading
import time
# Pino de comando dos dois relês usados para habilitar/desabilitar os drivers
DRIVER_ENABLE_PIN = "P9_14"
# Inicializa o pino de comando dos relês, desabilitando em seguida os drivers
GPIO.setup(DRIVER_ENABLE_PIN, GPIO.OUT)
GPIO.output(DRIVER_ENABLE_PIN, GPIO.LOW)
class RF_MotorControllers_Driver():
def __init__(self):
self.ADC1 = converters.ADC()
self.ADC2 = converters.ADC2()
self.drvStsVal = 0
self.drvEnbl(1)
def data(self):
ADC1_values = [0] * 8
ADC2_values = [0] * 8
for i in range(10):
ADC1_readings = self.ADC1.read(range(8))
ADC2_readings = self.ADC2.read(range(8))
def run(self):
global TimeBase, Counting
while (True):
try:
# TCP/IP socket initialization
self.tcp = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.tcp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.tcp.bind(("", self.port))
self.tcp.listen(1)
sys.stdout.write(time_string() + "TCP/IP Server on port " +
str(self.port) + " started.\n")
sys.stdout.flush()
while (True):
sys.stdout.write(time_string() +
"Waiting for connection.\n")
sys.stdout.flush()
con, client_info = self.tcp.accept()
# New connection
sys.stdout.write(time_string() +
"Connection accepted from " +
client_info[0] + ":" +
str(client_info[1]) + ".\n")
sys.stdout.flush()
while (True):
# Get message
message = [ord(i) for i in con.recv(100)]
if (message):
if (verifyChecksum(message) == 0):
# Variable Read
if message[1] == 0x10:
# TimeBase
if message[4] == 0x00:
con.send(
sendVariable(
message[4], TimeBase, 2))
sys.stdout.write(time_string() +
"Read time base " +
str(TimeBase) + " \n")
sys.stdout.flush()
elif message[4] <= 0x08:
con.send(
sendVariable(
message[4],
Counting[message[4] - 1], 4))
sys.stdout.write(time_string() +
"Read counting " +
str(message[4] - 1) +
" \n")
sys.stdout.flush()
# Inhibit pins - 8 bits: X X X X B2 A2 B1 A1
elif message[4] == 0x09:
inh_value = 0
for i in range(4):
inh_value += GPIO.input(Inhibit[
Inhibit.keys()[i]]) * (2**i)
con.send(
sendVariable(
message[4], inh_value, 1))
sys.stdout.write(
time_string() +
"Read Inhibits values " +
bin(inh_value) + " \n")
sys.stdout.flush()
# Group Read
elif message[1] == 0x12:
if message[4] == 0x01:
con.send(
sendGroup(message[4], Counting,
len(Counting) * 4))
sys.stdout.write(time_string() +
"Read Group " +
str(message[4]) +
" \n")
sys.stdout.flush()
# Variable Write
elif message[1] == 0x20:
# TimeBase
if message[4] == 0x00:
TimeBase = message[5] * 256 + message[6]
con.send(sendMessage(COMMAND_OK))
sys.stdout.write(time_string() +
"Write time base " +
str(TimeBase) + " \n")
sys.stdout.flush()
# Counting channels
elif message[4] <= 0x08:
con.send(sendMessage(ERROR_READ_ONLY))
sys.stdout.write(time_string() +
"Write to counting " +
str(message[4]) +
" not permited.\n")
sys.stdout.flush()
# Inhibit pins - 8 bits: X X X X B2 A2 B1 A1
elif message[4] == 0x09:
for i in range(4):
GPIO.output(
Inhibit[Inhibit.keys()[i]],
bool(message[5] & (1 << i)))
con.send(sendMessage(COMMAND_OK))
sys.stdout.write(time_string() +
"Write Inhibits to " +
bin(message[5]
& 0x0f) + " \n")
sys.stdout.flush()
else:
sys.stdout.write(time_string() +
"Unknown message\n")
sys.stdout.flush()
continue
else:
# Disconnection
sys.stdout.write(time_string() + "Client " +
client_info[0] + ":" +
str(client_info[1]) +
" disconnected.\n")
sys.stdout.flush()
break
except Exception:
self.tcp.close()
sys.stdout.write(
time_string() +
"Connection problem. TCP/IP server was closed. Error:\n\n")
traceback.print_exc(file=sys.stdout)
sys.stdout.write("\n")
sys.stdout.flush()
time.sleep(5)
import Adafruit_BBIO.GPIO as GPIO
import time
GPIO.setup("P8_11",GPIO.IN) #Echo
GPIO.setup("P8_12",GPIO.OUT) #Trigger
GPIO.output("P8_12", GPIO.HIGH)
def distanceMeasurement(TRIG,ECHO):
pulseStart = pulseEnd = 0
GPIO.output(TRIG, True)
time.sleep(0.00001)
GPIO.output(TRIG, False)
while GPIO.input(ECHO) == 0:
pulseStart = pulseEnd = time.time()
while GPIO.input(ECHO) == 1:
pulseEnd = time.time()
pulseDuration = pulseEnd - pulseStart
SPEED_OF_SOUND_CM = 17150
distance = pulseDuration * SPEED_OF_SOUND_CM
distance = round(distance, 2)
return distance
#main Loop
try:
print "Starting Up...."
while True:
recoveredDistance = distanceMeasurement("P8_12","P8_11")
print "Distance1: ",recoveredDistance,"cm"
time.sleep(0.2)
except KeyboardInterrupt:
import Adafruit_BBIO.GPIO as GPIO
import Adafruit_BBIO.ADC as ADC
from time import sleep
# pinos = [16, 21, 22, 13, 12, 11]
GPIO.setup("P9_16", GPIO.OUT)
while True:
GPIO.output("P9_16", GPIO.HIGH)
sleep(1)
GPIO.output("P9_16", GPIO.LOW)
#sleep(1)
