res[cfg[0]] = cfg[1] return res ### ARDUINO board = Arduino('COM7') pins = [board.get_pin('d:{0}:s'.format(i)) for i in range(2, 10)] def move_servo(pin, angle): print 'Writing {0} to pin {1}'.format(angle, pin) pins[pin - 2].write(angle) mute_angles = all_mute_angles() for pin in mute_angles: board.servo_config(pin, mute_angles[pin]) move_servo(pin, mute_angles[pin]) ### MIDI midi.init() inp = midi.Input(1) def play_note(note): pin, mute_angle, play_angle = pins_by_note(note) if pin is None: print "Error: No pin for note {0}".format(note) return move_servo(pin, play_angle)
#!/usr/bin/env python import rospy from std_msgs.msg import String #from std_msgs.msg import UInt16 from std_msgs.msg import Int32 import time from pyfirmata import Arduino on_hardware = True # whether we are running this node on the actual car (so it can access the IO board) wheelpin = 5 # to which pin on the IO sheld the whee pin got connected drivepin = 3 # to which pin on the IO shield the drive cable got connected if on_hardware == True: # running on hardware -- we need to set the board connection board = Arduino('/dev/ttyACM99', baudrate=57600) board.servo_config(wheelpin, min_pulse=1, max_pulse=100, angle=90) # set initial direction to straight forward board.servo_config(drivepin, min_pulse=1, max_pulse=20, angle=90) # set initial speed to natural speed_natural = 90 speed_current_angle = speed_natural # this variable will carry the actual speed at any time and will be used to determine direction of change (in case of decay or full stop) speed_min_angle_reverse = 75 # this is the angle below which the car start moving in reverse speed_min_angle_forward = 107 # this is the angle above which the car start moving forward speed_max_angle_reverse = 65 # maximum angle allowed in reverse (which is actually a minimum mathematically, as the angle goes 0-90) speed_max_angle_forward = 115 # maximum angle allowed in forward speed_decay_angle = 2 # how much we decrease the angle when there is a decay request speed_change_angle = 1 # when we receive a request to change the speed, this is the angle change we will do speed_change_angle_decrease = 2 # this is the speed slowdown (breaking) speed_direction_change_delay = 2 # in sec - delay enforced between changing direction (forward-backward) last_stop_timestamp = 0.0 # the last time we have reached the zero speed from a non-zero speed (used with the speed_direction_change_delay) direction_natural = 90 # this is the natural (straight ahead) position of the wheel in angles
Lmotor1 = board.get_pin('d:5:p') Lmotor2 = board.get_pin('d:6:p') #registr the rmotor Rmotor1 = board.get_pin('d:10:p') Rmotor2 = board.get_pin('d:11:p') #pointer for current image capture imageNumber = 0 #pin out constants for the rpi shutdownBtn = 10 inputBtn = 12 #configure the mast servo motor mastServo = board.servo_config(3) #setup the GPIO GPIO.setwarnings(False) GPIO.setmode(GPIO.BOARD) #Register pins GPIO.setup(shutdownBtn, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) GPIO.setup(inputBtn, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) GPIO.add_event_detect(shutdownBtn, GPIO.RISING, callback=shutdown) #function to shutdown the rpi def shutdown(): GPIO.cleanup() call("sudo shutdown now", shell=True)
'model': 'cfg/yolo.cfg', 'load': 'bin/yolov2.weights', 'threshold': 0.3, 'gpu': 0.6 } #constant declareation height = 2.15 #stage to LIGHT distance frameWidth = 2.2 #stage width height = height * 1280 / frameWidth #normalized height #Arduino Initialization board = Arduino('COM5') ypin = board.get_pin('d:9:s') xpin = board.get_pin('d:10:s') board.servo_config(9, 544, 2400, 0) board.servo_config(10, 544, 2400, 0) xpin.write(0) ypin.write(0) #tensorflow code to detect and track tfnet = TFNet(options) colors = [tuple(255 * np.random.rand(3)) for _ in range(10)] capture = cv2.VideoCapture(0) capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1280) posx = 0 posy = 0 time.sleep(5)
class firmata_servo(OpenRTM_aist.DataFlowComponentBase): ## # @brief constructor # @param manager Maneger Object # def __init__(self, manager): OpenRTM_aist.DataFlowComponentBase.__init__(self, manager) self._d_servo_data = RTC.TimedVector2D(RTC.Time(0,0),0) """ """ self._servo_dataIn = OpenRTM_aist.InPort("servo_data", self._d_servo_data) # initialize of configuration-data. # <rtc-template block="init_conf_param"> """ - Name: com_port - DefaultValue: /dev/tty.USB0 """ self._com_port = ['/dev/tty.USB0'] # </rtc-template> ## # # The initialize action (on CREATED->ALIVE transition) # formaer rtc_init_entry() # # @return RTC::ReturnCode_t # # def onInitialize(self): # Bind variables and configuration variable self.bindParameter("com_port", self._com_port, "/dev/tty.USB0") # Set InPort buffers self.addInPort("servo_data",self._servo_dataIn) # Set OutPort buffers # Set service provider to Ports # Set service consumers to Ports # Set CORBA Service Ports return RTC.RTC_OK # ## # # # # The finalize action (on ALIVE->END transition) # # formaer rtc_exiting_entry() # # # # @return RTC::ReturnCode_t # # # #def onFinalize(self): # # return RTC.RTC_OK # ## # # # # The startup action when ExecutionContext startup # # former rtc_starting_entry() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # def onStartup(self, ec_id): return RTC.RTC_OK # ## # # # # The shutdown action when ExecutionContext stop # # former rtc_stopping_entry() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # #def onShutdown(self, ec_id): # # return RTC.RTC_OK # ## # # # # The activated action (Active state entry action) # # former rtc_active_entry() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # def onActivated(self, ec_id): print "onActivated Begin" pinNumber = [3, 5, 6, 7, 11] self.pin = [None, None, None, None, None] try: self.board = Arduino(self._com_port[0]) for i in range(5): self.board.servo_config(pinNumber[i], angle=90) # Caution: Don't use board.get_pin('d:*:s') as it calls servo_config method with angle=0, which damages your servo. self.pin[i] = self.board.digital[pinNumber[i]] print "onActivated End" except Exception as e: print "some errors %s" % str(e) self.pin[0].write(80) return RTC.RTC_OK # ## # # # # The deactivated action (Active state exit action) # # former rtc_active_exit() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # #def onDeactivated(self, ec_id): # # return RTC.RTC_OK # ## # # # # The execution action that is invoked periodically # # former rtc_active_do() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # def onExecute(self, ec_id): if self._servo_dataIn.isNew(): print "onExecute New" servoData = self._servo_dataIn.read() servoId = int(servoData.data.x) angle = int(servoData.data.y) print "%d %d" % (servoId, angle) if servoId < len(self.pin) and angle <= 180 and angle >= 0: self.pin[servoId].write(angle) return RTC.RTC_OK # ## # # # # The aborting action when main logic error occurred. # # former rtc_aborting_entry() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # #def onAborting(self, ec_id): # # return RTC.RTC_OK # ## # # # # The error action in ERROR state # # former rtc_error_do() # # # # @param ec_id target ExecutionContext Id # # # # @return RTC::ReturnCode_t # # # # def onError(self, ec_id): print "error" return RTC.RTC_OK
class MultiServo: def __init__(self): self.ardu1 = Arduino('COM9') self.ardu2 = Arduino('COM3') self.pinlist = np.array([3, 5, 6, 9, 10, 11]) self.servoset = np.array([[600, 2300], [500, 2380]]) self.logdata = [] self.usedservo = np.zeros((2, 6), dtype=np.uint8) self.delaysec = 0.75 self.ispacked = False time.sleep(2) def command(self, pin, servotype, angle, ardu): if (isinstance(pin, int)) and (isinstance(servotype, int)) and (isinstance(angle, int)) and \ (isinstance(ardu, int)) and (0 <= pin <= 5) and (0 <= servotype <= 1) and (1 <= ardu <= 2): if ardu == 1: self.ardu1.servo_config(self.pinlist[pin], self.servoset[servotype, 0], self.servoset[servotype, 1], angle) elif ardu == 2: self.ardu2.servo_config(self.pinlist[pin], self.servoset[servotype, 0], self.servoset[servotype, 1], angle) else: print('Wrong Command') def run_command(self, cmd): # Run command, with delay if cmd[0] == 6: sec = cmd[2] time.sleep(sec) print('Delay ' + str(sec) + ' sec') if self.ispacked: if sec > self.delaysec: self.usedservo = np.zeros((2, 6), dtype=np.uint8) else: pinnow = int(cmd[0]) ardunow = int(cmd[3]) if self.ispacked: if self.usedservo[ardunow - 1, pinnow] == 1: time.sleep(self.delaysec) self.usedservo = np.zeros((2, 6), dtype=np.uint8) self.command(pinnow, int(cmd[1]), int(cmd[2]), ardunow) else: self.command(pinnow, int(cmd[1]), int(cmd[2]), ardunow) self.usedservo[ardunow - 1, pinnow] = 1 else: self.command(pinnow, int(cmd[1]), int(cmd[2]), ardunow) def run_data(self, data): # Run packed data self.ispacked = True for cmd in data: self.run_command(cmd) self.usedservo = np.zeros((2, 6), dtype=np.uint8) self.ispacked = False print('Moveset Ended') def get_data(self, file): try: print(file) data = np.genfromtxt(file, delimiter=',', defaultfmt='%d') except Exception as ex: print('Error Occured') print(ex) # Temp data-1 sec delay data = [6, 0, 1, 0] # print(data) self.run_data(data) def save_data(self, inittime): # PIN TYPE ANGLE ARDU # PIN 6 then ANGLE is DELAY # print(logdata) np.savetxt('MoveSet_' + inittime.strftime('%y%m%d_%H%M%S.csv'), self.logdata, fmt='%d', delimiter=',') def log_data(self, cmd): # cmd format: # PIN TYPE ANGLE ARDU self.logdata.append(cmd)
### Values for motor direction FORWARD = 1 BACKWARD = 2 BRAKE = None # These don't work. RELEASE = None clients = [] yun = Arduino('/dev/ttyATH0', baudrate=115200) s = yun.get_shield() m1 = s.get_motor(1) m2 = s.get_motor(2) m1.dir, m2.dir = FORWARD, FORWARD m1.spd, m2.spd = 0,0 yun.servo_config(9); yun.servo_config(10); yun.digital[10].write(60); # Start it at a different angle, so it can move opposite the other servo def butane_on(): yun.digital[9].write(60); yun.digital[10].write(0); def butane_off(): yun.digital[9].write(0); yun.digital[10].write(60); def retrieve(client,value): client.send(value) ### Class to handle websocket connections
from common.servoserver import ServoServer from pyfirmata import Arduino, util port = '/dev/tty.usbmodem1411' board = Arduino(port) board.servo_config(9) board.servo_config(10) def callback(channels): board.digital[9].write(channels[0] / 255.0 * 180) board.digital[10].write(channels[1] / 255.0 * 180) server = ServoServer(callback) server.start()