def __init__(self,channel,sensor_name,topic_name,modo):
self.channel = channel
self.sensor_name = sensor_name
self.topic_name = topic_name
self.modo = modo
self.pub = rospy.Publisher(self.topic_name, DeviceInfo, queue_size=10)
self.msg = DeviceInfo()
self.modo = modo
if modo == 'ANALOG':
self.ch = VoltageInput()
elif modo == 'DIGITAL':
self.ch = DigitalInput()
def __init__(self):
# Name of the controller
self.name = "PAUL controller for Raspberry Pi"
# Current Speed of the robot
self.speed = 0
# Speed that the robot will travel up (note negative speed goes up)
self.up_speed = -70
#Speed that the robot will travel down (note negative speed goes down)
self.down_speed = 70
# Threshold reading on top distance sensors before triggering
self.top_threshold = 1.9
# Threshold reading on bottom distance sensors before triggering
self.bottom_threshold = 2.6
# Note: The following should only be set if distance sensors are not attached,
# they should be set to 0 if sensors are attached.
self.top_ds_reading = 0
self.bottom_ds_reading = 0
# Create an instance of the Motors class used in SDP
self.mc = Motors()
#Sensors
#define array of digital inputs to store readings from the bump sensors
self.bump_sensors = [DigitalInput() for i in range (0,6)]
#addressing the channels for the bump sensors
#0-2 are top sensors
#3-5 are bottom channels
for i in range(0,6):
self.bump_sensors[i].setChannel(i)
self.bump_sensors[i].openWaitForAttachment(5000)
#define array of analogue inputs to store readings from the ir sensors
self.ir_sensors = [VoltageInput() for i in range (0,6)]
#addressing the channels for the ir sensors
#0-2 are top sensors
#3-5 are bottom channels
for i in range(0,6):
self.ir_sensors[i].setChannel(i)
#this line will basically call the event handler when the readings change
self.ir_sensors[i].openWaitForAttachment(5000)
#Light
# connect to pin 12(slot PWM)
PIN = 18
# For Grove - WS2813 RGB LED Strip Waterproof - 30 LED/m
# there is 30 RGB LEDs.
COUNT = 60
self.strip = GroveWS2813RgbStrip(PIN, COUNT)
def init_arm():
global stepper_r
stepper_r = Stepper()
stepper_r.setHubPort(0)
global stepper_p
stepper_p = Stepper()
stepper_p.setHubPort(1)
global limit_switch
limit_switch = DigitalInput()
limit_switch.setIsHubPortDevice(True)
limit_switch.setHubPort(2)
stepper_r.openWaitForAttachment(5000)
stepper_p.openWaitForAttachment(5000)
limit_switch.openWaitForAttachment(5000)
stepper_r.setCurrentLimit(2.0)
stepper_r.setRescaleFactor((1.0 / 16.0) * (STEP_ANGLE / R_JOINT_RATIO))
stepper_p.setRescaleFactor((1.0 / 16.0) * (STEP_ANGLE / DEG_PER_IN_P))
stepper_r.setAcceleration(ACC)
stepper_p.setAcceleration(ACC)
stepper_r.setVelocityLimit(MAX_VEL)
stepper_p.setVelocityLimit(MAX_VEL)
stepper_r.setEngaged(True)
stepper_p.setEngaged(True)
# Drive P joint until limit switch is hit to find 0 coordinate
calib_pos = 0
offset_pos = None
while calib_pos < 2 * JOINT_LENGTH_P and offset_pos is None:
calib_pos += 0.25
stepper_p.setTargetPosition(-calib_pos)
while stepper_p.getIsMoving():
if limit_switch.getState():
offset_pos = stepper_p.getPosition()
break
print(offset_pos)
stepper_p.setTargetPosition(offset_pos + DIST_SWITCH_TO_CENTER)
while stepper_p.getIsMoving():
print('target: ' + str(offset_pos + DIST_SWITCH_TO_CENTER))
print('current: ' + str(stepper_p.getPosition()))
print("centering")
stepper_p.addPositionOffset(-(offset_pos + DIST_SWITCH_TO_CENTER))
def init_interface(interfaceKit_serial, hubPort, isHubPortDevice):
if with_interface:
try:
print("Connecting Output Channels (%s): %s" %
(interfaceKit_serial, output_channels))
for ich, this_channel in enumerate(output_channels):
print("... %s: %s" % (ich, this_channel))
ch = DigitalOutput()
ch.setDeviceSerialNumber(interfaceKit_serial)
ch.setHubPort(hubPort)
ch.setIsHubPortDevice(isHubPortDevice)
ch.setChannel(df_params.loc[1, this_channel])
ch.setOnAttachHandler(onAttachHandler)
ch.setOnDetachHandler(onDetachHandler)
ch.setOnErrorHandler(onErrorHandler)
ch.openWaitForAttachment(5000)
interfaceKit_output[ich] = ch
interfaceKit_output[ich].setState(False) #init off
print("Connecting Input Channels %s: %s" %
(interfaceKit_serial, input_channels))
for ich, this_channel in enumerate(input_channels):
ch = DigitalInput()
if ich == 1: # 'input_zLimitSwitch', 'input_fLimitSwitch']
ch.linkedOutput = f_stepper
ch.linkedOutput.setOnPositionChangeHandler(
fpositionChangeHandler)
ch.setOnStateChangeHandler(fonStateChangeHandler)
else:
ch.linkedOutput = z_stepper
ch.linkedOutput.setOnPositionChangeHandler(
zpositionChangeHandler)
ch.setOnStateChangeHandler(zonStateChangeHandler)
ch.setDeviceSerialNumber(interfaceKit_serial)
ch.setHubPort(hubPort)
ch.setIsHubPortDevice(isHubPortDevice)
ch.setChannel(df_params.loc[1, this_channel])
ch.setOnAttachHandler(onAttachHandler)
ch.setOnDetachHandler(onDetachHandler)
ch.setOnErrorHandler(onErrorHandler)
ch.openWaitForAttachment(5000)
interfaceKit_input[ich] = ch
except PhidgetException as e:
print("Attachment Terminated: Open Failed", e)
print("Cleaning up...")
for ch in interfaceKit_output:
ch.close()
return False
except RuntimeError as e:
print("Attachment Terminated: RunTimeError", e)
sys.stderr.write("Runtime Error: \t")
return False
return [interfaceKit_output, interfaceKit_input]
zstepper_accelleration = 1000
output_channels = [
'output_filterDARK', 'output_filterGFP', 'output_filterRFP',
'output_filterCFP', 'output_filterYFP', 'output_camera',
'output_incubator', 'output_humidity'
]
input_channels = ['input_zLimitSwitch', 'input_fLimitSwitch']
########################## Initialize InterfaceKit
f_stepper = Stepper()
z_stepper = Stepper()
interfaceKit_output = [DigitalOutput() for i in range(0, len(output_channels))]
interfaceKit_input = [DigitalInput() for i in range(0, len(input_channels))]
########################## USER-DEFINED PARAMETERS
#MODIFY TO DEFINE SERIAL NUMBERS!!
z_stepper_serial = '506023'
f_stepper_serial = '522559'
interfaceKit_serial = '313877'
arduino_usbport = '/dev/ttyUSB0'
#DEFAULT VALUES
fpos_filterBRIGHT = -3250
fpos_filterDARK = -3250
fpos_filterGFP = -4994
fpos_filterRFP = -6555
fpos_filterCFP = -1690
class PhidgetMonitor:
def __init__(self,channel,sensor_name,topic_name,modo):
self.channel = channel
self.sensor_name = sensor_name
self.topic_name = topic_name
self.modo = modo
#self.pub = rospy.Publisher(self.topic_name, DeviceInfo, queue_size=10)
self.serv = rospy.Service(self.topic_name, ProximityStatus, self.ReturnStatus)
self.msg = DeviceInfo()
self.modo = modo
if modo == 'ANALOG':
self.ch = VoltageInput()
elif modo == 'DIGITAL':
self.ch = DigitalInput()
def setup(self):
self.msg.channel = self.channel
self.msg.device_name = self.sensor_name
self.ch.setChannel(self.channel)
self.ch.setOnAttachHandler(self.VoltageInputAttached)
self.ch.setOnDetachHandler(self.VoltageInputDetached)
self.ch.setOnErrorHandler(self.ErrorEvent)
if self.modo == 'ANALOG':
self.ch.setOnVoltageChangeHandler(self.VoltageChangeHandler)
elif self.modo == 'DIGITAL':
self.ch.setOnStateChangeHandler(self.VoltageChangeHandler)
print("Waiting for the Phidget VoltageInput Object to be attached...")
self.ch.openWaitForAttachment(5000)
def close(self):
self.ch.close()
print("Closed VoltageInput device")
def VoltageInputAttached(self,attached):
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Channel: %d" % attached.getChannel())
print("Channel Name: %s" % attached.getChannelName())
print("Device ID: %d" % attached.getDeviceID())
print("Device Name: %s" % attached.getDeviceName())
print("Mode: %s" %self.modo)
print("\n")
def VoltageInputDetached(self,detached):
print("\nDetach event on Port %d Channel %d" % (detached.getHubPort(), detached.getChannel()))
def ErrorEvent(self, subject, eCode, description):
print("Error %i : %s" % (eCode, description))
def VoltageChangeHandler(self, subject, voltage):
self.msg.voltage = voltage
#self.pub.publish(self.msg)
def ReturnStatus(self,req):
return self.msg.voltage
def setup_phidget(self, port, serialNumber=559177, channel=0):
ch = DigitalInput()
ch.setDeviceSerialNumber(serialNumber)
ch.setHubPort(port)
ch.setChannel(channel)
ch.setIsHubPortDevice(True)
ch.setOnAttachHandler(onAttachHandler)
ch.setOnDetachHandler(onDetachHandler)
ch.setOnErrorHandler(onErrorHandler)
ch.setOnStateChangeHandler(self.onStateChangeHandler)
return ch
import sys
import time
from Phidget22.Devices.DigitalInput import *
from Phidget22.PhidgetException import *
from Phidget22.Phidget import *
from Phidget22.Net import *
try:
ch = DigitalInput()
except RuntimeError as e:
print("Runtime Exception %s" % e.details)
print("Press Enter to Exit...\n")
readin = sys.stdin.read(1)
exit(1)
def DigitalInputAttached(e):
try:
attached = e
print("\nAttach Event Detected (Information Below)")
print("===========================================")
print("Library Version: %s" % attached.getLibraryVersion())
print("Serial Number: %d" % attached.getDeviceSerialNumber())
print("Channel: %d" % attached.getChannel())
print("Channel Class: %s" % attached.getChannelClass())
print("Channel Name: %s" % attached.getChannelName())
print("Device ID: %d" % attached.getDeviceID())
print("Device Version: %d" % attached.getDeviceVersion())
print("Device Name: %s" % attached.getDeviceName())
print("Device Class: %d" % attached.getDeviceClass())
print("\n")
#continue
#this event handler will deal with the bumper switches input
def BSonStateChange(self, state):
print("State [" + str(self.getChannel()) + "]: " + str(state))
#this will check that the bumper switches have not hit anything
if(state):
#invert motors
else:
#continue
#defining array of digital inputs
digitalIN = [DigitalInput() for i in range (0,8)]
#addressing the channels
for i in range(0,8):
digitalIN[i].setChannel(i)
digitalIN[i].setOnStateChangeHandler(BSonStateChange)
digitalIN[i].openWaitForAttachment(5000)
#defining array of analogue inputs
analogueIN = [VoltageInput() for i in range (0,8)]
#addressing the channels
for i in range(0,8):
analogueIN[i].setChannel(i)
#this line will basically call the event handler when the readings change
analogueIN[i].setOnVoltageChangeHandler(IRonVoltageChange)
import random
import httplib
import urllib
import urllib2
import datetime
import time
#Phidget specific imports
from Phidget22.PhidgetException import *
#from Phidget22.Events import *
from Phidget22.Devices.DigitalInput import *
from Phidget22.Phidget import *
#Create an interfacekit object
try:
interfaceKit = DigitalInput()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Event Handler Callback Functions
def interfaceKitAttached(e):
#attached = e.device
print("Pedal Attached!")
def interfaceKitDetached(e):
#detached = e.device
print("Pedal Detached!")
def main():
try:
"""
* Allocate a new Phidget Channel object
"""
ch = DigitalInput()
"""
* Set matching parameters to specify which channel to open
"""
#You may remove this line and hard-code the addressing parameters to fit your application
channelInfo = AskForDeviceParameters(ch)
ch.setDeviceSerialNumber(channelInfo.deviceSerialNumber)
ch.setHubPort(channelInfo.hubPort)
ch.setIsHubPortDevice(channelInfo.isHubPortDevice)
ch.setChannel(channelInfo.channel)
if (channelInfo.netInfo.isRemote):
ch.setIsRemote(channelInfo.netInfo.isRemote)
if (channelInfo.netInfo.serverDiscovery):
try:
Net.enableServerDiscovery(
PhidgetServerType.PHIDGETSERVER_DEVICEREMOTE)
except PhidgetException as e:
PrintEnableServerDiscoveryErrorMessage(e)
raise EndProgramSignal(
"Program Terminated: EnableServerDiscovery Failed")
else:
Net.addServer("Server", channelInfo.netInfo.hostname,
channelInfo.netInfo.port,
channelInfo.netInfo.password, 0)
"""
* Add event handlers before calling open so that no events are missed.
"""
print("\n--------------------------------------")
print("\nSetting OnAttachHandler...")
ch.setOnAttachHandler(onAttachHandler)
print("Setting OnDetachHandler...")
ch.setOnDetachHandler(onDetachHandler)
print("Setting OnErrorHandler...")
ch.setOnErrorHandler(onErrorHandler)
#This call may be harmlessly removed
PrintEventDescriptions()
print("Setting OnStateChangeHandler...")
ch.setOnStateChangeHandler(onStateChangeHandler)
"""
* Open the channel with a timeout
"""
print("\nOpening and Waiting for Attachment...")
try:
ch.openWaitForAttachment(5000)
except PhidgetException as e:
PrintOpenErrorMessage(e, ch)
raise EndProgramSignal("Program Terminated: Open Failed")
print("Sampling data for 10 seconds...")
time.sleep(10)
"""
* Perform clean up and exit
"""
print("\nDone Sampling...")
print("Cleaning up...")
ch.close()
print("\nExiting...")
return 0
except PhidgetException as e:
sys.stderr.write("\nExiting with error(s)...")
DisplayError(e)
traceback.print_exc()
print("Cleaning up...")
ch.close()
return 1
except EndProgramSignal as e:
print(e)
print("Cleaning up...")
ch.close()
return 1
except RuntimeError as e:
sys.stderr.write("Runtime Error: \n\t" + e)
traceback.print_exc()
return 1
finally:
print("Press ENTER to end program.")
readin = sys.stdin.readline()
def main():
digitalInput0 = DigitalInput()
digitalInput1 = DigitalInput()
digitalInput2 = DigitalInput()
digitalInput3 = DigitalInput()
digitalInput4 = DigitalInput()
digitalInput5 = DigitalInput()
digitalInput6 = DigitalInput()
digitalInput7 = DigitalInput()
digitalInput0.setChannel(0)
digitalInput1.setChannel(1)
digitalInput2 = DigitalInput()
digitalInput3 = DigitalInput()
digitalInput4 = DigitalInput()
digitalInput5 = DigitalInput()
digitalInput6 = DigitalInput()
digitalInput7 = DigitalInput()
digitalInput0.setChannel(0)
digitalInput1.setChannel(1)
digitalInput2.setChannel(2)
digitalInput3.setChannel(3)
digitalInput4.setChannel(4)
digitalInput5.setChannel(5)
digitalInput6.setChannel(6)
digitalInput7.setChannel(7)
digitalInput0.setOnStateChangeHandler(onStateChange)
digitalInput1.setOnStateChangeHandler(onStateChange)
digitalInput2.setOnStateChangeHandler(onStateChange)
digitalInput3.setChannel(3)
digitalInput4.setChannel(4)
digitalInput5.setChannel(5)
digitalInput6.setChannel(6)
digitalInput7.setChannel(7)
digitalInput0.setOnStateChangeHandler(onStateChange)
digitalInput1.setOnStateChangeHandler(onStateChange)
digitalInput2.setOnStateChangeHandler(onStateChange)
digitalInput3.setOnStateChangeHandler(onStateChange)
digitalInput4.setOnStateChangeHandler(onStateChange)
digitalInput5.setOnStateChangeHandler(onStateChange)
digitalInput6.setOnStateChangeHandler(onStateChange)
digitalInput7.setOnStateChangeHandler(onStateChange)
digitalInput0.openWaitForAttachment(5000)
digitalInput1.openWaitForAttachment(5000)
digitalInput2.openWaitForAttachment(5000)
digitalInput3.openWaitForAttachment(5000)
digitalInput4.setOnStateChangeHandler(onStateChange)
digitalInput5.setOnStateChangeHandler(onStateChange)
digitalInput6.setOnStateChangeHandler(onStateChange)
digitalInput7.setOnStateChangeHandler(onStateChange)
digitalInput0.openWaitForAttachment(5000)
digitalInput1.openWaitForAttachment(5000)
digitalInput2.openWaitForAttachment(5000)
digitalInput3.openWaitForAttachment(5000)
digitalInput4.openWaitForAttachment(5000)
digitalInput5.openWaitForAttachment(5000)
digitalInput6.openWaitForAttachment(5000)
digitalInput7.openWaitForAttachment(5000)
try:
input("Press Enter to Stop\n")
except (Exception, KeyboardInterrupt):
pass
digitalInput0.close()
digitalInput1.close()
digitalInput2.close()
digitalInput3.close()
digitalInput4.close()
digitalInput5.close()
digitalInput6.close()
digitalInput7.close()
import sys
import time
from Phidget22.Phidget import *
from Phidget22.Devices.DigitalOutput import *
from Phidget22.Devices.DigitalInput import *
number_of_inputs = 4
number_of_outputs = 4
timeout_duration = 5 * 1000 # its in miliseconds
serial_number = 96781
# create input pins
input_pins = [DigitalInput() for each_index in range(number_of_inputs)]
for each_index, each in enumerate(input_pins):
each.setChannel(each_index)
each.setDeviceSerialNumber(serial_number)
each.openWaitForAttachment(timeout_duration)
# create ouput pins
output_pins = [DigitalOutput() for each_index in range(number_of_outputs)]
for each_index, each in enumerate(output_pins):
each.setChannel(each_index)
each.setDeviceSerialNumber(serial_number)
each.openWaitForAttachment(timeout_duration)
def which_key(input_pin, index_of_input):
key_values = [["1", "2", "3", "A"], ["4", "5", "6", "B"],
["7", "8", "9", "C"], ["*", "0", "#", "D"]]
return_value = None
from Phidget22.Phidget import *
from Phidget22.Devices.DigitalInput import *
import time
start_time = time.time()
def onStateChange(self, state):
global start_time
print("State: " + str(state))
first = time.time()
if state == 1:
start_time = time.time()
else:
first = time.time()
netTime = first - start_time
speedMps = ((0.219075 / netTime))
print(speedMps, "m/s")
print(speedMps * 3.6, "kmh")
digitalInput0 = DigitalInput()
digitalInput0.setOnStateChangeHandler(onStateChange)
digitalInput0.openWaitForAttachment(5000)