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() # вийти """ ![](fig2.png) Рисунок - Графік температури """
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