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()
def main():
args = get_arguments()
log('Initializing....')
board = Arduino(args.arduino_device)
log('Arduino connected to: {}'.format(args.arduino_device))
initialize_arduino_reading(board, args.analog_ports)
client = None
if args.influxdb_host:
client = InfluxDBClient(args.influxdb_host, 8086, 'root', 'root',
args.database)
client.create_database(args.database)
points = []
for port in args.analog_ports:
port_read = board.analog[port].read()
log('Port {}: {}'.format(port, port_read))
if client:
time = datetime.datetime.utcnow().strftime(TIME_FORMAT)
port_name = 'A{}'.format(port)
point = dict(measurement='humidity_sensor',
time=time,
fields={'value': port_read},
tags={'sensor_port': port_name})
log(point)
points.append(point)
if client and points:
client.write_points(points)
board.exit()
log('Connection with board has been closed')
log('Finish')
def main():
parser = argparse.ArgumentParser()
parser.add_argument("-s", "--serial", default="ttyACM0", type=str,
help="Arduino serial port [ttyACM0] (OSX is cu.usbmodemXXXX)")
cli.grpc_host(parser)
parser.add_argument("-x", "--xservo", default=5, type=int, help="X servo PWM pin [5]")
parser.add_argument("-y", "--yservo", default=6, type=int, help="Y servo PWM pin [6]")
cli.alternate(parser)
cli.calib(parser)
cli.log_level(parser)
args = vars(parser.parse_args())
alternate = args["alternate"]
calib = args["calib"]
xservo = args["xservo"]
yservo = args["yservo"]
setup_logging(level=args[LOG_LEVEL])
# Setup firmata client
port = ("" if is_windows() else "/dev/") + args["serial"]
try:
board = Arduino(port)
logger.info("Connected to Arduino at: {0}".format(port))
except OSError as e:
logger.error("Failed to connect to Arduino at {0} - [{1}]".format(port, e))
sys.exit(0)
with LocationClient(args[GRPC_HOST]) as client:
# Create servos
servo_x = FirmataServo("Pan", alternate, board, "d:{0}:s".format(xservo), 1.0, 8)
servo_y = FirmataServo("Tilt", alternate, board, "d:{0}:s".format(yservo), 1.0, 8)
try:
if calib:
try:
calib_t = Thread(target=calibrate_servo.calibrate, args=(client, servo_x, servo_y))
calib_t.start()
calib_t.join()
except KeyboardInterrupt:
pass
else:
if alternate:
# Set servo X to go first
servo_x.ready_event.set()
try:
servo_x.start(True, lambda: client.get_x(), servo_y.ready_event if not calib else None)
servo_y.start(False, lambda: client.get_y(), servo_x.ready_event if not calib else None)
servo_x.join()
servo_y.join()
except KeyboardInterrupt:
pass
finally:
servo_x.stop()
servo_y.stop()
finally:
board.exit()
logger.info("Exiting...")
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 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()
class ArduinoUnoHandler:
board = None
connected = 'Disconnected'
di_states = ['0' for i in range(12)]
thread = None
def connect(self, com_port):
try:
self.board = Arduino(com_port)
thread = util.Iterator(self.board)
thread.start()
for i in range(12):
self.board.digital[i + 2].mode = 0
for i in range(6):
self.board.analog[i].enable_reporting()
self.connected = 'Connected'
return True
except:
self.connected = 'Error'
return False
def disconnect(self):
try:
self.board.exit()
self.connected = 'Disconnected'
return True
except:
self.connected = 'Error'
return False
def get_digital(self, pin):
if self.board.digital[pin].mode == 0:
return not self.board.digital[pin].read()
else:
return False
def set_digital(self, pin, val):
if self.board.digital[pin].mode == 1:
self.board.digital[pin].write(val)
def set_mode(self, pin, mode):
self.board.digital[pin].mode = mode
def get_analog(self, pin):
return self.board.analog[pin].read()
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()
class ArduinoConnection(ConnectionBase):
def __init__(self, connection_type, port_name):
super().__init__()
self.connectionStatus = ConnectionStatus.Closed
self.connectionType = connection_type
self.portName = port_name
self._iterator = None
def open_connection(self):
if self.connectionStatus == ConnectionStatus.Open:
print(self.connectionStatus)
return None
try:
if self.board is None:
print(
"Initializing board object specified connected to port {} ..."
.format(self.portName))
self.board = Arduino(self.portName)
print("Board object created!")
if self._iterator is None:
print(
"Initializing iterator object to prevent overflow serial buffer ..."
)
self._iterator = util.Iterator(self.board)
print("Iterator object created!")
self._iterator.start()
self.connectionStatus = ConnectionStatus.Open
print("{}, {} , {} ".format(self.connectionStatus,
self.connectionType, self.portName))
except serial.SerialException as e:
print(e.args[0])
if self.board is not None:
self.board.exit()
raise SystemExit
def close_connection(self):
if self.connectionStatus == ConnectionStatus.Closed:
print("Connection is already closed!")
else:
self.board.exit()
self.board = None
self._iterator = None
self.connectionStatus = ConnectionStatus.Closed
print(self.connectionStatus)
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()
class ArduinoConnection(ConnectionBase):
def __init__(self, connection_type, port_name):
super().__init__()
self.connectionStatus = ConnectionStatus.Closed
self.connectionType = connection_type
self.portName = port_name
self._iterator = None
def open_connection(self):
if self.connectionStatus == ConnectionStatus.Open:
print(self.connectionStatus)
return None
try:
if self.board is None:
print("Initializing board object specified connected to port {} ...".format(self.portName))
self.board = Arduino(self.portName)
print("Board object created!")
if self._iterator is None:
print("Initializing iterator object to prevent overflow serial buffer ...")
self._iterator = util.Iterator(self.board)
print("Iterator object created!")
self._iterator.start()
self.connectionStatus = ConnectionStatus.Open
print("{}, {} , {} ".format(self.connectionStatus, self.connectionType, self.portName))
except serial.SerialException as e:
print(e.args[0])
if self.board is not None:
self.board.exit()
raise SystemExit
def close_connection(self):
if self.connectionStatus == ConnectionStatus.Closed:
print("Connection is already closed!")
else:
self.board.exit()
self.board = None
self._iterator = None
self.connectionStatus = ConnectionStatus.Closed
print(self.connectionStatus)
def iterate_config_list(config_df,port_id):
board = Arduino(port_id)
iterator = util.Iterator(board)
iterator.start()
for index, row in config_df.iterrows():
voltage = board.get_pin(row['pin_number'])
time.sleep(.3)
voltage=voltage.read()
config_df.at[index,'environmental_value'] = translate(voltage,0,100,row['sensor_low'],row['sensor_high'])
column_names = ['plant_id', 'measurement_timestamp', 'environmental_dimension', 'environmental_value']
df_row = pd.DataFrame(columns=column_names)
dict = {'plant_id': row['plant_id'],
'measurement_timestamp': datetime.datetime.fromtimestamp(time.time()).strftime('%Y-%m-%d %H:%M:%S'),
'environmental_dimension': row['environmental_dimension'],
'environmental_value': row['environmental_value']
}
r = requests.post('https://pauls-garden.herokuapp.com/api/plant_measurement/', json=dict)
print(r.json())
board.exit()
def serial_ports():
""" Lists serial port names
:raises EnvironmentError:
On unsupported or unknown platforms
:returns:
A list of the serial ports available on the system
"""
if sys.platform.startswith('win'):
ports = ['COM%s' % (i + 1) for i in range(256)]
elif sys.platform.startswith('linux') or sys.platform.startswith('cygwin'):
# this excludes your current terminal "/dev/tty"
ports = [
"/dev/" + os.popen("dmesg | egrep ttyACM | cut -f3 -d: | tail -n1"
).read().strip()
]
elif sys.platform.startswith('darwin'):
ports = glob.glob('/dev/tty.*')
else:
raise EnvironmentError('Unsupported platform')
result = []
for port in ports:
try:
board = Arduino(port)
board.exit()
result.append(port)
except (OSError, serial.SerialException):
print(
"Please try in the terminal:\n" + "1. ls -l /dev/ttyACM*\n" +
"2. You will get something like: crw-rw---- 1 root dialout 188, 0 5 apr 23.01 ttyACM0\n"
+
"3. The '0' at the end of ACM might be a different number, or multiple entries might be returned. \n"
+
"The data we need is 'dialout' (is the group owner of the file).\n"
+ "4. sudo usermod -a -G dialout $USER")
return result
def main():
parser = ArgumentParser(
description='Bridge connecting Arduino and hat webapp')
parser.add_argument('-s', '--server', default='127.0.0.1:3000',
help='Server the webapp is running on')
parser.add_argument('device',
help='Path to Arduino device, e.g., /dev/tty/ACM0')
args = parser.parse_args()
url = ddp.ServerUrl(args.server)
board = None
hat_controller = None
try:
print 'Connecting to Arduino board...'
board = Arduino(args.device)
print 'Connecting to DDP server...'
hat_controller = HatController(url, board)
hat_controller.connect()
wait_for_user_to_exit()
finally:
if hat_controller is not None:
try:
hat_controller.disconnect()
except:
print (
'An error occurred while disconnecting from the DDP '
'server.'
)
if board is not None:
try:
board.exit()
except:
print 'An error occurred while exiting from the Arduino board.'
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
class Form(QDialog):
def __init__(self, app, parent=None):
super(Form, self).__init__(parent)
# Pyfirmata code
self.potPin = 0
self.servoPin = 9
self.ledPin = 13
self.board = Arduino("/dev/ttyACM0")
iterator = util.Iterator(self.board)
iterator.start()
self.board.analog[self.potPin].enable_reporting()
# self.board.digital[self.servoPin].mode = SERVO
# PyQT Code
servoLabel = QLabel("Servo")
self.servoDial = QDial()
self.servoDial.setMinimum(0)
self.servoDial.setMaximum(180)
self.servoPosition = 0
potLabel = QLabel("Potenciometro")
self.potSlider = QSlider()
self.potSlider.setMinimum(0)
self.potSlider.setMaximum(1000)
ledLabel = QLabel("Led 13")
self.ledButton = QPushButton("Light")
grid = QGridLayout()
grid.addWidget(servoLabel, 0, 0)
grid.addWidget(potLabel, 0, 1)
grid.addWidget(ledLabel, 0, 2)
grid.addWidget(self.servoDial, 1, 0)
grid.addWidget(self.potSlider, 1, 1)
grid.addWidget(self.ledButton, 1, 2)
self.setLayout(grid)
self.connect(self.ledButton, SIGNAL("pressed()"), self.ledOn)
self.connect(self.ledButton, SIGNAL("released()"), self.ledOff)
# self.connect(self.servoDial,
# SIGNAL("valueChanged(int)"), self.moveServo)
self.connect(app, SIGNAL("lastWindowClosed()"), self.cleanup)
self.timer = QTimer()
self.timer.setInterval(500)
self.connect(self.timer, SIGNAL("timeout()"), self.readPot)
self.setWindowTitle("Arduino")
self.timer.start()
def ledOn(self):
print "Led on"
self.board.digital[self.ledPin].write(1)
def ledOff(self):
print "Led off"
self.board.digital[self.ledPin].write(0)
"""
def moveServo(self):
position = self.servoDial.value()
if abs(position - self.servoPosition) > 10:
self.servoPosition = position
print "Servo at position: %s" % position
self.board.digital[self.servoPin].write(position)
"""
def cleanup(self):
print "cleanup"
self.board.exit()
def readPot(self):
value = self.board.analog[self.potPin].read()
if value is not None:
position = value * 1000
print "Read Pot: %s" % position
self.potSlider.setValue(position)
class Uarm(object):
# uarm = None
def __init__(self, port, claw=False):
self.uarm_status = 0
# self.pin2_status = 0
self.coord = {}
self.g_interpol_val_arr = {}
self.angle = {}
self.claw = claw
self.uarm = Arduino(port)
self.assignServos()
self.uarmDetach()
def assignServos(self):
self.servo_base = Servo(self.uarm, 1, 11, 2)
self.servo_left = Servo(self.uarm, 2, 13, 0)
self.servo_right = Servo(self.uarm, 3, 12, 1)
self.servo_end = Servo(self.uarm, 4, 10, 3)
self.servomap = {1: self.servo_base,
2: self.servo_left,
3: self.servo_right,
4: self.servo_end}
if self.claw:
self.servo_claw = Servo(self.uarm, 5, 9, 5)
self.servomap[5] = self.servo_claw;
def servos(self, servo_number=None):
if servo_number is None:
servo_number = list(self.servomap.keys())
servo_number = self._aslist(servo_number)
return [self.servomap[servo] for servo in servo_number]
def servoAttach(self, servo_number=None, preserveAngle=True):
for servo in self.servos(servo_number):
servo.attach(preserveAngle=preserveAngle)
def servoDetach(self, servo_number=None):
for servo in self.servos(servo_number):
servo.detach()
self.uarm_status = 0
def _aslist(self, v):
try:
return list(v)
except TypeError:
return [v]
def uarmDisconnect(self, detach=True):
if detach:
self.servoDetach()
self.uarm.exit()
def uarmAttach(self):
self.servoAttach()
self.uarm_status = 1
def uarmDetach(self):
self.servoDetach()
def angleConstrain(self, Angle):
if Angle < 0:
return 0
elif Angle > 180:
return 180
else:
return Angle
def writeServoAngleRaw(self, servo_number, angle, constrain=False):
self.servomap[servo_number].writeAngle(raw=True, constrain=constrain)
def writeServoAngle(self, servo_number, angle, constrain=True):
self.servomap[servo_number].writeAngle(raw=False, constrain=constrain)
def writeAngle(self, *args, raw=False, constrain=True):
for angle, servo in zip(args, self.servos()):
servo.writeAngle(angle, raw=raw, constrain=constrain)
def writeAngleRaw(self, *args, constrain=False):
self.writeAngle(*args, raw=True, constrain=constrain)
def readAnalog(self, servo_number=None):
if servo_number is None or not hasattr(servo_number, 'len'):
return [servo.readAnalog() for servo in self.servos(servo_number)]
else:
return self.servomap[servo_number].readAnalog()
def readServoOffset(self, servo_number):
if servo_number is None or not hasattr(servo_number, 'len'):
return [servo.readOffset() for servo in self.servos(servo_number)]
else:
return self.servomap[servo_number].readServoOffset()
def readToAngle(self, input_analog, servo_number, trigger):
return self.servomap[servo_number].readToAngle(input_analog, raw=trigger)
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, raw=False):
return self.servomap[servo_number].readAngle(raw=False)
def readAngleRaw(self, servo_number):
return self.readAngle(servo_number, raw=True)
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
class Form(QDialog):
def __init__(self, app, parent=None):
super(Form, self).__init__(parent)
# Pyfirmata code
self.potPin = 0
self.servoPin = 9
self.ledPin = 13
self.board = Arduino('/dev/ttyACM0')
iterator = util.Iterator(self.board)
iterator.start()
self.board.analog[self.potPin].enable_reporting()
#self.board.digital[self.servoPin].mode = SERVO
# PyQT Code
servoLabel = QLabel("Servo")
self.servoDial = QDial()
self.servoDial.setMinimum(0)
self.servoDial.setMaximum(180)
self.servoPosition = 0
potLabel = QLabel("Potenciometro")
self.potSlider = QSlider()
self.potSlider.setMinimum(0)
self.potSlider.setMaximum(1000)
ledLabel = QLabel("Led 13")
self.ledButton = QPushButton('Light')
grid = QGridLayout()
grid.addWidget(servoLabel, 0, 0)
grid.addWidget(potLabel, 0, 1)
grid.addWidget(ledLabel, 0, 2)
grid.addWidget(self.servoDial, 1, 0)
grid.addWidget(self.potSlider, 1, 1)
grid.addWidget(self.ledButton, 1, 2)
self.setLayout(grid)
self.connect(self.ledButton,
SIGNAL("pressed()"), self.ledOn)
self.connect(self.ledButton,
SIGNAL("released()"), self.ledOff)
#self.connect(self.servoDial,
#SIGNAL("valueChanged(int)"), self.moveServo)
self.connect(app,
SIGNAL("lastWindowClosed()"), self.cleanup)
self.timer = QTimer()
self.timer.setInterval(500)
self.connect(self.timer, SIGNAL("timeout()"), self.readPot)
self.setWindowTitle("Arduino")
self.timer.start()
def ledOn(self):
print "Led on"
self.board.digital[self.ledPin].write(1)
def ledOff(self):
print "Led off"
self.board.digital[self.ledPin].write(0)
"""
def moveServo(self):
position = self.servoDial.value()
if abs(position - self.servoPosition) > 10:
self.servoPosition = position
print "Servo at position: %s" % position
self.board.digital[self.servoPin].write(position)
"""
def cleanup(self):
print "cleanup"
self.board.exit()
def readPot(self):
value = self.board.analog[self.potPin].read()
if value is not None:
position = value * 1000
print "Read Pot: %s" % position
self.potSlider.setValue(position)
class MotorDriver(object):
def __init__(self, wheel_distance=0.098, wheel_diameter=0.066):
"""
M1 = Right Wheel
M2 = Left Wheel
:param wheel_distance: Distance Between wheels in meters
:param wheel_diameter: Diameter of the wheels in meters
"""
self.ENB = 5
self.ENA = 6
self.PWM1 = 0
self.PWM2 = 0
self.BASE_PWM = 50
self.MAX_PWM = 256
# Wheel and chasis dimensions
self._wheel_distance = wheel_distance
self._wheel_radius = wheel_diameter / 2.0
self.MULTIPLIER_STANDARD = 0.1
self.MULTIPLIER_PIVOT = 1.0
self.board = None
if os.name == 'nt':
self.board = Arduino("COM9")
else:
self.board = Arduino("/dev/ttyACM0")
self.IN1 = self.board.get_pin('d:7:o')
self.IN2 = self.board.get_pin('d:8:o')
self.IN3 = self.board.get_pin('d:11:o')
self.IN4 = self.board.get_pin('d:9:o')
# self.ENA = self.board.get_pin('d:6:p')
# self.ENB = self.board.get_pin('d:5:p')
iterator = util.Iterator(self.board)
iterator.start()
self.board.digital[self.ENA].write(1)
self.board.digital[self.ENB].write(1)
def __del__(self):
# GPIO.cleanup()
self.board.exit()
def set_motor(self, A1, A2, B1, B2):
self.IN1.write(A1)
self.IN2.write(A2)
self.IN3.write(B1)
self.IN4.write(B2)
def forward(self):
self.set_motor(1, 0, 1, 0)
def stop(self):
self.set_motor(0, 0, 0, 0)
def reverse(self):
self.set_motor(0, 1, 0, 1)
def left(self):
self.set_motor(0, 1, 0, 0)
def pivot_left(self):
self.set_motor(1, 0, 0, 1)
def right(self):
self.set_motor(0, 0, 0, 1)
def pivot_right(self):
self.set_motor(0, 1, 1, 0)
def set_M1M2_speed(self, rpm_speedM1, rpm_speedM2, multiplier):
self.set_M1_speed(rpm_speedM1, multiplier)
self.set_M2_speed(rpm_speedM2, multiplier)
def set_M1_speed(self, rpm_speed, multiplier):
self.PWM1 = min(int((rpm_speed * multiplier) * self.BASE_PWM),
self.MAX_PWM)
# if(self.PWM1 > 256):
# self.PWM1 = 256
# self.ENA.write(self.PWM1)
#self.p1.ChangeDutyCycle(self.PWM1)
print("M1=" + str(self.PWM1))
def set_M2_speed(self, rpm_speed, multiplier):
self.PWM2 = min(int(rpm_speed * multiplier * self.BASE_PWM),
self.MAX_PWM)
# if(self.PWM2 > 256):
# self.PWM1 = 256
# self.ENB.write(self.PWM2)
#self.p2.ChangeDutyCycle(self.PWM2)
print("M2=" + str(self.PWM2))
def calculate_body_turn_radius(self, linear_speed, angular_speed):
if angular_speed != 0.0:
body_turn_radius = linear_speed / angular_speed
else:
# Not turning, infinite turn radius
body_turn_radius = None
return body_turn_radius
def calculate_wheel_turn_radius(self, body_turn_radius, angular_speed,
wheel):
if body_turn_radius is not None:
"""
if angular_speed > 0.0:
angular_speed_sign = 1
elif angular_speed < 0.0:
angular_speed_sign = -1
else:
angular_speed_sign = 0.0
"""
if wheel == "right":
wheel_sign = 1
elif wheel == "left":
wheel_sign = -1
else:
assert False, "Wheel Name not supported, left or right only."
wheel_turn_radius = body_turn_radius + (
wheel_sign * (self._wheel_distance / 2.0))
else:
wheel_turn_radius = None
return wheel_turn_radius
def calculate_wheel_rpm(self, linear_speed, angular_speed,
wheel_turn_radius):
"""
Omega_wheel = Linear_Speed_Wheel / Wheel_Radius
Linear_Speed_Wheel = Omega_Turn_Body * Radius_Turn_Wheel
--> If there is NO Omega_Turn_Body, Linear_Speed_Wheel = Linear_Speed_Body
:param angular_speed:
:param wheel_turn_radius:
:return:
"""
if wheel_turn_radius is not None:
# The robot is turning
wheel_rpm = (angular_speed *
wheel_turn_radius) / self._wheel_radius
else:
# Its not turning therefore the wheel speed is the same as the body
wheel_rpm = linear_speed / self._wheel_radius
return wheel_rpm
def set_wheel_movement(self, right_wheel_rpm, left_wheel_rpm):
print("W1,W2=[" + str(right_wheel_rpm) + "," + str(left_wheel_rpm) +
"]")
if right_wheel_rpm > 0.0 and left_wheel_rpm > 0.0:
print("All forwards")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_STANDARD)
self.forward()
elif right_wheel_rpm > 0.0 and left_wheel_rpm == 0.0:
print("Right Wheel forwards, left stop")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_STANDARD)
self.left()
elif right_wheel_rpm > 0.0 and left_wheel_rpm < 0.0:
print("Right Wheel forwards, left backwards --> Pivot left")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_PIVOT)
self.pivot_left()
elif right_wheel_rpm == 0.0 and left_wheel_rpm > 0.0:
print("Right stop, left forwards")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_STANDARD)
self.right()
elif right_wheel_rpm < 0.0 and left_wheel_rpm > 0.0:
print("Right backwards, left forwards --> Pivot right")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_PIVOT)
self.pivot_right()
elif right_wheel_rpm < 0.0 and left_wheel_rpm < 0.0:
print("All backwards")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_STANDARD)
self.reverse()
elif right_wheel_rpm == 0.0 and left_wheel_rpm == 0.0:
print("Right stop, left stop")
self.set_M1M2_speed(abs(right_wheel_rpm), abs(left_wheel_rpm),
self.MULTIPLIER_STANDARD)
self.stop()
else:
assert False, "A case wasn't considered==>" + str(
right_wheel_rpm) + "," + str(left_wheel_rpm)
pass
def set_cmd_vel(self, linear_speed, angular_speed):
body_turn_radius = self.calculate_body_turn_radius(
linear_speed, angular_speed)
wheel = "right"
right_wheel_turn_radius = self.calculate_wheel_turn_radius(
body_turn_radius, angular_speed, wheel)
wheel = "left"
left_wheel_turn_radius = self.calculate_wheel_turn_radius(
body_turn_radius, angular_speed, wheel)
right_wheel_rpm = self.calculate_wheel_rpm(linear_speed, angular_speed,
right_wheel_turn_radius)
left_wheel_rpm = self.calculate_wheel_rpm(linear_speed, angular_speed,
left_wheel_turn_radius)
self.set_wheel_movement(right_wheel_rpm, left_wheel_rpm)
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
class Collector():
"""
Handles the communication to arduino SensorReader
The arduino firmware publishes messages using firmata once per second with sensor readings.
This class is responsible for collection and persistance of readings.
AttachTo: ""
"""
NAME = "Collector"
def __init__(self, port):
'''
Constructor. Adds the callback handler to accept firmata messages from the arduino board connected to the sensors.
'''
self.last_rain_reading_saved = None
self.last_sky_temperature_reading_saved = 0
self.last_ambient_temperature_reading_saved = 0
self.last_reading_saved_time = time.time()
self.board = Arduino(port)
# start an iterator thread so that serial buffer doesn't overflow
it = util.Iterator(self.board)
it.start()
self.board.add_cmd_handler(pyfirmata.pyfirmata.STRING_DATA,
self._messageHandler)
def _messageHandler(self, *args, **kwargs):
'''
Calback method envoked by the firmata library. Handles the string message sent from the arduino.
Grabs the sensor data from the message and persists in the DB table.
:param args:
:param kwargs:
:return:
'''
readings = json.loads(util.two_byte_iter_to_str(args))
sky_temperature = readings['sky']
ambient_temperature = readings['ambient']
rain = readings['rain']
if self.should_persist_sensor_reading(sky_temperature,
ambient_temperature, rain):
conn = sqlite3.connect('weather_sensor.db')
c = conn.cursor()
print('inserting observation')
c.execute(
"INSERT INTO weather_sensor (rain,sky_temperature,ambient_temperature) VALUES (?,?,?)",
(rain, sky_temperature, ambient_temperature))
c.execute(
"DELETE FROM weather_sensor WHERE date_sensor_read <= date('now','-365 day')"
)
print('inserted')
# new_id = c.lastrowid
conn.commit()
self.last_rain_reading_saved = rain
self.last_sky_temperature_reading_saved = sky_temperature
self.last_ambient_temperature_reading_saved = ambient_temperature
self.last_reading_saved_time = time.time()
c.close()
def update(self):
super(Collector, self).update()
def writeData(self, data):
self.board.send_sysex(pyfirmata.pyfirmata.STRING_DATA, data)
def dispose(self):
super(Collector, self).dispose()
try:
self.board.exit()
except AttributeError:
print('exit() raised an AttributeError unexpectedly!')
def should_persist_sensor_reading(self, sky_temperature,
ambient_temperature, rain):
'''
Returns true if the sensor data should be persisted in the DB.
The rules are if any temp change is more than 1 degree, or the rain state changes
or 60sec have elapsed since the last save
:param sky_temperature:
:param ambient_temperature:
:param rain:
:return:
'''
if abs(sky_temperature -
self.last_sky_temperature_reading_saved) > 1 or abs(
ambient_temperature -
self.last_ambient_temperature_reading_saved
) > 1 or rain != self.last_rain_reading_saved or time.time(
) - self.last_reading_saved_time > 60:
return True
else:
return False
class FMB:
def __init__(self):
#board
#self.board = Arduino('/dev/ttyACM0')
self.board = Arduino('/dev/ttyUSB0') #for NANO 328
self.it = util.Iterator(self.board)
self.it.start()
self.board.analog[0].enable_reporting()
#pins
self.monitor_pin = self.board.get_pin('d:3:p')
self.stim_pin = self.board.get_pin('d:6:p')
self.off_on = self.board.get_pin('d:9:p')
self.trigger = self.board.get_pin('d:5:p')
#monitor details
self.res = [1280,1024]
self.monitor_width = 37.5 #cm
self.monitor_distance = 14 #14 #14=small box) #cm from middle #20 #20=normal box) #cm from middle
self.mon = monitors.Monitor("mymon", distance = self.monitor_distance, width = self.monitor_width)
self.mon.currentCalib['sizePix'] = [self.res[0], self.res[1]]
self.mon.saveMon()
#Psychopy windows
self.window1 = visual.Window(size=[self.res[0],self.res[1]],monitor=self.mon, fullscr = False,allowGUI=False, units="deg", screen =1)
self.window2 = visual.Window(size=[self.res[0],self.res[1]],monitor=self.mon, fullscr = False,allowGUI=False, units="deg", screen =2)
#can't change gamma values; color set above middle gray to maintain the same brightness as grating
self.fixation1 = visual.GratingStim(win=self.window1, size=300, colorSpace ='rgb255', pos=[0,0], sf=0, color=(143,143,143))
self.fixation2 = visual.GratingStim(win=self.window2, size=300, colorSpace ='rgb255', pos=[0,0], sf=0, color=(143,143,143))
def habituation(self, time):
#pause on gray screens until key press (make space key?)
self.both_screens_gray(1.0)
print "Press any key to begin..."
sys.stdout.flush() #ensure msg appears now, regardless of print buffering
event.waitKeys()
self.trigger.write(1.0)
print "Now recording for "+str(time)+" s"
self.both_screens_gray(time)
self.trigger.write(0.0)
print "Habituation complete..."
#xset s on
from subprocess import call
screensaver_ON = ('xset s on')
call ([screensaver_ON], shell=True)
print('screen saver ON')
def training(self, **kwargs):
#get parameters
if "pin" in kwargs:
if kwargs["pin"] == "high":
pin = 1
elif kwargs["pin"] == "low":
pin = 0
else:
print("invalid pin state, use /'low/' or /'high/'")
return
if "orientation" in kwargs:
orientation = kwargs["orientation"]
if orientation == 135:
pin = 1
elif orientation == 45:
pin = 0
elif "pin" not in kwargs:
print("you must specify a pin state")
return
else:
print("defaulting to 45 degrees")
orientation = 45
pin = 0
if "screen" in kwargs:
if kwargs["screen"]==1:
print("*** Stim on screen 1 ***")
window = self.window1
self.monitor_pin.write(0)
elif kwargs["screen"]==2:
print("*** Stim on screen 2 ***")
window = self.window2
self.monitor_pin.write(1)
else:
print("defaulting to screen 1")
window = self.window1
self.monitor_pin.write(0)
if "reversals" in kwargs:
reversals = kwargs["reversals"]
else:
reversals = 100
if "startdelay" in kwargs:
startdelay = kwargs["startdelay"]
else:
startdelay = 300.0
if "interval" in kwargs:
interval = kwargs["interval"]
else:
interval = 30.0
if "blocks" in kwargs:
blocks = kwargs["blocks"]
else:
blocks = 5
stimulus = visual.GratingStim(tex = "sin", win=window, mask="circle", size=300, pos=[0,0], sf=0.05 , ori=(orientation))
#pause on gray screens until key press (make space key?)
self.both_screens_gray(1.0)
print "Press any key to begin..."
sys.stdout.flush() #ensure msg appears now, regardless of print buffering
event.waitKeys()
#procedure
print "TRIAL RUNNING"
sys.stdout.flush() #ensure msg appears now, regardless of print buffering
self.trigger.write(1.0)
self.both_screens_gray(startdelay)
for i in range(blocks):
self.stim_pin.write(pin)
self.off_on.write(1)
print('Session '+str(i+1)+' of '+str(blocks))
sys.stdout.flush()
for i in range(reversals):
stimulus.draw()
window.flip()
stimulus.setPhase(0.5, '+'
)
core.wait(0.5)
stimulus.draw()
window.flip()
stimulus.setPhase(0.5, '+')
core.wait(0.5)
self.off_on.write(0)
self.both_screens_gray(interval)
self.trigger.write(0.0)
print "TRIAL COMPLETE"
event.waitKeys()
#xset s on
from subprocess import call
screensaver_ON = ('xset s on')
call ([screensaver_ON], shell=True)
print('screen saver ON')
event.clearEvents()
self.board.exit()
self.window1.close()
self.window2.close()
core.quit()
def winflip(self):
self.window1.flip()
self.window2.flip()
def both_screens_gray(self, time):
self.fixation1.draw()
self.fixation2.draw()
self.winflip()
core.wait(time)
from pyfirmata import Arduino
from time import sleep
PORT = '/tmp/ttyS1'
board = Arduino(PORT)
try:
while True:
board.digital[13].write(1)
sleep(1)
board.digital[13].write(0)
sleep(1)
except:
print('termino el programa')
board.exit() # indicamos que cierre la conexion de la placa
# potentiometer -> brightness of led
from pyfirmata import Arduino, util, PWM
from time import sleep
import os
# setup board
port = '/dev/cu.usbmodem1421'
board = Arduino(port)
sleep(5)
#s = board.get_firmata_version()
#print("Firmata version=", s)
it = util.Iterator(board) # iterate over the board
it.start() #start
# setup pin A0 which is connected to potentiometer
# setup pin 3 for LED - PWM
a0 = board.get_pin('a:0:i')
ledPin = board.get_pin('d:3:p') #digital, pin 3, PWM
# main section, interruptable gracefully with Ctrl-c
try:
# read A0 and set brightness...
while True:
p = a0.read() # 0.0 < p < 1.0
#print(p) #debugging
ledPin.write(p)
except KeyboardInterrupt:
board.exit() #exit board
os._exit(0) #exit program
from pyfirmata import Arduino, util
import time
board = Arduino("COM4")
it = util.Iterator(board)
it.start()
print("Hello!Arduino")#not necessary
LED = board.get_pin("d:13:o")#stting led to pin 13
while True:
LED.write(1)#to give input value to LED pin
time.sleep(0.5)
LED.write(0)
time.sleep(0.5)
board.exit()#to exit arduino board
from pyfirmata import Arduino, util
import time
carte = Arduino('/dev/ttyACM0')
acquisition = util.Iterator(carte)
acquisition.start()
tension_A0 = carte.get_pin('a:0:i')
time.sleep(1.0)
R1 = 3000
R2 = 1000
Umes = tension_A0.read() * 5 + (R1 + R2) / R2
print(Umes, "V")
carte.exit()
class ArduinoDevice(object):
"""Connects to external arduino hardware"""
def __init__(self):
self.main_pin = 'd:{}:o'.format(ARDPIN) # digital, pin 6, output
self.test_pin = 'd:13:o' # ask arduino for connection status. Added benefit of seeing LED 13 as visual aid
self.ping_state = 0
self.ping_interval = 1
self.ping_timer = StopWatch()
self.ping_timer.start()
self.manual_mode = False
self.connected = False
def connect(self):
"""Attempts to connect to the device"""
self.connected = False
for port in range(1, 256 + 1):
port = 'COM{}'.format(port)
try:
temp1 = serial.Serial(port)
temp1.flush()
temp1.close()
self.board = Arduino(port)
except serial.serialutil.SerialException:
try:
self.board.exit()
except AttributeError:
try:
temp2 = serial.Serial(port)
except serial.serialutil.SerialException:
pass
else:
temp2.flush()
temp2.close()
else:
self.main_output = self.board.get_pin(self.main_pin)
self.test_output = self.board.get_pin(self.test_pin)
print('Arduino Port Found at: {}'.format(port))
self.connected = True
break
def toggle_manual(self):
"""Turns manual mode on or off"""
self.manual_mode = not self.manual_mode
def __send_signal__(self):
"""Sends a pulse"""
self.write(1)
time.sleep(STIM_ON)
self.write(0)
def send_signal(self):
"""Sends a pulse using a worker thread"""
thread = thr.Thread(target=self.__send_signal__,
daemon=True,
name=SEND_SIGNAL)
thread.start()
def write(self, num):
"""Writes to arduino while handling any serial errors"""
try:
self.main_output.write(num)
except (serial.serialutil.SerialTimeoutException,
serial.serialutil.SerialException, AttributeError):
self.connected = False
def exit(self):
"""Close device cleanly"""
try:
self.board.exit()
except (serial.serialutil.SerialException,
serial.serialutil.SerialTimeoutException, AttributeError):
pass
def ping(self):
"""test if arduino is still connected"""
if self.ping_timer.elapsed() > self.ping_interval:
try:
self.ping_state ^= 1
self.test_output.write(self.ping_state)
except (serial.serialutil.SerialTimeoutException,
serial.serialutil.SerialException, AttributeError):
self.connected = False
finally:
self.ping_timer.reset()
self.ping_timer.start()
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
class HoduinoBoardInterface():
"""
This is the Arduino Board Interface.
This class initialize the Board and provides all the methods
for the interaction with it.
"""
__metaclass__ = Singleton
def __init__(self, port):
try:
self.board = Arduino(port)
except OSError as e:
raise Exception("Arduino not found on: {0}".format(port))
self._setup()
def _setup(self):
# Setup the pins
self.pin_manager = PinManager(self.board)
self.motor_pin = self.pin_manager.get_digital_pin_out(9)
self.pin_manager.set_pin_mode(self.motor_pin, "Servo")
self.buzzer_pin = self.pin_manager.get_digital_pin_out(3)
self.pin_manager.set_pin_mode(self.buzzer_pin, "PWM")
def _spin(self, degrees):
# spin the weel
self.motor_pin.write(degrees)
time.sleep(SLEEP_TIME)
def _buzzer(self, value):
self.buzzer_pin.write(value)
time.sleep(SLEEP_TIME)
self.buzzer_pin.write(0.0)
def _melody(self):
notes = {
'c': 62,
'd': 94,
'e': 30,
'f': 49,
'g': 50,
'a': 140,
'b': 194,
'C': 223
}
freqs = ['c', 'g', 'a', 'a', 'f', 'e']
for f in freqs:
self.buzzer_pin.write(notes[f]/255.0)
time.sleep(0.2)
self.buzzer_pin.write(0.0)
def donation_reaction(self):
# buzzer
# self._melody()
# spin procedure for donation reaction
self._spin(90)
self._spin(150)
self._spin(30)
self._spin(150)
self._spin(30)
self._spin(90)
def close_arduino(self):
self._exit()
def _exit(self):
try:
self.board.exit()
print "Succesfully exit from arduino board"
except:
print "Error while exiting arduino board"
# This code is supporting material for the book
# Python Programming for Arduino
# by Pratik Desai
# published by PACKT Publishing
from pyfirmata import Arduino, PWM
from time import sleep
import random
# Setting up the Arduino board
port = '/dev/cu.usbmodemfa1311'
board = Arduino(port)
# Need to give some time to pyFirmata and Arduino to synchronize
sleep(5)
# Setup mode of pin 2 as PWM
pin = 11
board.digital[pin].mode = PWM
for i in range(0, 99):
# This python method generate random value between given range
r = random.randint(1, 100)
board.digital[pin].write(r / 100.00)
# divide by 100.00 here as the answer will be float, integer otherwise
sleep(0.1)
board.digital[pin].write(0)
board.exit()
if string6E:
try:
close_stream("6E")
except NameError:
pass
wf6E = wave.open("C:\\Users\\Joanna\\Desktop\\music\\6E.wav", 'rb')
stream6E = set_stream(pyaudio.PyAudio(), wf6E, "6E")
stream6E.start_stream()
string6E = False
if string6G:
try:
close_stream("6G")
except NameError:
pass
wf6G = wave.open("C:\\Users\\Joanna\\Desktop\\music\\6G.wav", 'rb')
stream6G = set_stream(pyaudio.PyAudio(), wf6G, "6G")
stream6G.start_stream()
string6G = False
except KeyboardInterrupt:
break
for note in ["5C", "5E", "5G", "6C", "6E", "6G"]:
try:
close_stream(i)
except NameError:
pass
arduino.exit()
print "koniec"
port=3306)
cursor = db.cursor()
print("Arduino start~")
try:
while True:
gas = a4.read()
tmp = a5.read()
try:
gasValue = round(gas * 1024)
Vout = arduino_map(tmp, 0, 1, 0, 5)
tmpValue = round((((Vout * 1000) - 500) / 10), 2)
#tmpValue = ((round(tmp * 1024)) * (5.0/1024) -0.5) / 0.01
sleep(5)
except TypeError:
pass
print('{0} {1}'.format(gasValue, tmpValue))
sql = "update Student.articles_envdata set tmpValue = {1}, gasValue = {0} where data_id = 1".format(
gasValue, tmpValue)
cursor.execute(sql)
db.commit()
print("Update Success~")
sleep(5)
except Exception as e:
db.rollback()
print("Error!:{0}".format(e))
except KeyboardInterrupt:
uno.exit()
from pyfirmata import Arduino
from time import sleep
PORT = '/tmp/ttyS1'
board = Arduino(PORT)
try:
print('inicio el blink')
while True:
print('enciendo el led')
board.digital[13].write(1)
sleep(1)
print('apago el led')
board.digital[13].write(0)
sleep(1)
except:
print('se cierra la conexion con la placa')
board.exit() # indicamos que cierre la conexion con la tarjeta
from pyfirmata import Arduino, util
# Crea un objeto de tipo Arduino especificando el puerto de conexion
tarjeta = Arduino('/dev/ttyACM0')
#Inicia el iterador para evitar el overflow en el puerto serial
it = util.Iterator(tarjeta)
it.start()
#Crea un referencia para leer el pin analogo A0
analogo_0 = tarjeta.get_pin('a:0:i')
try:
while True:
print("Cana analogo A0: ", analogo_0.read())
except KeyboardInterrupt:
tarjeta.exit()
print("\nSaliendo.....")
Рисунок - Під'єднання датчика температури
"""
import matplotlib.pyplot as plt
import time
from pyfirmata import Arduino, util
board = Arduino('COM9') # з'єднати Arduino з портом COM9
it = util.Iterator(board)
it.start() # для використання аналогових портів
board.analog[0].enable_reporting()
X = [] # список зі значеннями температури
plt.ion() # інтерактивна побудова графіка
while len(X) < 30: # поки довжина списку мала
time.sleep(1) # затримка 1 с
x = board.analog[0].read() # читати значення з аналогового входу 0
print x
X.append(x) # додати в список
plt.plot(X, 'ko-')
plt.draw() # рисувати графік (plt.clf() - очистити)
if x > 0.35: # якщо температура висока
board.digital[13].write(1) # включити світлодіод
else:
board.digital[13].write(0) # виключити світлодіод
board.exit() # вийти
"""
Рисунок - Графік температури
"""
joint4_angle = 0
elif joint4_angle>180:
joint4_angle = 180
rospy.loginfo(rospy.get_caller_id() + ']--->Joint4 Angle :%d', joint4_angle)
arm.servo_config(9,0,255,joint4_angle)
joint5_angle = data.position[5]*360/6.28+90
if joint5_angle<0:
joint5_angle = 0
elif joint5_angle>180:
joint5_angle = 180
rospy.loginfo(rospy.get_caller_id() + ']--->Joint5 Angle :%d', joint5_angle)
arm.servo_config(10,0,255,joint5_angle)
joint6_angle = data.position[6]*360/6.28+90
if joint6_angle<0:
joint6_angle = 0
elif joint6_angle>50:
joint6_angle = 50
rospy.loginfo(rospy.get_caller_id() + ']--->Joint6 Angle :%d', joint6_angle)
arm.servo_config(11,0,255,joint6_angle)
def driver():
rospy.init_node('Arm_Driver', anonymous=True)
rospy.Subscriber('joint_states', JointState, callback)
rospy.spin()
if __name__ == '__main__':
driver()
arm.exit()
currentBoard.digital[pin].write(round(minPWM.value/255,2))
else:
currentBoard.digital[pin].write(0)
plot_values.append(minPWM.value/255)
if (len(plot_values) > 40):
plot_values.pop(0)
time.sleep(float(currentInterval))
else:
return
print('Checking communication..')
for nx in range(0,30):
try:
testBoard = Arduino('COM' + str(nx))
testBoard.exit()
finalCom = nx
except:
continue
print('Found Arduino port : COM' + str(finalCom))
try:
currentBoard = Arduino('COM' + str(finalCom))
except:
print('Error while connecting to the serial port.')
currentBoard.exit()
time.sleep(3)
try:
acquisition = util.Iterator(currentBoard)
acquisition.start()
except:
# -*- coding: utf-8 -*-
"""
Created on Thu Sep 5 19:23:41 2019
@author: admin
"""
from pyfirmata import Arduino, util
import time
board = Arduino('COM4')
iterator = util.Iterator(board)
iterator.start()
temperature = board.get_pin('a:0:i')
time.sleep(2)
print((temperature.read() * 5000.0 - 500.0) / 10.0)
board.exit()
class MoveOakD(object):
def __init__(self):
self.pitchServo = None
self.yawServo = None
self.board = None
self.min_track_confidence = 0.7
self.oakd_sdp = None
self.sweeping = False
self.sweeping_thread = None
self.last_tracked_object = None
self.last_detected_time = None
self.detected_time = time.monotonic()
self.tracked_duration = 0
self.track_base_track_vel = 0.0
self.track_base_track_pan_ave = None
self.track_turn_base = True
self.track_base_time = time.monotonic()
self.track_status = TrackStatus()
self.track_status_lock = Lock()
self.tracking_thread = None
self.tracking_run = False
def initialize(self):
self.board = Arduino('COM6')
iter = util.Iterator(self.board)
iter.start()
self.pitchServo = OakDServo(ServoAxis.Pitch, self.board)
self.yawServo = OakDServo(ServoAxis.Yaw, self.board)
def allHome(self):
self.pitchServo.setHome()
self.yawServo.setHome()
def yawHome(self):
self.yawServo.setHome()
def pitchHome(self):
self.pitchServo.setHome()
def isSweeping(self):
return self.sweeping
def getYaw(self):
return self.yawServo.getAngle()
def getPitch(self):
return self.pitchServo.getAngle()
def setYaw(self, angle):
self.yawServo.setAngle(angle)
def setPitch(self, angle):
self.pitchServo.setAngle(angle)
def startSweepingBackAndForth(self, count=1, speed=2, min=45, max=135):
self.sweeping = True
self.sweeping_thread = Thread(target=self.sweepYawBackAndForth,
args=(count, speed, min, max),
name="oakd sweep",
daemon=False)
self.sweeping_thread.start()
def stopSweepingBackAndForth(self):
self.sweeping = False
if self.sweeping_thread is not None:
self.sweeping_thread.join()
self.sweeping_thread = None
def sweepYawBackAndForth(self, count, speed=2, min=45, max=135):
self.sweeping = True
sweep_count = 0
self.yawServo.setHome()
time.sleep(1.0)
sweep_min = min
sweep_max = max
eyes.setHome()
while self.sweeping and (count == 0 or sweep_count < count):
self.sweepYaw(_YAW_HOME_, sweep_max, speed)
self.sweepYaw(sweep_max, _YAW_HOME_, speed)
self.sweepYaw(_YAW_HOME_, sweep_min, speed)
self.sweepYaw(sweep_min, _YAW_HOME_, speed)
sweep_count += 1
self.sweeping = False
def sweepYaw(self, begin, end, speed):
if not self.sweeping:
return
inc = speed if begin <= end else -speed
for pos in range(begin, end, inc):
self.yawServo.setAngle(pos, 0)
if eyes._going:
eyes.setTargetPitchYaw(targetYaw=servoToEyeYaw(pos))
if not self.sweeping:
return
time.sleep(0.05)
time.sleep(0.50)
def suspend(self):
self.pitchServo.suspend()
self.yawServo.suspend()
def resume(self):
self.pitchServo.resume()
self.yawServo.resume()
def update_base_pose_tracking(self):
pan = self.yawServo.getAngle()
if self.track_base_track_pan_ave == None:
self.track_base_track_pan_ave = pan
else:
self.track_base_track_pan_ave = self.track_base_track_pan_ave * 0.5 + pan * 0.5
if abs(self.yawServo.getAngle()) > 30.0:
self.track_base_track_vel = math.copysign(
0.05, self.track_base_track_pan_ave)
else:
if self.track_base_track_vel == 0.0:
return
self.track_base_track_vel = 0.0
if self.track_base_track_vel != 0.0:
self.oakd_sdp.rotate(self.track_base_track_vel)
def get_track_status(self):
with self.track_status_lock:
return deepcopy(self.track_status)
def clearLastTrackedObj(self):
self.last_tracked_object = None
def publish_tracked(self, detection, wasLost=False):
with self.track_status_lock:
if detection != None:
self.track_status.object = detection
self.track_status.tracking = TrackingResult.Tracked
elif wasLost:
self.track_status.tracking = TrackingResult.Lost
else:
self.track_status.tracking = TrackingResult.Not_Tracked
# Duration tracked/not tracked
self.tracked_duration = time.monotonic() - self.detected_time
def update_tracking(self, detections):
tracked_object = None
publish = False
if detections != None:
for det in detections:
if det.label != "person" or \
det.confidence < self.min_track_confidence or \
det.status != dai.dai.Tracklet.TrackingStatus.TRACKED:
continue
if self.last_tracked_object != None and \
self.last_tracked_object.id == det.id:
# Currently tracked object is still detected
tracked_object = self.last_tracked_object = det
self.last_detected_time = time.monotonic()
publish = True
break
# Select the closest person
if tracked_object == None or tracked_object.z > det.z:
tracked_object = det
# Delay a bit before switching away to different person
if self.last_tracked_object == None or \
(time.monotonic() - self.last_detected_time > 1.0):
# Reset the start time of tracking/not tracking if
# transitioning from not tracking to tracking or
# vice versa (but not on each timeout while not
# tracking)
if self.last_tracked_object != None:
self.detected_time = time.monotonic()
self.last_tracked_object = tracked_object
self.last_detected_time = time.monotonic()
publish = True
#print("Now tracking: %s" % ("none" if tracked_object == None else tracked_object.id))
if publish:
self.publish_tracked(tracked_object)
return tracked_object
def set_track_turn_base(self, value):
self.track_turn_base = value
def start_tracking(self,
mdai,
modeIn=TrackerMode.TrackScan,
trackTurnBase=False):
self.track_turn_base = trackTurnBase
self.tracking_thread = Thread(target=self.tracker_thread,
args=(
mdai,
modeIn,
),
name="tracker",
daemon=False)
self.tracking_run = True
self.tracking_thread.start()
def stop_tracking(self):
if self.tracking_thread != None:
self.tracking_run = False
self.tracking_thread.join()
self.tracking_thread = None
def tracker_thread(self, mdai, mode):
#print("Tracking thread started")
# must establish a separate client and server and connection to SDP because msl loadlib is not thread safe
self.oakd_sdp = my_sdp_client.MyClient()
sdp_comm.connectToSdp(self.oakd_sdp)
self.oakd_sdp.wakeup()
last_wakeup = time.monotonic()
last_heading_update = time.monotonic()
if mode == TrackerMode.TrackScan:
self.startSweepingBackAndForth(count=3, speed=2)
while self.tracking_run:
# keep lidar spinning for quick movement response
if time.monotonic() - last_wakeup > 50:
self.oakd_sdp.wakeup()
last_wakeup = time.monotonic()
detections = mdai.getPersonDetections()
tracked_object = self.update_tracking(detections)
if mode == TrackerMode.TrackScan:
if tracked_object == None:
if not self.sweeping:
break
else: # found object
self.stopSweepingBackAndForth()
mode = TrackerMode.Track
elif mode == TrackerMode.Track:
if time.monotonic() - last_heading_update >= 0.25:
if not self.track_turn_base:
baseYaw = self.oakd_sdp.heading() + 360
else:
baseYaw = -1
last_heading_update = time.monotonic()
self.yawServo.update(tracked_object, baseYaw)
self.pitchServo.update(tracked_object, baseYaw)
if self.track_turn_base:
self.update_base_pose_tracking()
if tracked_object == None:
self.last_detected_time == None
time.sleep(0.050)
self.oakd_sdp.disconnect()
self.oakd_sdp.shutdown_server32(kill_timeout=1)
self.oakd_sdp = None
self.allHome()
eyes.setHome()
#print("Tracking thread ending")
def shutdown(self):
self.stop_tracking()
self.allHome()
if self.board is not None:
self.board.exit()
self.board = None
