class ArdUno():
def __init__(self):
self.port = 'COM21'
self.board = Arduino(self.port)
self.anticrash()
self.readenable()
self.board.digital[8].write(1)
def anticrash(self):
""" Iterator evita que se crashee el puerto (no permite que se sobrecargue de trafico) """
it = util.Iterator(self.board)
it.start() #corremos el iterator
def readenable(self):
self.board.analog[0].enable_reporting()
def getpin(self,numchannel):
ch=str(numchannel)
pin='a:' + ch + ':i'
R=self.board.get_pin(pin)
return(R)
def chdef(self,namechannel,numchannel):
ch=str(numchannel)
R='A' + ch + ' = h.getpin(' + ch + ')'
return(R)
def voltread(self,namechannel,numchannel):
ch=str(numchannel)
A=namechannel+ ch +'.read()'
return(str(A))
class LightsController:
def __init__(self):
self.board = Arduino("/dev/ttyACM0")
self.lightPin = self.board.get_pin("d:3:p")
def Off(self):
self.lightPin.write(0)
sleep(1)
def On(self):
self.lightPin.write(1)
sleep(1)
def Fade(self):
for pwm in arange(0.02, 0.1, 0.004):
self.lightPin.write(pwm)
sleep(0.075)
for pwm in arange(0.1, 0.8, 0.01):
self.lightPin.write(pwm)
sleep(0.075)
for pwm in arange(0.8, 0.1, -0.01):
self.lightPin.write(pwm)
sleep(0.075)
for pwm in arange(0.1, 0.02, -0.004):
self.lightPin.write(pwm)
sleep(0.075)
def Blink(self):
self.lightPin.write(1)
sleep(1)
self.lightPin.write(0)
sleep(1)
class Hinako:
def __init__(self, port, b_pin_id, w_pin_id, h_pin_id):
self.board = Arduino(port)
'''
d: digital output
n: number PWM pin
s: servo control
'''
self.b_pin = self.board.get_pin(b_pin_id)
self.w_pin = self.board.get_pin(w_pin_id)
self.h_pin = self.board.get_pin(h_pin_id)
self.b = 0
self.w = 0
self.h = 0
def _move_servo(self, pin, begin_val, end_val, ds=0.1):
step = 1 if begin_val < end_val else -1
print '%d -> %d' % (begin_val, end_val)
print step
for i in range(begin_val, end_val, step):
print i
pin.write(i)
time.sleep(ds)
def set_bust(self, size_cm, ds=0.1):
print "bust: %d cm" % size_cm
val = int(round(map_value(size_cm, 70, 100, 65, 0)))
self._move_servo(self.b_pin, self.w, val, ds=ds)
self.w = val
def set_waist(self, val):
'''
dc motor
self.w_pin
'''
def set_hip(self, val):
self._move_servo(self.h_pin, self.h, val)
self.h = val
from pyfirmata import Arduino, util
import time
refVoltage = 4.50
R1 = 3270.3
R2 = 1013.5
vdivide = (R1+R2)/R2
vscale = refVoltage * vdivide
board = Arduino('/dev/ttyACM0')
analogPin = 4
it = util.Iterator(board)
it.start()
voltPin = board.get_pin('a:'+str(analogPin)+':i')
voltPin.enable_reporting()
avg10 = [0 for i in range(10)]
time.sleep(2)
while(1):
voltval = voltPin.read()*vscale
avg10.insert(0, voltval)
avg10.pop()
avg = 0.0
for n in avg10:
avg += n
print avg / len(avg10)
time.sleep(.5)
def excution_irrigation():
"""
Performs irrigation regardless of soil condition.
:return:
"""
try:
Board = Arduino('COM4') # Defines the port where the Arduino is
it = util.Iterator(Board) # Controls arduino port readings
it.start() # Starts communication with the arduino.
# Sets relay pins to OUTPUT
Board.get_pin('d:10:o')
# relay starts off
Board.digital[10].write(1)
# Defines analog port -> sensor
Board.get_pin('a:0:i')
time.sleep(0.5)
Board.digital[10].write(0) # turn on the relay
time.sleep(2.5) # Hold open for 3 seconds
Board.digital[10].write(1) # turn off the relay
except Exception as e:
print(e)
def setup():
# setup runs once
board = Arduino('/dev/ttyACM0')
board.digital[13].write(1) # sets the onboard LED to ON.
analog_0 = board.get_pin('a:0:i') # pot pin
pin0 = board.get_pin('d:8:o') # pin0 for motor 1
pin1 = board.get_pin('d:9:o') # pin1 for motor 1
pin2 = board.get_pin('d:10:o') # pin2 for motor 2
pin3 = board.get_pin('d:11:o') # pin3 for motor 2
pwm1 = board.get_pin('d:5:p') # pwm pin for motor 1
pwm2 = board.get_pin('d:6:p') # pwm pin for motor 2
# start these values with the correct values for a forward shot.
pin0state = 0
pin1state = 0
pin2state = 0
pin3state = 0
pin0.write(pin0state)
pin1.write(pin1state)
pin2.write(pin2state)
pin3.write(pin3state)
iter = util.Iterator(board)
iter.start()
board.analog[0].enable_reporting() # change to be port of potentiometer.
print('done!')
def initiate_arduino():
""" Sets up the Arduino port and pins. Returns a microController object for saving data about the Arduino."""
# Pyfirmata
board = Arduino("COM4")
it = util.Iterator(board)
it.start()
# Setting up pins on the Arduino board
red_led = board.get_pin('d:6:o')
blue_led = board.get_pin('d:10:o')
temp_sensor = board.get_pin('a:0:i')
temp_sensor.enable_reporting()
photo_sensor = board.get_pin('a:2:i')
photo_sensor.enable_reporting()
center_button = board.get_pin('d:13:i')
center_button.enable_reporting()
# Starting values
time_start = time.time() # Used for the interval
debounce_start = time.time() # Used for the button bouncing
# Custom object to store info of the Arduino micro controller
micro_controller = MicroController(center_button, debounce_start,
photo_sensor, temp_sensor, time_start,
red_led, blue_led)
time.sleep(
1
) # Important for pyfirmata to initialize before trying to read values
print("Ready!")
return micro_controller
class ArduinoHelper(object):
portName = 'COM4'
def __init__(self):
self.arduino = Arduino(self.portName)
it = util.Iterator(self.arduino)
it.start()
self.start_time = time.time()
self.input_pin = self.arduino.get_pin(
'a:0:i') #analog, input to the computer
self.output_pin = self.arduino.get_pin(
'd:6:p') #digital, PWM mode, from the computer
self.switch_pin = self.arduino.get_pin(
'd:9:i') #digital, input to the computer (switch-state)
# make sure the PWM pin (6) is off, wait a number of seconds
# (gives enough time so everything is ready to be read - otherwise it may read "None")
time.sleep(2)
self.update_duty_cycle(0)
def read_input_pin(self):
return self.read_voltage(self.input_pin)
def read_voltage(self, pin):
"""
Returns the voltage of the pin passed in (between 0 and 5 Volts)
It takes a number of measurements, then averages them to reduce noise
"""
outputs = []
for i in range(10):
time.sleep(.1)
outputs.append(pin.read())
return 5.0 * np.average(outputs)
def read_voltage_fast(self, pin):
"""
Returns the voltage of the pin passed in (between 0 and 5 Volts)
It takes a single of measurement
"""
return 5.0 * pin.read()
def write_to_lcd(self, *args):
message = ''
for arg in args:
message += str(arg)
print(message)
self.arduino.send_sysex(STRING_DATA,
util.str_to_two_byte_iter(str(message)))
def is_switch_on(self):
switch_voltage = self.read_voltage_fast(self.switch_pin)
return switch_voltage > 1
def update_duty_cycle(self, DC):
self.output_pin.write(DC)
def switch():
board = Arduino('/dev/ttyACM0')
it = util.Iterator(board)
it.start()
digital_input = board.get_pin('d:10:i')
led_13 = board.get_pin('d:13:o')
while True:
sw_value = digital_input.read()
if sw_value:
led_13.write(1)
print('Led ON')
else:
led_13.write(0)
print('Led OFF')
def run(self):
interrupted_flag = False
try:
board = Arduino('/dev/cu.usbserial-A600egZF')
except Exception:
self.stopped.emit(State.CannotConnect)
return
iterator = util.Iterator(board)
iterator.start()
pressure_pin = None
if self.record_pressure:
pressure_pin = board.get_pin('a:5:i')
temp_pin = None
if self.record_temp:
temp_pin = board.get_pin('a:4:i')
self.led_pin = board.get_pin('d:2:o')
sleep(1)
current_thread = QThread().currentThread()
self.timer_start.emit()
# Turn on led to indicate test is running
self.led_pin.write(1)
counter = 0
pressure = Sample.NDV
temp = Sample.NDV
while counter != self.number_of_samples:
sleep(self.delay)
if self.record_pressure:
pressure = (pressure_pin.read() *
5) * 150 / 4 + self.CORRECTION
if self.record_temp:
temp = (temp_pin.read() * 5) * 212 / 4 + 32 + self.CORRECTION
logger.info("P and T readings from arduino: {:.2f}, {:.2f}".format(
pressure, temp))
self.sample_received.emit(Sample(pressure, temp))
if current_thread.isInterruptionRequested():
interrupted_flag = True
break
if not iterator.is_alive():
self.stopped.emit(State.Disconnected)
break
counter += 1
if interrupted_flag:
self.stopped.emit(State.Aborted)
else:
self.stopped.emit(State.Completed)
board.exit()
def main():
print('Hello there, please record your command'
' immediately after you see start')
board = Arduino('/dev/ttyUSB0')
RightLeftServoPin = board.get_pin('d:5:s')
ForwardBackwardServoLPin = board.get_pin('d:6:s')
UpDownServoPin = board.get_pin('d:7:s')
# gripServoPin = board.get_pin('d:8:p')
sleep(3)
iterSer = util.Iterator(board)
iterSer.start()
angle = 90
change = 45
while True:
command = imp()
if command == "left":
print('Your command is : {} \n Turning Left'.format("Left"))
RightLeftServoPin.write(angle - change)
board.pass_time(2)
elif command == "right":
print('Your command is : {} \n Turning Right'.format("Right"))
RightLeftServoPin.write(angle + change)
board.pass_time(2)
elif command == "up":
print('Your command is : {} \n Going Up'.format("Up"))
UpDownServoPin.write(angle + change)
board.pass_time(2)
elif command == "down":
print('Your command is : {} \n Going Down'.format("Down"))
UpDownServoPin.write(angle - change)
board.pass_time(2)
elif command == "forward":
print('Your command is : {} \n Going forward'.format("forward"))
ForwardBackwardServoLPin.write(angle + change)
board.pass_time(2)
elif command == "backward":
print('Your command is : {} \n Going backward'.format("backward"))
ForwardBackwardServoLPin.write(angle - change)
board.pass_time(2)
elif command == "off":
print('Your command is : {} \n Turning off '.format("off"))
break
board.exit()
def LED_GAR(data):
board = Arduino('/dev/ttyACM0')
led_gar = board.get_pin('d:2:o')
if data == 'off_garage':
led_gar.write(0)
elif data == 'on_garage':
led_gar.write(1)
def connectArduino():
global duino, heater, fan, humidifier, devices
a = 0
while a < 5:
duino = ""
comport = "/dev/ttyACM" + str(a)
if path.exists(comport):
duino = Arduino(comport)
logger.debug(duino)
if str(duino).startswith("Arduino"):
logger.debug("Connected to Arduino.")
a = 5
logger.debug("Connecting peripherals.")
l = 0
while l < len(devices):
devices[l]["device"] = duino.get_pin('d:' +
devices[l]["pin"] +
':o')
if devices[l]["device"] != "Uninit":
devname = devices[l]["name"]
logger.debug("Connected to " + devname +
" successfully on " +
str(devices[l]["device"]))
dowrite(devname, 0)
logger.debug("New status of " + devname + ": " +
str(devices[l]["status"]))
l = l + 1
# duino.digital[2].write(1)
# duino.digital[4].write(1)
# duino.digital[7].write(1)
a = a + 1
def connect(port):
global arduinoNano, leftPWM, rightPWM, leftEnable, rightEnable, throttlePin
if arduinoNano == None:
arduinoNano = Arduino(port)
it = util.Iterator(arduinoNano)
it.start()
arduinoNano.analog[steerSensorPin].enable_reporting()
arduinoNano.analog[throttleSensorPin].enable_reporting()
leftPWM = arduinoNano.get_pin('d:6:p')
rightPWM = arduinoNano.get_pin('d:9:p')
throttlePin = arduinoNano.get_pin('d:10:p')
arduinoNano.digital[rightEnable].write(1)
arduinoNano.digital[leftEnable].write(1)
return "succes"
else:
return "error"
def clima(data):
board = Arduino('/dev/ttyACM0')
vent = board.get_pin('d:4:o')
if data == 'on_vent' or '1':
vent.write(1)
elif data == 'off_vent' or data == '0':
vent.write(0)
def RunSensor():
print "wait for around 10 seconds..."
#set the board
board = Arduino('/dev/ttyS0')
#print "sleeping..."
sleep(2) #very important.
#start a thread
it = util.Iterator(board)
it.start()
#set the analog input pin on the Arduino ( Sleepy Pi )
a0 = board.get_pin('a:0:i')
try:
#ignore first few 'nonetype' garbage values
for i in range(10):
garbage = a0.read()
#print garbage
sleep(0.1)
#continually read values
while True:
v = a0.read()*1000
sleep(0.5)
print "Pressure: ", v, " kPa"
except KeyboardInterrupt:
board.exit()
os._exit()
class ArduinoConnection(Thread):
def __init__(self):
Thread.__init__(self)
self.SAMPLING_INTERVAL = 0.100
self.MEAN_INTERVAL = 5
self.MEAN_SAMPLES_NUMBER = round(self.MEAN_INTERVAL/self.SAMPLING_INTERVAL)
PORT = '/dev/ttyACM0'
self.board = Arduino(PORT)
it = util.Iterator(self.board)
it.start()
self.analog_pin_value_arr = [self.board.get_pin('a:0:i'), self.board.get_pin('a:1:i'), self.board.get_pin('a:2:i'), self.board.get_pin('a:3:i'), self.board.get_pin('a:4:i'), self.board.get_pin('a:5:i')]
for i in range(len(self.analog_pin_value_arr)):
self.analog_pin_value_arr[i].enable_reporting()
self.mean_analog_valuea_arr = [0.0] * 6
self.mean_analog_valuea_assigned_arr = [0.0] * 6
def run(self):
#s= ''
sample_number = 0
while True:
while (sample_number < self.MEAN_SAMPLES_NUMBER):
# time.sleep(DELAY)
self.board.pass_time(self.SAMPLING_INTERVAL)
for i in range(len(self.mean_analog_valuea_arr)):
self.mean_analog_valuea_arr[i] = self.mean_analog_valuea_arr [i] + self.analog_pin_value_arr[i].read()
sample_number = sample_number + 1
for i in range(len(self.mean_analog_valuea_arr)):
self.mean_analog_valuea_arr[i] = self.mean_analog_valuea_arr[i] / self.MEAN_SAMPLES_NUMBER
#s = s + str(self.mean_analog_valuea_arr[i]) + ' '
self.mean_analog_valuea_assigned_arr = self.mean_analog_valuea_arr
#print s
#s = ''
sample_number = 0
self.mean_analog_valuea_arr = [0.0] * 6
def getMeanAnalogArduinoValueArray(self):
return self.mean_analog_valuea_assigned_arr
def __init__(self, configs: dict):
arduino = PyfirmataArduino(configs['usb_port'])
print('arduino is online!')
it = util.Iterator(arduino)
it.start()
self._btn_door = arduino.get_pin('d:5:i')
self._btn_door.enable_reporting()
self._btn_outside = arduino.get_pin('d:4:i')
self._btn_outside.enable_reporting()
self._btn_inside = arduino.get_pin('d:6:i')
self._btn_inside.enable_reporting()
self._servo_motor = arduino.get_pin('d:9:s')
self._servo_motor.write(self._UNLOCK_POSITION)
self._servo_position = self._UNLOCK_POSITION
self._servo_is_in_use = False
def smart_shield():
board = Arduino('/dev/ttyACM0')
it = util.Iterator(board)
it.start()
motor_a_dir = board.get_pin('d:7:o')
# PWM
motor_a_speed = board.get_pin('d:6:p')
my_motor = board.get_pin('d:3:p')
while True:
print('I\'m in')
motor_a_dir.write(0)
motor_a_speed.write(0.9)
# my_motor.write()
time.sleep(0.1)
def rgb():
board = Arduino('/dev/ttyACM0')
# it = util.Iterator(board)
# it.start()
red_pin = board.get_pin('d:11:p')
green_pin = board.get_pin('d:10:p')
blue_pin = board.get_pin('d:9:p')
print('PWM says: \"Let\'s start!\"')
red_pin.write(0)
green_pin.write(0)
blue_pin.write(0)
time.sleep(0.5)
print('START')
green_pin.write(0.1)
time.sleep(5)
print('STOP')
green_pin.write(0)
def irrigation():
"""
Function that performs communication with the arduino and infinitely checks the status of the ground
:return:
"""
try:
Board = Arduino('COM4') # Defines the port where the Arduino is
it = util.Iterator(Board) # Controls arduino port readings
it.start() # Starts communication with the arduino.
# Sets relay pins to OUTPUT
Board.get_pin('d:10:o')
# relay starts off
Board.digital[10].write(1)
# Defines analog port -> sensor
valor_analog = Board.get_pin('a:0:i')
time.sleep(0.5)
while True:
# Value the sensor read
valor_analogico = valor_analog.read()
if 0 < valor_analogico < 0.5: # If soil is wet
print("STATUS: Moist soil - {} \n".format(valor_analogico))
elif 0.5 < valor_analogico < 0.8: # If the solo is normal
print("STATUS: Solo moderado - {} \n".format(valor_analogico))
elif 0.8 < valor_analogico <= 1: # If the soil is dry
print(" \nSTATUS: Dry soil - {}\n".format(valor_analogico))
Board.digital[10].write(0) # turn on the relay
print("WATERING...")
time.sleep(2.5) # Hold open for 3 seconds
Board.digital[10].write(1) # turn off the relay
print("SUCCESSFULLY WATERED PLANTS!!!")
time.sleep(60) # 3 sec delay, then the loop repeats
except Exception as e:
print(e)
def main():
print('Hello there, please record your command'
'immediately after you see start')
# windows = 'COM8'
# PiOS = '/dev/ttyUSB0'
board = Arduino('COM9')
RightLeftServoPin = board.get_pin('d:5:s')
ForwardBackwardServoLPin = board.get_pin('d:6:s')
UpDownServoPin = board.get_pin('d:7:s')
# gripServoPin = board.get_pin('d:8:p')
sleep(5)
iterSer = util.Iterator(board)
iterSer.start()
angle = 90
change = 45
while True:
command = imp()
if command == "left":
print('Your command is : {}'.format("Turning Left"))
RightLeftServoPin.write(angle - change)
elif command == "right":
print('Your command is : {}'.format("Turning Right"))
RightLeftServoPin.write(angle + change)
elif command == "up":
print('Your command is : {}'.format("Going Up"))
UpDownServoPin.write(angle + change)
elif command == "down":
print('Your command is : {}'.format("Going Down"))
UpDownServoPin.write(angle - change)
elif command == "forward":
print('Your command is : {}'.format("Going forward"))
ForwardBackwardServoLPin.write(angle + change)
elif command == "backward":
print('Your command is : {}'.format("Going backward"))
ForwardBackwardServoLPin.write(angle - change)
elif command == "off":
break
board.exit()
def potentiometer():
board = Arduino('/dev/ttyACM0')
it = util.Iterator(board)
it.start()
analog_input = board.get_pin('a:0:i')
while True:
analog_value = analog_input.read()
print(analog_value)
time.sleep(0.1)
def DC_DOOR(data):
board = Arduino('/dev/ttyACM0')
mot1_3 = board.get_pin('d:12:o')
mot1_4 = board.get_pin('d:13:o')
if data == 'on_DOOR':
mot1_3.write(1)
time.sleep(0.5)
mot1_3.write(0)
elif data == 'off_DOOR':
mot1_4.write(1)
time.sleep(0.5)
mot1_4.write(0)
def blink(b_time):
board = Arduino('/dev/ttyACM0')
it = util.Iterator(board)
it.start()
led_13 = board.get_pin('d:13:o')
while True:
led_13.write(1)
print('Led ON')
time.sleep(b_time)
led_13.write(0)
print('Led OFF')
time.sleep(b_time)
def main():
board = Arduino('/dev/ttyUSB0')
#starting values
multiplier = 400.0
cam = Camera()
js = JpegStreamer()
analog_4 = board.get_pin('a:1:i')
analog_5 = board.get_pin('a:2:i')
button_13 = board.get_pin('d:13:i')
it = util.Iterator(board)
it.start()
while (1):
#print str(analog_5.read()) + "\t\t" + str(analog_4.read())
t1 = analog_5.read()
t2 = analog_4.read()
b13 = button_13.read()
if not t1:
t1 = 50
else:
t1 *= multiplier
if not t2:
t2 = 100
else:
t2 *= multiplier
print "t1 " + str(t1) + ", t2 " + str(t2) + ", b13 " + str(b13)
cam.getImage().flipHorizontal().edges(
int(t1), int(t2)).invert().smooth().save(js.framebuffer)
time.sleep(0.01)
def LED_R1(data):
from pyfirmata import Arduino, util
board = Arduino('/dev/ttyACM0')
led_r1 = board.get_pin('d:3:p')
if data == 'off_room1' or data == 'b1:0':
led_r1.write(0)
elif data == 'on_room1' or data == 'b1:10':
led_r1.write(1)
elif 'b1:1' <= data <= 'b1:2':
led_r1.write(0.2)
elif 'b1:2' < data <= 'b1:5':
led_r1.write(0.5)
elif 'b1:5' < data <= 'b1:9':
led_r1.write(0.8)
def DC_GARAGE(data):
board = Arduino('/dev/ttyACM0')
mot1_1 = board.get_pin('d:7:o')
mot1_2 = board.get_pin('d:8:o')
if data == 'on_GAR':
mot1_1.write(1)
time.sleep(2)
mot1_1.write(0)
mot1_2.write(0)
elif data == 'off_GAR':
mot1_2.write(1)
time.sleep(2)
mot1_2.write(0)
mot1_1.write(0)
def main():
board = Arduino('/dev/ttyUSB0')
#starting values
multiplier = 400.0
cam = Camera()
js = JpegStreamer()
analog_4 = board.get_pin('a:1:i')
analog_5 = board.get_pin('a:2:i')
button_13 = board.get_pin('d:13:i')
it = util.Iterator(board)
it.start()
while (1):
#print str(analog_5.read()) + "\t\t" + str(analog_4.read())
t1 = analog_5.read()
t2 = analog_4.read()
b13 = button_13.read()
if not t1:
t1 = 50
else:
t1 *= multiplier
if not t2:
t2 = 100
else:
t2 *= multiplier
print "t1 " + str(t1) + ", t2 " + str(t2) + ", b13 " + str(b13)
cam.getImage().flipHorizontal().edges(int(t1), int(t2)).invert().smooth().save(js.framebuffer)
time.sleep(0.01)
def LED_R2(data):
board = Arduino('/dev/ttyACM0')
led_r2 = board.get_pin('d:5:p')
if data == 'off_room2' or data == 'b2:0':
led_r2.write(0)
elif data == 'on_room2' or data == 'b2:10':
led_r2.write(1)
elif 'b2:1' <= data <= 'b2:2':
led_r2.write(0.2)
elif 'b2:2' < data <= 'b2:5':
led_r2.write(0.5)
elif 'b2:5' < data <= 'b2:9':
led_r2.write(0.8)
else:
led_r2.write(0)
def LED_HAL(data):
board = Arduino('/dev/ttyACM0')
led_hal = board.get_pin('d:9:p')
if data == 'off_hall' or data == 'ha:0':
led_hal.write(0)
elif data == 'on_hall' or data == 'ha:10':
led_hal.write(1)
elif 'ha:1' <= data <= 'ha:2':
led_hal.write(0.2)
elif 'ha:2' < data <= 'ha:5':
led_hal.write(0.5)
elif 'ha:5' < data <= 'ha:9':
led_hal.write(0.8)
else:
led_hal.write(0)
def LED_KIT(data):
board = Arduino('/dev/ttyACM0')
led_kit = board.get_pin('d:10:p')
if data == 'off_kitchen' or data == 'kit:0':
led_kit.write(0)
elif data == 'on_kitchen' or data == 'kit:10':
led_kit.write(1)
elif 'kit:1' <= data <= 'kit:2':
led_kit.write(0.2)
elif 'kit:2' < data <= 'kit:5':
led_kit.write(0.5)
elif 'kit:5' < data <= 'kit:9':
led_kit.write(0.8)
else:
led_kit.write(0)
def LED_LIV(data):
from pyfirmata import Arduino, util
board = Arduino('/dev/ttyACM0')
led_liv = board.get_pin('d:6:p')
if data == 'off_livingroom' or data == 'liv:0':
led_liv.write(0)
elif data == 'on_livingroom' or data == 'liv:10':
led_liv.write(1)
elif 'liv:1' <= data <= 'liv:2':
led_liv.write(0.2)
elif 'liv:2' < data <= 'liv:5':
led_liv.write(0.5)
elif 'liv:5' < data <= 'liv:9':
led_liv.write(0.8)
else:
led_liv.write(0)
def smart_irrigation(moisture, key):
board = Arduino('COM4')
iterator = util.Iterator(board)
iterator.start()
board.digital[4].write(0)
value = board.get_pin('a:0:i')
print("Machine Started")
while key == 1:
time.sleep(1.0)
print(value.read())
if (value.read() < moisture):
print("Moisture")
board.digital[4].write(0)
else:
print("NO Moisture")
board.digital[4].write(1)
def testWrite(digital_pins, analog_pins):
board = Arduino(serial_ports()[0])
iterator = util.Iterator(board)
iterator.start()
board.pass_time(1)
for i in range(
1, 14
): # 1-13 because there are problems with writing in Digital Pin 0
pin = board.get_pin(getPin('d', i, 'o'))
if (i in digital_pins):
pin.write(1)
else:
pin.write(0)
for pinNo in analog_pins:
continue
board.exit()
def servo():
board = Arduino('/dev/ttyACM0')
# it = util.Iterator(board)
# it.start()
servo1_pin = board.get_pin('d:10:p')
print('PWM says: \"Let\'s start!\"')
servo1_pin.write(0)
# return 0
while True:
print('Loop it!')
for i in range(0, 100):
s_value = i / 10
servo1_pin.write(s_value)
print(s_value)
time.sleep(5)
servo1_pin.write(0)
time.sleep(1)
def init():
global board
board = Arduino('/dev/ttyACM0')
global lazer1
lazer1 = board.get_pin('d:2:o')
global lazer2
lazer2 = board.get_pin('d:3:o')
global lazer3
lazer3 = board.get_pin('d:4:o')
global pump1
pump1 = board.get_pin('d:11:o')
global pump2
pump2 = board.get_pin('d:12:o')
global pwm_led
pwm_led = board.get_pin('d:6:p')
class ArduinoWrapper:
__arduino = None
def __init__(self, com=None):
self.connect(com)
def connect(self, com=None):
if com != None:
self.__arduino = Arduino(com)
else:
self.__arduino = Arduino(DEFAULT_COMPORT)
def update_pin(self, num, status):
if status:
print "update pin: %s -> ON" %str(num)
return self.__arduino.digital[num].write(1)
else:
print "update pin: %s -> OFF" %str(num)
return self.__arduino.digital[num].write(0)
def get_pin(self, status):
return self.__arduino.get_pin(status)
class Board(SerializableModel):
pk = None
port = None
pins = None
name = None
written_pins = set()
json_export = ('pk', 'port', 'pins')
def __init__(self, pk, port, *args, **kwargs):
self.pk = pk
self.port = port
self._board = Arduino(self.port)
self.pins = dict(((i, Pin(pk, i)) for i in range(14)))
[setattr(self, k, v) for k, v in kwargs.items()]
super(Board, self).__init__(*args, **kwargs)
def __del__(self):
try:
self.disconnect()
except:
print(traceback.format_exc())
def disconnect(self):
for pin in self.written_pins:
pin.write(0)
return self._board.exit()
def firmata_pin(self, identifier):
return self._board.get_pin(identifier)
def release_pin(self, identifier):
bits = identifier.split(':')
a_d = bits[0] == 'a' and 'analog' or 'digital'
pin_nr = int(bits[1])
self._board.taken[a_d][pin_nr] = False
#!/usr/bin/python
from pyfirmata import Arduino, util
# para buscar el puerto ls /dev/tty.*
import time
board = Arduino('/dev/tty.usbserial-A400eMcd')
pin5 = board.get_pin('d:5:p')
pin9 = board.get_pin('d:9:p')
pin11 = board.get_pin('d:11:p')
pin3 = board.get_pin('d:3:p')
contadorAgua = 0
contadorRaices = 0
class maquinaRaiz(object) :
ordCount = 0
def __init__(self, sentido, rampa):
self.rampa = rampa
self.sentido = sentido
maquinaRaiz.ordCount += 1
def encender(self):
https://bitbucket.org/tino/pyfirmata
'''
print __doc__
import time
from SimpleCV import *
import pygame
#----Start Config-----#
ARDUINO = False #Set this to true to enable arduino support
if ARDUINO:
from pyfirmata import Arduino, util
board = Arduino('/dev/ttyUSB0') #This value may have to change to match your system
it = util.Iterator(board) #load arduino drivers
it.start() #start the arduino
pin9 = board.get_pin('d:9:p') #the pin to control the motor
pin2 = board.get_pin('d:2:o') #pin to control the light
pin3 = board.get_pin('d:3:o') #pin to control the light
pin4 = board.get_pin('d:4:o') #pin to control the light
# Sound Config
SOUND = True
if SOUND:
pygame.mixer.init()
ping = pygame.mixer.Sound('ping.wav')
found_sound = pygame.mixer.Sound('found.wav')
searching_sound = pygame.mixer.Sound('searching.wav')
# Time Config
found_time = time.time()
search_timelimit = 2
def dcMotorControl(r, deltaT):
pwmPin.write(r/100.00)
sleep(deltaT)
pwmPin.write(0)
from pyfirmata import Arduino
from time import sleep
import os
port = '/dev/cu.usbmodem621'
board = Arduino(port)
sleep(5)
# set mode of pin 3 as PWM
pwmPin = board.get_pin('d:3:p')
try:
while True:
r = input("Enter value to set motor speed: ")
if ( r > 100) or ( r <= 0 ) :
print "betwee 0 to 100 only"
board.exit()
break
t = input("How long? in seconds")
dcMotorControl(r, t)
except KeyboardInterrupt:
board.exit()
os._exit
from sense_hat import SenseHat
from random import randint
from time import sleep
import pyfirmata
from pyfirmata import Arduino, util
sense = SenseHat()
board = Arduino('/dev/ttyACM1', baudrate=57600);
def setColor(r, g, b):
for x in range(0,8):
for y in range(0,8):
sense.set_pixel(x , y , r, g, b)
thread = util.Iterator(board)
thread.start()
pin = board.get_pin('d:2:i');
pin.enable_reporting()
while True:
pinValue = pin.read()
print pinValue
if pinValue == 0:
setColor(255, 0, 0)
else:
setColor(0, 255, 0)
sleep(.5)
DELAY = 2 # A 2 seconds delay
from pyfirmata import Arduino, util
board = Arduino('/dev/tty.usbmodemfa141')
#board.digital[PIN].write(1) # Set the LED pin to 1 (HIGH)
#board.digital[PIN].write(0) # Set the LED pin to 0 (LOW)
#board.digital[PIN].mode = PWM
#board.digital[PIN].write(256)
DURATION = 5
STEPS = 10
digital_0 = board.get_pin('d:9:p')
digital_1 = board.get_pin('d:10:p')
#board.digital[PIN].write(0.9)
# Waiting time between the
#wait_time = DURATION/float(STEPS)
# Note: Value range for PWM is 0.0 till 1.0
# Up
#for i in range(1, STEPS + 1):
# value = i/float(STEPS)
# digital_0.write(value)
# board.pass_time(wait_time)
pscom = devconfig.get('Parameters', 'pscom')
arduinocom = devconfig.get('Parameters', 'arduinocom')
ports = devconfig.getint('Parameters', 'ports')
tubevol = devconfig.getfloat('Parameters', 'tubevol')
length1 = devconfig.getfloat('Parameters', 'length1')
length2 = devconfig.getfloat('Parameters', 'length2')
length3 = devconfig.getfloat('Parameters', 'length3')
piv = devconfig.getint('Parameters', 'piv')
len1 = int(tubevol*length1)
len2 = int(tubevol*length2)
len3 = int(tubevol*length3)
ps = serial.Serial(port=pscom, baudrate=9600, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS) # VICI port selector
board = Arduino(arduinocom) # Arduino Uno
n2 = board.get_pin('d:2:o') # Solenoid valve normally closed, connected to digital pin 2
vent = board.get_pin('d:3:o') # Solenoid valve normally open, connected to digital pin 3
reagent = board.get_pin('d:4:o') # Solenoid valve normally closed, connected to digital pin 4
waste = board.get_pin('d:5:o') # Solenoid valve normally closed, connected to digital pin 5
prime = board.get_pin('d:6:o') # Solenoid valve normally closed, connected to digital pin 6
pump = board.get_pin('d:7:o') # Solenoid micro pump with an internal volume of 20 microliter and rated for a maximum pumping rate of 2.4 ml/min or 40 microliter/sec, connected to digital pin 7
print(" ")
print("--------------------------------------------------------------------------------------------------")
print(" PepSy ")
print("--------------------------------------------------------------------------------------------------")
print(" ")
print(datetime.datetime.now().strftime('%m-%d-%Y %I:%M:%S %p'))
print(" ")
seqfile = input("Enter the sequence configuration file name ")
On linux it is typically:
/dev/ttyUSBO
but the Arduino IDE should tell you where you should mount the arduino from.
"""
print __doc__
from SimpleCV import *
import sys, curses, time
from pyfirmata import Arduino, util
board = Arduino('/dev/ttyUSB0') #the location of the arduino
analog_pin_1 = board.get_pin('a:1:i') #use pin 1 of the arduino as input
analog_pin_2 = board.get_pin('a:2:i') #use pin 2 of the arduino as input
button_13 = board.get_pin('d:13:i') #use pin 13 of the arduino for button input
it = util.Iterator(board) # initalize the pin monitor for the arduino
it.start() # start the pin monitor loop
multiplier = 400.0 # a value to adjust the edge threshold by
cam = Camera() #initalize the camera
while True:
t1 = analog_pin_1.read() # read the value from pin 1
t2 = analog_pin_2.read() # read the value from pin 2
b13 = button_13.read() # read if the button has been pressed.
if not t1: #Set a default if no value read
class Controls(object):
iterator = ''
board = ''
state = ''
servo = None
#servo setup
fwheel = None
fwheel_pos1 = 165
fwheel_pos2 = 115
fwheel_pos3 = 51
fwheel_pos4 = 25
fwheel_pos5 = 0
rwheel = None
rwheel_pos1 = 110
rwheel_pos2 = 137
rwheel_pos3 = 165
awheel_right = 0
awheel_stop = 158
awheel_left = 180
SLEEPTIME = 1
aggtime = 15
ROTATE_TIME = 1
def firecolor(self, state, color):
if state == True and self.state == 'waitforbutton':
print color, " button pressed"
self.state = "inspect"
self.matchcolor = color
self.starttime = datetime.datetime.utcnow()
def fire_green(self, state):
self.firecolor(state, "green")
def fire_yellow(self, state):
self.firecolor(state, "yellow")
def fire_orange(self, state):
self.firecolor(state, "orange")
def fire_red(self, state):
self.firecolor(state, "red")
def servo_initialize(self):
self.fwheel.write(self.fwheel_pos2)
self.rwheel.write(self.rwheel_pos3)
time.sleep(self.SLEEPTIME)
def servo_inspection(self):
self.fwheel.write(self.fwheel_pos3 - 10)
time.sleep(0.2)
self.fwheel.write(self.fwheel_pos1)
self.rwheel.write(self.rwheel_pos3)
time.sleep(self.SLEEPTIME)
def servo_bad(self):
self.fwheel.write(self.fwheel_pos3)
self.rwheel.write(self.rwheel_pos1)
time.sleep(self.SLEEPTIME)
def servo_notgood(self):
self.fwheel.write(self.fwheel_pos4)
self.rwheel.write(self.rwheel_pos2)
time.sleep(self.SLEEPTIME)
def servo_good(self):
self.fwheel.write(self.fwheel_pos5)
time.sleep(0.1)
self.fwheel.write(self.fwheel_pos5+10)
time.sleep(0.05)
self.fwheel.write(self.fwheel_pos5)
self.rwheel.write(self.rwheel_pos3)
time.sleep(self.SLEEPTIME)
def servo_mix(self):
self.awheel.write(self.awheel_right)
time.sleep(self.ROTATE_TIME)
self.awheel.write(self.awheel_left)
time.sleep(self.ROTATE_TIME * 3)
self.awheel.write(self.awheel_stop)
def __init__(self, config, SS):
from pyfirmata import Arduino, util, SERVO
self.SS = SS
self.board = Arduino(config['board'])
self.iterator = util.Iterator(self.board)
self.iterator.daemon = True
self.iterator.start()
self.fwheel = self.board.get_pin('d:11:p')
self.fwheel.mode = SERVO
self.rwheel = self.board.get_pin('d:10:p')
self.rwheel.mode = SERVO
self.awheel = self.board.get_pin('d:3:p')
self.awheel.mode = SERVO
self.awheel.write(self.awheel_stop)
self.state = 'start'
self.controlobjects = [
ControlObject(9, self, [self.fire_red]),
ControlObject(8, self, [self.fire_orange]),
ControlObject(12, self, [self.fire_yellow]),
ControlObject(13, self, [self.fire_green])
]
self.colormatch_measurement = M.Measurement.objects(method="closestcolorml")[0]
self.timesince_measurement = M.Measurement.objects(method="timesince_manual")[0]
self.deliveredcolor_measurement = M.Measurement.objects(method="closestcolor_manual")[0]
self.region_inspection = M.Inspection.objects[0]
self.SS = SS
if not "debug" in config:
self.cw = ControlWatcher()
self.cw.control = self
self.cw.daemon = True
self.cw.start()
from pyfirmata import Arduino, util
import pygame
import sys, traceback
from pykalman import KalmanFilter
board = Arduino('/dev/ttyUARM')
#board = Arduino('/dev/ttyUARM_W')
clock = pygame.time.Clock()
it = util.Iterator(board)
it.start()
board.analog[0].enable_reporting()
board.analog[1].enable_reporting()
board.analog[2].enable_reporting()
board.analog[3].enable_reporting()
pin4 = board.get_pin('d:4:o')
pin6 = board.get_pin('d:6:o')
pin9 = board.get_pin('d:9:o')
pin10 = board.get_pin('d:10:s')
pin11 = board.get_pin('d:11:s')
pin12 = board.get_pin('d:12:s')
pin13 = board.get_pin('d:13:s')
#pin9 = board.get_pin('a:9:o')
pin14 = board.get_pin('a:0:o')
pin15 = board.get_pin('a:1:o')
pin16 = board.get_pin('a:2:o')
pin17 = board.get_pin('a:3:o')
pin9.write(0)
(sid, hbtimeout, ctimeout) = handshake(HOSTNAME, PORT) #handshaking according to socket.io spec.
except Exception as e:
print e
sys.exit(1)
ws = websocket.create_connection("ws://%s:%d/socket.io/1/websocket/%s" % (HOSTNAME, PORT, sid))
# handshake this player with the server
playerID = uuid.uuid4();
ws.send('5:1::{"name":"handshake", "args":"player|' + str(playerID) + '"}')
board = Arduino("COM6")
it = util.Iterator(board)
it.start()
button1Pin = board.get_pin('d:2:i')
button1Pin.enable_reporting()
button2Pin = board.get_pin('d:3:i')
button2Pin.enable_reporting()
ledPin = board.get_pin('d:13:o')
while 1:
b1Val = button1Pin.read()
b2Val = button2Pin.read()
if b1Val == True:
print("button 1 click")
ws.send('5:1::{"name":"buttonClick", "args":"' + str(playerID) + '|button1"}')
#!/usr/bin/python
from pyfirmata import Arduino, util
# para buscar el puerto ls /dev/tty.*
import time
if __name__ == '__main__':
try :
board = Arduino('/dev/tty.usbserial-A400eMcd')
pin10 = board.get_pin('d:10:p')
pin11 = board.get_pin('d:11:p')
def rampaMotor1 (rampa_) :
rampaLen = len(rampa_)
pwm = 0
delay = 0
for x in xrange(0,rampaLen):
#encendido de luz 1
if x == 0 :
board.digital[6].write(1)
#apagado de luz 1 con delay
elif x == (rampaLen - 1) :
time.sleep(rampa_[rampaLen-1] + 0.2)
board.digital[6].write(0)
from socket import * #import the socket library
##let's set up some constants
HOST = '' #we are the host
PORT = 29876 #arbitrary port not currently in use
ADDR = (HOST,PORT) #we need a tuple for the address
BUFSIZE = 256 #reasonably sized buffer for data
response = "HTTP/1.1 200 OK\n\rConnection: close\n\r\n\r"
led=board.digital[13];
EN1=board.get_pin('d:9:p');
IN1=board.digital[2];
IN2=board.digital[3];
EN2=board.get_pin('d:10:p');
IN3=board.digital[5];
IN4=board.digital[7];
GAIN=25.0;
def catch(recievedata):
button=0;
angle=0;
num=0;
num = recievedata.find(',');
button=int(recievedata[0:num]);
from pyfirmata import Arduino, util
import ImageGrab
import Image
import time
board = Arduino("COM3")
blue = board.get_pin("d:9:p")
green = board.get_pin("d:10:p")
red = board.get_pin("d:11:p")
while True:
im = ImageGrab.grab()
im.resize((5, 5), Image.NEAREST)
pix = im.load()
(r, g, b) = pix[2, 2]
red.write(r / 255.0)
green.write(g / 255.0)
blue.write(b / 255.0)
time.sleep(1)
if args.v:
v = board.get_firmata_version()
try:
print("{0}.{1}".format(v[0], v[1]))
exit(0)
except (NameError, TypeError):
print("could not get board firmata version")
exit(1)
# handle configuration file
if args.f:
config = Config(args.f)
else:
config = Config()
# turn off board led
led = board.get_pin('d:13:o')
led.write(0)
# configuring pins
ch1 = Channel(board.get_pin('d:9:o'), config.get_boardname(1))
ch2 = Channel(board.get_pin('d:8:o'), config.get_boardname(2))
ch3 = Channel(board.get_pin('d:7:o'), config.get_boardname(3))
ch4 = Channel(board.get_pin('d:6:o'), config.get_boardname(4))
channels = {'1': ch1, '2': ch2, '3': ch3, '4': ch4}
# start shell
signal.signal(signal.SIGINT, Sh.handle_sigint)
Sh().cmdloop()
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0, # type_mask (not used)
0, 0, 0, # x, y, z positions (not used)
0, 0, 0, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
yaw, 0.2) # yaw, yaw_rate (not used)
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()
board = Arduino('/dev/ttyACM99')
time.sleep(1)
rightSonar = board.get_pin('a:1:i')
frontSonar = board.get_pin('a:2:i')
leftSonar = board.get_pin('a:3:i')
it = util.Iterator(board)
it.start()
rightSonar.enable_reporting()
frontSonar.enable_reporting()
leftSonar.enable_reporting()
api = local_connect()
v = api.get_vehicles()[0]
print "Mode: %s" % v.mode
print "Armed: %s" % v.armed
print "groundspeed: %s" % v.groundspeed
Once they are installed you also have to verify the USB location of the Arduino.
On linux it is typically: /dev/ttyUSBO or /dev/ttyACM0
but the Arduino IDE should tell you where you should mount the Arduino from.
"""
print __doc__
import time
from simplecv.api import Camera
from pyfirmata import Arduino, util
board = Arduino('/dev/ttyUSB0') # The location of the Arduino
analog_pin_1 = board.get_pin('a:1:i') # Use pin 1 as input
analog_pin_2 = board.get_pin('a:2:i') # Use pin 2 as input
button_13 = board.get_pin('d:13:i') # Use pin 13 for button input
it = util.Iterator(board) # Initalize the pin monitor for the Arduino
it.start() # Start the pin monitor loop
multiplier = 400.0 # A value to adjust the edge threshold by
cam = Camera() # Initalize the camera
while True:
t1 = analog_pin_1.read() # Read the value from pin 1
t2 = analog_pin_2.read() # Read the value from pin 2
b13 = button_13.read() # Read if the button has been pressed.
if not t1: # Set a default if no value read
import time
from pyfirmata import Arduino, util
board = Arduino('/dev/tty.usbmodem411')
led = board.get_pin('d:13:o')
while True:
led.write(1)
time.sleep(.1)
led.write(0)
time.sleep(.1)
led.write(1)
time.sleep(1)
led.write(0)
time.sleep(1)
from pyfirmata import Arduino, util
import time
board = Arduino('/dev/cu.wch')
print "yo"
#pin9 = board.get_pin('d:10:s')
pin10 = board.get_pin('d:10:s')
board.digital[13].write(1)
while 1:
pin10.write(90)
time.sleep(1)
pin10.write(70)
time.sleep(1)
print "yeah"
class Uarm(object):
uarm = None
kAddrOffset = 90
kAddrAandB = 60
uarm_status = 0
pin2_status = 0
coord = {}
g_interpol_val_arr = {}
angle = {}
attachStaus = 0
def __init__(self,port):
self.uarm = Arduino(port)
self.uarmDetach()
def servoAttach(self,servo_number):
if servo_number == 1:
self.servo_base = self.uarm.get_pin('d:11:s')
elif servo_number == 2:
self.servo_left = self.uarm.get_pin('d:13:s')
elif servo_number == 3:
self.servo_right = self.uarm.get_pin('d:12:s')
elif servo_number == 4:
self.servo_end = self.uarm.get_pin('d:10:s')
else:
return
def servoDetach(self,servo_number):
if servo_number == 1:
self.uarm.servoDetach(11)
elif servo_number == 2:
self.uarm.servoDetach(13)
elif servo_number == 3:
self.uarm.servoDetach(12)
elif servo_number == 4:
self.uarm.servoDetach(10)
else:
return
def uarmDisconnect(self):
self.uarm.exit()
def uarmAttach(self):
curAngles = {}
if self.uarm_status == 0:
for n in range(1,5):
curAngles[n] = self.readAngle(n)
time.sleep(0.1)
n = 1
while n<5:
self.servoAttach(n)
n += 1
time.sleep(0.1)
self.writeAngle(curAngles[1],curAngles[2],curAngles[3],curAngles[4])
self.uarm_status = 1
def uarmDetach(self):
n = 1
while n<5:
self.servoDetach(n)
n += 1
self.uarm_status = 0
def angleConstrain(self,Angle):
if Angle <0:
return 0
elif Angle >180:
return 180
else:
return Angle
def writeServoAngleRaw(self,servo_number,Angle):
if servo_number == 1:
self.servo_base.write(self.angleConstrain(round(Angle)))
elif servo_number == 2:
self.servo_left.write(self.angleConstrain(round(Angle)))
elif servo_number == 3:
self.servo_right.write(self.angleConstrain(round(Angle)))
elif servo_number == 4:
self.servo_end.write(self.angleConstrain(round(Angle)))
else:
return
def writeServoAngle(self,servo_number,Angle):
if servo_number == 1:
self.servo_base.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number)))
elif servo_number == 2:
self.servo_left.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number)))
elif servo_number == 3:
self.servo_right.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number)))
elif servo_number == 4:
self.servo_end.write(self.angleConstrain(Angle+ self.readServoOffset(servo_number)))
else:
return
def writeAngle(self,servo_1,servo_2,servo_3,servo_4):
servoAngles = {}
servoAngles[1] = servo_1 + self.readServoOffset(1)
servoAngles[2] = servo_2 + self.readServoOffset(2)
servoAngles[3] = servo_3 + self.readServoOffset(3)
servoAngles[4] = servo_4 + self.readServoOffset(4)
self.servo_base.write(self.angleConstrain(servoAngles[1]))
self.servo_left.write(self.angleConstrain(servoAngles[2]))
self.servo_right.write(self.angleConstrain(servoAngles[3]))
self.servo_end.write(self.angleConstrain(servoAngles[4]))
def writeAngleRaw(self,servo_1,servo_2,servo_3,servo_4):
self.servo_base.write(self.angleConstrain(servo_1))
self.servo_left.write(self.angleConstrain(servo_2))
self.servo_right.write(self.angleConstrain(servo_3))
self.servo_end.write(self.angleConstrain(servo_4))
def readAnalog(self,servo_number):
if servo_number == 1:
for i in range(1,4):
data = self.uarm.readAnalogPin(2)
return data
elif servo_number == 2:
for i in range(1,4):
data = self.uarm.readAnalogPin(0)
return data
elif servo_number == 3:
for i in range(1,4):
data = self.uarm.readAnalogPin(1)
return data
elif servo_number == 4:
for i in range(1,4):
data = self.uarm.readAnalogPin(3)
return data
else:
return
def readServoOffset(self,servo_number):
if servo_number ==1 or servo_number ==2 or servo_number ==3:
addr = self.kAddrOffset + (servo_number - 1)*2
servo_offset = (self.uarm.readEEPROM(addr+1))/10.00
if(self.uarm.readEEPROM(addr) == 0):
servo_offset *= -1
return servo_offset
elif servo_number == 4:
return 0
else:
pass
def readToAngle(self,input_analog,servo_number,tirgger):
addr = self.kAddrAandB + (servo_number-1)*6
data_a = (self.uarm.readEEPROM(addr+1)+ (self.uarm.readEEPROM(addr+2)*256))/10.0
if (self.uarm.readEEPROM(addr) == 0):
data_a = -data_a
data_b = (self.uarm.readEEPROM(addr+4)+ (self.uarm.readEEPROM(addr+5)*256))/100.0
if (self.uarm.readEEPROM(addr+3) == 0):
data_b = -data_b
if tirgger == 0 :
return (data_a + data_b *input_analog) - self.readServoOffset(servo_number)
elif tirgger == 1:
return (data_a + data_b *input_analog)
else:
pass
def fwdKine(self,theta_1,theta_2,theta_3):
g_l3_1 = MATH_L3 * math.cos(theta_2/MATH_TRANS)
g_l4_1 = MATH_L4 * math.cos(theta_3 / MATH_TRANS);
g_l5 = (MATH_L2 + MATH_L3*math.cos(theta_2 / MATH_TRANS) + MATH_L4*math.cos(theta_3 / MATH_TRANS));
self.coord[1] = -math.cos(abs(theta_1 / MATH_TRANS))*g_l5;
self.coord[2] = -math.sin(abs(theta_1 / MATH_TRANS))*g_l5;
self.coord[3] = MATH_L1 + MATH_L3*math.sin(abs(theta_2 / MATH_TRANS)) - MATH_L4*math.sin(abs(theta_3 / MATH_TRANS));
return self.coord
def currentCoord(self):
return self.fwdKine(self.readAngle(1),self.readAngle(2),self.readAngle(3))
def currentX(self):
self.currentCoord()
return self.coord[1]
def currentY(self):
self.currentCoord()
return self.coord[2]
def currentZ(self):
self.currentCoord()
return self.coord[3]
def readAngle(self,servo_number):
if servo_number == 1:
return self.readToAngle(self.readAnalog(1),1,0)
elif servo_number == 2:
return self.readToAngle(self.readAnalog(2),2,0)
elif servo_number == 3:
return self.readToAngle(self.readAnalog(3),3,0)
elif servo_number == 4:
return self.readToAngle(self.readAnalog(4),4,0)
else:
pass
def readAngleRaw(self,servo_number):
if servo_number == 1:
return self.readToAngle(self.readAnalog(1),1,1)
elif servo_number == 2:
return self.readToAngle(self.readAnalog(2),2,1)
elif servo_number == 3:
return self.readToAngle(self.readAnalog(3),3,1)
elif servo_number == 4:
return self.readToAngle(self.readAnalog(4),4,1)
else:
pass
def interpolation(self,init_val,final_val):
#by using the formula theta_t = l_a_0 + l_a_1 * t + l_a_2 * t^2 + l_a_3 * t^3
#theta(0) = init_val; theta(t_f) = final_val
#theta_dot(0) = 0; theta_dot(t_f) = 0
l_time_total = 1
l_a_0 = init_val;
l_a_1 = 0;
l_a_2 = (3 * (final_val - init_val)) / (l_time_total*l_time_total);
l_a_3 = (-2 * (final_val - init_val)) / (l_time_total*l_time_total*l_time_total);
i = 0
while i<51:
l_t_step = (l_time_total / 50.0) *i
self.g_interpol_val_arr[i] = l_a_0 + l_a_1 * (l_t_step) + l_a_2 * (l_t_step *l_t_step ) + l_a_3 * (l_t_step *l_t_step *l_t_step);
i+=1
return self.g_interpol_val_arr
def pumpOn(self):
self.uarm.digital[PUMP_EN].write(1)
self.uarm.digital[VALVE_EN].write(0)
def pumpOff(self):
self.uarm.digital[PUMP_EN].write(0)
self.uarm.digital[VALVE_EN].write(1)
time.sleep(0.02)
self.uarm.digital[VALVE_EN].write(0)
def ivsKine(self, x, y, z):
if z > (MATH_L1 + MATH_L3 + TopOffset):
z = MATH_L1 + MATH_L3 + TopOffset
if z < (MATH_L1 - MATH_L4 + BottomOffset):
z = MATH_L1 - MATH_L4 + BottomOffset
g_y_in = (-y-MATH_L2)/MATH_L3
g_z_in = (z - MATH_L1) / MATH_L3
g_right_all = (1 - g_y_in*g_y_in - g_z_in*g_z_in - MATH_L43*MATH_L43) / (2 * MATH_L43)
g_sqrt_z_y = math.sqrt(g_z_in*g_z_in + g_y_in*g_y_in)
if x == 0:
# Calculate value of theta 1
g_theta_1 = 90;
# Calculate value of theta 3
if g_z_in == 0:
g_phi = 90
else:
g_phi = math.atan(-g_y_in / g_z_in)*MATH_TRANS
if g_phi > 0:
g_phi = g_phi - 180
g_theta_3 = math.asin(g_right_all / g_sqrt_z_y)*MATH_TRANS - g_phi
if g_theta_3<0:
g_theta_3 = 0
# Calculate value of theta 2
g_theta_2 = math.asin((z + MATH_L4*math.sin(g_theta_3 / MATH_TRANS) - MATH_L1) / MATH_L3)*MATH_TRANS
else:
# Calculate value of theta 1
g_theta_1 = math.atan(y / x)*MATH_TRANS
if (y/x) > 0:
g_theta_1 = g_theta_1
if (y/x) < 0:
g_theta_1 = g_theta_1 + 180
if y == 0:
if x > 0:
g_theta_1 = 180
else:
g_theta_1 = 0
# Calculate value of theta 3
g_x_in = (-x / math.cos(g_theta_1 / MATH_TRANS) - MATH_L2) / MATH_L3;
if g_z_in == 0:
g_phi = 90
else:
g_phi = math.atan(-g_x_in / g_z_in)*MATH_TRANS
if g_phi > 0:
g_phi = g_phi - 180
g_sqrt_z_x = math.sqrt(g_z_in*g_z_in + g_x_in*g_x_in)
g_right_all_2 = -1 * (g_z_in*g_z_in + g_x_in*g_x_in + MATH_L43*MATH_L43 - 1) / (2 * MATH_L43)
g_theta_3 = math.asin(g_right_all_2 / g_sqrt_z_x)*MATH_TRANS
g_theta_3 = g_theta_3 - g_phi
if g_theta_3 <0 :
g_theta_3 = 0
# Calculate value of theta 2
g_theta_2 = math.asin(g_z_in + MATH_L43*math.sin(abs(g_theta_3 / MATH_TRANS)))*MATH_TRANS
g_theta_1 = abs(g_theta_1);
g_theta_2 = abs(g_theta_2);
if g_theta_3 < 0 :
pass
else:
self.fwdKine(g_theta_1,g_theta_2, g_theta_3)
if (self.coord[2]>y+0.1) or (self.coord[2]<y-0.1):
g_theta_2 = 180 - g_theta_2
if(math.isnan(g_theta_1) or math.isinf(g_theta_1)):
g_theta_1 = self.readAngle(1)
if(math.isnan(g_theta_2) or math.isinf(g_theta_2)):
g_theta_2 = self.readAngle(2)
if(math.isnan(g_theta_3) or math.isinf(g_theta_3) or (g_theta_3<0)):
g_theta_3 = self.readAngle(3)
self.angle[1] = g_theta_1
self.angle[2] = g_theta_2
self.angle[3] = g_theta_3
return self.angle
pass
def moveToWithS4(self,x,y,z,timeSpend,servo_4_relative,servo_4):
if self.uarm_status == 0:
self.uarmAttach()
self.uarm_status = 1
curXYZ = self.currentCoord()
x_arr = {}
y_arr = {}
z_arr = {}
if time >0:
self.interpolation(curXYZ[1],x)
for n in range(0,50):
x_arr[n] = self.g_interpol_val_arr[n]
self.interpolation(curXYZ[2],y)
for n in range(0,50):
y_arr[n] = self.g_interpol_val_arr[n]
self.interpolation(curXYZ[3],z)
for n in range(0,50):
z_arr[n] = self.g_interpol_val_arr[n]
for n in range(0,50):
self.ivsKine(x_arr[n],y_arr[n],z_arr[n])
self.writeAngle(self.angle[1],self.angle[2],self.angle[3],self.angle[1]*servo_4_relative+servo_4)
time.sleep(timeSpend/50.0)
elif time == 0:
self.ivsKine(x,y,z)
self.writeAngle(self.angle[1],self.angle[2],self.angle[3],self.angle[1]*servo_4_relative+servo_4)
else:
pass
def moveTo(self,x,y,z):
if self.uarm_status == 0:
self.uarmAttach()
self.uarm_status = 1
curXYZ = self.currentCoord()
x_arr = {}
y_arr = {}
z_arr = {}
self.interpolation(curXYZ[1],x)
for n in range(0,50):
x_arr[n] = self.g_interpol_val_arr[n]
self.interpolation(curXYZ[2],y)
for n in range(0,50):
y_arr[n] = self.g_interpol_val_arr[n]
self.interpolation(curXYZ[3],z)
for n in range(0,50):
z_arr[n] = self.g_interpol_val_arr[n]
for n in range(0,50):
self.ivsKine(x_arr[n],y_arr[n],z_arr[n])
self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0)
time.sleep(0.04)
def moveToWithTime(self,x,y,z,timeSpend):
if self.uarm_status == 0:
self.uarmAttach()
self.uarm_status = 1
curXYZ = self.currentCoord()
x_arr = {}
y_arr = {}
z_arr = {}
if time >0:
self.interpolation(curXYZ[1],x)
for n in range(0,50):
x_arr[n] = self.g_interpol_val_arr[n]
self.interpolation(curXYZ[2],y)
for n in range(0,50):
y_arr[n] = self.g_interpol_val_arr[n]
self.interpolation(curXYZ[3],z)
for n in range(0,50):
z_arr[n] = self.g_interpol_val_arr[n]
for n in range(0,50):
self.ivsKine(x_arr[n],y_arr[n],z_arr[n])
self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0)
time.sleep(timeSpend/50.0)
elif time == 0:
self.ivsKine(x,y,z)
self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0)
else:
pass
def moveToAtOnce(self,x,y,z):
if self.uarm_status == 0:
self.uarmAttach()
self.uarm_status = 1
self.ivsKine(x,y,z)
self.writeAngle(self.angle[1],self.angle[2],self.angle[3],0)
def moveRelative(self,x,y,z,time,servo_4_relative,servo_4):
pass
def stopperStatus(self):
val = self.uarm.pumpStatus(0)
if val ==2 or val ==1:
return val-1
else:
print 'ERROR: Stopper is not deteceted'
return -1
# Open Serial Connection to Firmata on Arduino
#
print "Intializing Arduino connection ..."
# initialize pyfirmata arduino instance
board = Arduino('COM6')
# get major/minor from the Arduino slave and output to console
firmata_version = board.get_firmata_version()
print "Connected to Firmata " + str(firmata_version[0]) + "." + str(firmata_version[1])
######################################################################
# Set digital pins to PWM mode
#
# RGB LED 1 PWM
r1p = board.get_pin('d:3:p')
g1p = board.get_pin('d:5:p')
b1p = board.get_pin('d:6:p')
#
# RGB LED 2 PWM
# Set digital pins to PWM mode
r2p = board.get_pin('d:9:p')
g2p = board.get_pin('d:10:p')
b2p = board.get_pin('d:11:p')
######################################################################
# Main Loop
#
try:
######################################################################
# Loop Forever
#!/usr/bin/python
#import sys
import socket
#sys.path.insert(0, '/usr/lib/python2.7/bridge/')
#from bridgeclient import BridgeClient as bridgeclient
from pyfirmata import Arduino, util
from time import sleep
#value = bridgeclient()
board = Arduino('/dev/ttyATH0', baudrate=115200)
pin9 = board.get_pin('d:9:s')
pin10 = board.get_pin('d:10:s')
UDP_IP = "192.168.240.1"
UDP_PORT = 5005
sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
pin9.write(40)
sleep(0.5)
pin9.write(140)
sleep(0.5)
pin9.write(90)
sleep(0.5)
#print 'server is running...'
