def __init__(self): ''' Constructor ''' self.azimuth_servo = Servo("P8_13", 0.0, 180.0) self.altitude_servo = Servo("P9_21",30.0,128.0) self.pupil_servo = Servo("P9_16",0.0,105.0); #130.0)
def __init__(self, config, logger): controller = open(config.fetch('servo_controller'), 'w') self.tilt = Servo( config.fetch('tilt_gpio'), controller, (int(config.fetch('tilt_max')), int(config.fetch('tilt_min')))) self.tilt.cur_pos = config.fetch('tilt_start') self.pan = Servo( config.fetch('pan_gpio'), controller, (int(config.fetch('pan_max')), int(config.fetch('pan_min')))) self.pan.cur_pos = config.fetch('pan_start') self.tilt_q_cur_pos = Queue() self.tilt_q_des_pos = Queue() self.tilt_q_speed = Queue() self.pan_q_cur_pos = Queue() self.pan_q_des_pos = Queue() self.pan_q_speed = Queue() self.cam = cv2.VideoCapture(int(config.fetch('camera_id'))) self.cam.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, float(config.fetch('frame_width'))) self.cam.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, float(config.fetch('frame_height'))) self.config = config self.logger = logger
class CarServer(object): def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012): self.port = port # The motor and servo for driving self.motor = Motor(*motor_pins) self.servo = Servo(servo_pin) # The most recent coordinates from the accelerameter self.coords = (0, 0, 0) # Whether or not to continue running the server self._run = True self.start() def start(self): """ Initialize and start threads. """ self._server_thread = threading.Thread(target=self._server_worker) self._server_thread.start() self._control_thread = threading.Thread(target=self._control_worker) self._control_thread.start() def stop(self): """ Shut down server and control threads. """ self._run = False def _server_worker(self): HOST = '' # Symbolic name meaning all available interfaces PORT = self.port s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.bind((HOST, PORT)) s.listen(1) conn, addr = s.accept() print 'Connected by', addr while self._run: data = conn.recv(1024) if data: coords = data[1:].split(',') x, y, z = [float(n) for n in coords] self.coords = (x, y, z) conn.sendall(data) conn.close() def _control_worker(self): while self._run: x, y, z = self.coords forward_speed = -y/10 turning_power = (x+10)/20 self.motor.drive(forward_speed) self.servo.set(turning_power)
def testServo(): print '=== Servo Test ===' print 'to do: servo Turn each 0.5 second' for servoPin in SERVO_A_GPIO, SERVO_B_GPIO : type = servo.SERVO_ANGLE if servoPin == SERVO_B_GPIO: type = servo.SERVO_CONTINUOUS servoMotor = Servo(servoPin,type) for i in range(0,11,1): print 0.1*i servoMotor.set_normalized(0.1*i) time.sleep(0.5) servoMotor.set_normalized(0.5)
class CatFeeder(object): def __init__(self): self.servo = Servo() self.title = None self.txt = None self.img = None self.ui = PiUi(img_dir=os.path.join(current_dir, 'imgs')) def main(self): self.main_menu() self.ui.done() def main_menu(self): self.page = self.ui.new_ui_page(title="Cat Feeder") self.page.add_element("hr") self.list = self.page.add_list() self.list.add_item("Nourrir", chevron=True, onclick=self.page_control) self.list.add_item("Journal", chevron=True, onclick=self.page_feed_logs) self.ui.done() def page_control(self): self.page = self.ui.new_ui_page(title="Nourrir", prev_text="<", onprevclick=self.main_menu) self.title = self.page.add_textbox("Control", "h1") plus = self.page.add_button("Ouvrir la trap", self.onupclick) minus = self.page.add_button("Fermer la trap", self.ondownclick) def page_feed_logs(self): self.page.add_textbox("Journal", "h1") self.page.add_element("hr") con = lite.connect('catfeeder.db') with con: cur = con.cursor() cur.execute("SELECT * FROM feedLogs") rows = cur.fetchall() for row in rows: self.page.add_textbox(row, "p") def onupclick(self): self.title.set_text("Ouvert ") self.servo.servo_CW(1200) def ondownclick(self): self.title.set_text("Ferme ") self.servo.servo_CCW(2000)
class TestServo(unittest.TestCase): def setUp(self): self.serial = Mock() self.channel,self.min_pulse,self.min_angle,self.max_pulse,self.max_angle = 1,500,-90,2500,90 self.servo = Servo(self.serial,self.channel,self.min_pulse,self.min_angle,self.max_pulse,self.max_angle) def position_bytes_from_angle(self,angle, shift=7, mask=0x7f): arc = self.max_angle - self.min_angle zeroed_angle = angle - self.min_angle pulse_range = self.max_pulse-self.min_pulse position = int(((float(zeroed_angle)/arc) * pulse_range) + self.min_pulse) * 4 position_low = chr(position & mask) position_high = chr((position >> shift) & mask) return position_low,position_high def test_should_initialise(self): servo = self.servo self.assertEqual(servo.serial,self.serial) self.assertEqual(servo.channel,self.channel) self.assertEqual(servo.min_pulse,self.min_pulse) self.assertEqual(servo.min_angle,self.min_angle) def test_should_set_the_angle_using_the_serial_object_provided(self): self.servo.set_position(10) position_low,position_high = self.position_bytes_from_angle(10) set_position_command = chr(0xaa) + chr(0x0c) + chr(0x04) self.serial.write.assert_called_with(set_position_command + chr(self.channel) + position_low + position_high) def test_should_issue_the_get_position_command_then_read_the_8bit_results(self): expected_position = -15 position_low,position_high = self.position_bytes_from_angle(expected_position,8,0xff) self.serial.read.side_effect = [position_low,position_high] get_position_command = chr(0xaa) + chr(0x0c) + chr(0x10) position = int(round(self.servo.get_position())) self.assertEqual(expected_position,position) self.serial.write.assert_called_with(get_position_command + chr(self.channel)) def test_should_get_errors(self): expected_errors = (50,51) self.serial.read.side_effect = map(chr,expected_errors) get_errors_command = chr(0xaa) + chr(0x0c) + chr(0x21) errors = self.servo.get_errors() self.assertEqual(expected_errors, errors) self.serial.write.assert_called_with(get_errors_command)
def test_mouth(): """-""" mouth = Servo(7) # Pin 25 while True: print 'Opening' mouth.move(180, 0.5) print 'Closing' mouth.move(0, 0.5) print 'Opening' mouth.named_move('OPEN', 0.5) print 'Closing' mouth.named_move('CLOSE', 0.5)
def __init__(self,gps=False,servo_port=SERVO_PORT): # devices self._gps = gps self.windsensor = WindSensor(I2C(WINDSENSOR_I2C_ADDRESS)) self.compass = Compass(I2C(COMPASS_I2C_ADDRESS),I2C(ACCELEROMETER_I2C_ADDRESS)) self.red_led = GpioWriter(17,os) self.green_led = GpioWriter(18,os) # Navigation self.globe = Globe() self.timer = Timer() self.application_logger = self._rotating_logger(APPLICATION_NAME) self.position_logger = self._rotating_logger("position") self.exchange = Exchange(self.application_logger) self.timeshift = TimeShift(self.exchange,self.timer.time) self.event_source = EventSource(self.exchange,self.timer,self.application_logger,CONFIG['event source']) self.sensors = Sensors(self.gps,self.windsensor,self.compass,self.timer.time,self.exchange,self.position_logger,CONFIG['sensors']) self.gps_console_writer = GpsConsoleWriter(self.gps) self.rudder_servo = Servo(serial.Serial(servo_port),RUDDER_SERVO_CHANNEL,RUDDER_MIN_PULSE,RUDDER_MIN_ANGLE,RUDDER_MAX_PULSE,RUDDER_MAX_ANGLE) self.steerer = Steerer(self.rudder_servo,self.application_logger,CONFIG['steerer']) self.helm = Helm(self.exchange,self.sensors,self.steerer,self.application_logger,CONFIG['helm']) self.course_steerer = CourseSteerer(self.sensors,self.helm,self.timer,CONFIG['course steerer']) self.navigator = Navigator(self.sensors,self.globe,self.exchange,self.application_logger,CONFIG['navigator']) self.self_test = SelfTest(self.red_led,self.green_led,self.timer,self.rudder_servo,RUDDER_MIN_ANGLE,RUDDER_MAX_ANGLE) # Tracking self.tracking_logger = self._rotating_logger("track") self.tracking_sensors = Sensors(self.gps,self.windsensor,self.compass,self.timer.time,self.exchange,self.tracking_logger,CONFIG['sensors']) self.tracker = Tracker(self.tracking_logger,self.tracking_sensors,self.timer)
def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 55, dps = 15.8): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.distancePerSecond = dps self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0
def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 100): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0 print "Left Speed: ", self.leftSpeed, "\nRight Speed: ", self.rightSpeed
def __init__(self, name, ammo): self.name = name self.current_degree = 0 self.ammunition_count = ammo self.degrees = range(0, 101, 10) self.serial_connection = serial.Serial("/dev/ttyACM0", 9600) self.seven_seg_one = Display(SDI=11, RCLK=12, SRCLK=13) self.seven_seg_two = Display(SDI=33, RCLK=32, SRCLK=35) self.x_axis = Servo(0, "X Axis", self) self.y_axis = Servo(1, "Y Axis", self) self.release= Servo(2, "Release", self) self.sonic = Sonic(16, 18) self.servos = [self.x_axis, self.y_axis] self.motor = Motor(37, 38, 40)
class Eye: ''' classdocs ''' def __init__(self): ''' Constructor ''' self.azimuth_servo = Servo("P8_13", 0.0, 180.0) self.altitude_servo = Servo("P9_21",30.0,128.0) self.pupil_servo = Servo("P9_16",0.0,105.0); #130.0) def setAzimuth(self, angle): self.azimuth_servo.setangle(angle) def setAltitude(self, angle): self.altitude_servo.setangle(angle) def setPupil(self, size): ''' Set pupil size, as a percentage of maximum ''' # largest angle => most interested pupil self.pupil_servo.setangle(self.pupil_servo.min_angle + size * (self.pupil_servo.max_angle - self.pupil_servo.min_angle) / 100.0)
def __init__(self): GPIO.setmode(GPIO.BCM) self.direction = Servo(27, (-45, 45), (72.5, 162.5)) # board pin: 13 self.direction.trim = -1 self.front_motor = DCMotor(20, 6, 13) # board pin: 38, 31, 33 self.rear_motor = DCMotor(21, 19, 26) # board pin: 40, 35, 37 self.speed = 0 self.w_angle = self.direction.angle self.mov = 'quieto' """
def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012): self.port = port # The motor and servo for driving self.motor = Motor(*motor_pins) self.servo = Servo(servo_pin) # The most recent coordinates from the accelerameter self.coords = (0, 0, 0) # Whether or not to continue running the server self._run = True self.start()
def __init__(self, logger = logging): self.logger = logger self.servo = Servo(logger = logger) self.dcMotor = DCMotor(logger = logger) self.config = "" self.setConfig(FINE) self.moveFuncs = { 'W': self.dcMotor.forward, 'S': self.dcMotor.backward, 'A': self.dcMotor.left, 'D': self.dcMotor.right, 'w': self.servo.up, 's': self.servo.down, 'a': self.dcMotor.left, 'd': self.dcMotor.right } self.logger.info('bot created')
class TestServo(unittest.TestCase): def set_and_read_position(self,position): self.servo.set_position(position) time.sleep(0.5) return self.servo.get_position() def setUp(self): serial_port0 = serial.Serial('/dev/ttyACM0') self.servo = Servo(serial_port0,0,500,-90,2500,90) def test_should_be_able_to_move_servo_and_read_position(self): for position in [-90,-45,5,45,90]: self.assertLess(percentage_diff(self.set_and_read_position(position),position),2) def test_for_errors(self): a,b = self.servo.get_errors() self.assertEqual(a,0) self.assertEqual(b,0)
def __init__(self, config): self.servo_power = Power(config) self.servo_power.set_relay_two_off() self.degrees_per_radian = 180.0 / math.pi self.home = ephem.Observer() self.home.lon = config['home']['lng'] self.home.lat = config['home']['lat'] self.home.elevation = float(config['home']['alt']) self.elements = [] self.lsm = LSM303DLHC() self.lsm.set_declination(10, 40) self.servo = Servo(config, self.lsm) self.log = logging.getLogger('boresight') self._running = True self._worker = threading.Thread(target=self._track_worker) self._worker.setDaemon(True) self._worker.start()
def __init__(self, stepper_pins, servo1_pin, servo2_pin): GPIO.setmode(GPIO.BCM) self.servo1 = Servo(servo1_pin) # Серво сверху self.servo2 = Servo(servo2_pin) # Серво снизу self.stepper = Stepper(stepper_pins)
loc_x = int(int(lin) % 8) loc_y = int(floor(int(lin) / 8)) #print 'LOC: ' + str(loc_x) + ',' + str(loc_y) + ' ' + servo.currentpos for y in range(0,7): for x in range(0,8): if (x == loc_x) and (y == loc_y): if monome.get_led(str(x), str(y)) == '0': monome.set_led('11', str(x), str(y)) else: if monome.get_led(str(x), str(y)) == '1': monome.set_led('10', str(x), str(y)) # INIT # a = Servo('COM5') a.open_servo() m = Monome('COM6') buttons = [ Button(m, 2, 7, 'trigger'), Button(m, 5, 7, 'trigger'), Button(m, 4, 7, 'toggle'), Button(m, 7, 7, 'trigger') ] m.open_serial() button_thread = ButtonHandler(m, buttons) button_thread.start() # MAIN # led_animate(m, a)
# -*- coding: utf-8 -*- """ 程式說明請參閱10-11頁 """ from servo import Servo s1 = Servo(0) s1.rotate(120) s1.rotate(60)
class objectDetect(): CountLostFrame = 0 Count = 0 mean_file = None labels = None net = None transformer = None status = 1 def __init__(self, net, transformer, mean_file, labels, withoutservo, config, bkb, Mem): self.mean_file = mean_file self.labels = labels self.net = net self.transformer = transformer self.withoutservo = withoutservo self.config = config self.bkb = bkb self.Mem = Mem self.kernel_perto = np.ones( (self.config.kernel_perto, self.config.kernel_perto), np.uint8) self.kernel_perto2 = np.ones( (self.config.kernel_perto2, self.config.kernel_perto2), np.uint8) self.kernel_medio = np.ones( (self.config.kernel_medio, self.config.kernel_medio), np.uint8) self.kernel_medio2 = np.ones( (self.config.kernel_medio2, self.config.kernel_medio2), np.uint8) self.kernel_longe = np.ones( (self.config.kernel_longe, self.config.kernel_longe), np.uint8) self.kernel_longe2 = np.ones( (self.config.kernel_longe2, self.config.kernel_longe2), np.uint8) self.kernel_muito_longe = np.ones( (self.config.kernel_muito_longe, self.config.kernel_muito_longe), np.uint8) self.kernel_muito_longe2 = np.ones( (self.config.kernel_muito_longe2, self.config.kernel_muito_longe2), np.uint8) if self.withoutservo == False: self.servo = Servo(self.config.CENTER_SERVO_PAN, self.config.POSITION_SERVO_TILT) def searchball(self, image, visionMask, visionMorph1, visionMorph2, visionMorph3, visionMorph4): YUV_frame = cv2.cvtColor(image, cv2.COLOR_BGR2YUV) white_mask = cv2.inRange(YUV_frame[:, :, 0], self.config.white_threshould, 255) if visionMask: cv2.imshow('Frame Mascara', white_mask) # start2 = time.time() BallFound = False frame, x, y, raio, maskM = self.Morphology(image, white_mask, self.kernel_perto, self.kernel_perto2, 1) if visionMorph1: cv2.imshow('Morfologia 1', maskM) # print "Search = ", time.time() - start2 if (x == 0 and y == 0 and raio == 0): frame, x, y, raio, maskM = self.Morphology(image, white_mask, self.kernel_medio, self.kernel_medio2, 2) if visionMorph2: cv2.imshow('Morfologia 2', maskM) if (x == 0 and y == 0 and raio == 0): frame, x, y, raio, maskM = self.Morphology( image, white_mask, self.kernel_longe, self.kernel_longe2, 3) if visionMorph3: cv2.imshow('Morfologia 3', maskM) if (x == 0 and y == 0 and raio == 0): frame, x, y, raio, maskM = self.Morphology( image, white_mask, self.kernel_muito_longe, self.kernel_muito_longe2, 4) if visionMorph4: cv2.imshow('Morfologia 4', maskM) if (x == 0 and y == 0 and raio == 0): self.CountLostFrame += 1 print("@@@@@@@@@@@@@@@@@@@", self.CountLostFrame) if self.CountLostFrame == self.config.max_count_lost_frame: BallFound = False self.CountLostFrame = 0 print( "----------------------------------------------------------------------" ) print( "----------------------------------------------------------------------" ) print( "----------------------------------------------------------------------" ) print( "--------------------------------------------------------Ball not found" ) print( "----------------------------------------------------------------------" ) print( "----------------------------------------------------------------------" ) print( "----------------------------------------------------------------------" ) if not self.withoutservo: self.status = self.SearchLostBall() if (x != 0 and y != 0 and raio != 0): BallFound = True return frame, x, y, raio, BallFound, self.status #Varredura def SearchLostBall(self): if self.bkb.read_int(self.Mem, 'IMU_STATE') == 0: if self.Count == 0: self.servo.writeWord( self.config.SERVO_PAN_ID, 30, self.config.CENTER_SERVO_PAN - self.config.SERVO_PAN_LEFT) #olha para a esquerda time.sleep(1) self.Count += 1 return 0 if self.Count == 1: self.servo.writeWord( self.config.SERVO_PAN_ID, 30, self.config.CENTER_SERVO_PAN) #olha para o centro time.sleep(1) self.Count += 1 return 1 if self.Count == 2: self.servo.writeWord( self.config.SERVO_PAN_ID, 30, self.config.CENTER_SERVO_PAN + self.config.SERVO_PAN_RIGHT) #olha para a direita 850- 440 time.sleep(1) self.Count = 0 return 2 def Morphology(self, frame, white_mask, kernel, kernel2, k): start3 = time.time() contador = 0 # cv2.imshow('mask',white_mask) mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel) mask = cv2.morphologyEx(mask, cv2.MORPH_DILATE, kernel2, 1) # Se a morfologia de perto k =1, recorta a parte de cima if k == 1: mask[0:200, :] = 0 # Se a morfologia medio k =2, recorta a parte de baixo if k == 2: mask[650:, :] = 0 # Se a morfologia de longe k =3, recorta a parte de baixo if k == 3: mask[450:, :] = 0 # Se a morfologia de muito longe k = 4, recorta a parte de baixo if k == 4: mask[350:, :] = 0 ret, th1 = cv2.threshold(mask, 25, 255, cv2.THRESH_BINARY) try: _, contours, _ = cv2.findContours(th1, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) except: contours, _ = cv2.findContours(th1, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for cnt in contours: contador = contador + 1 x, y, w, h = cv2.boundingRect(cnt) #Passa para o classificador as imagens recortadas----------------------- type_label, results = classify(cv2.cvtColor( frame[y:y + h, x:x + w], cv2.COLOR_BGR2RGB), self.net, self.transformer, mean_file=self.mean_file, labels=self.labels, batch_size=None) #----------------------------------------------------------------------- # print results, type_label # cv2.imshow('janela',images[0]) if type_label == 'Ball': return frame, x + w / 2, y + h / 2, (w + h) / 4, mask #================================================================================================= # print "CONTOURS = ", time.time() - start return frame, 0, 0, 0, mask
# otto example from botos import Bot from servo import Servo from action import Action otto = Bot() # add Ottos feet left_foot = Servo(name="left_foot", pin=1) right_foot = Servo("right_foot", pin=2) otto.add_servo(left_foot) otto.add_servo(right_foot) # add Ottos legs left_leg = Servo("left_leg", pin=3) right_leg = Servo("right_leg", pin=4) otto.add_servo(left_leg) otto.add_servo(right_leg) otto.show_pinouts() walk = Action() walk.add_step('set servo:left_leg angle=90') walk.add_step('set servo:left_foot angle=10') otto.actions.show()
class Controller: dev = False def __init__(self, camera, offset=0): self.camera = camera self.forward = 0 self.backward = 0 self.left = 0 self.right = 0 self.offset = offset if not Controller.dev: self.servo = Servo(0) self.servo.setup() self.left_wheel = Motor(17, offset=0) self.right_wheel = Motor(27, offset=0) self.pwm = PCA9685.PWM(bus_number=1) self.left_wheel.pwm = self.set_pwm_a self.right_wheel.pwm = self.set_pwm_b self.update_turn(offset) def set_pwm_a(self, value): pulse_wide = int(self.pwm.map(value, 0, 100, 0, 4095)) self.pwm.write(4, 0, pulse_wide) def set_pwm_b(self, value): pulse_wide = int(self.pwm.map(value, 0, 100, 0, 4095)) self.pwm.write(5, 0, pulse_wide) def handle_command(self, command, value): if command in ['DPadUp', 'DPadDown']: self.change_fps(value) elif command in ['RightTrigger', 'KeyW']: self.forward = value self.update_speed(self.forward - self.backward) elif command in ['LeftTrigger', 'KeyS']: self.backward = value self.update_speed(self.forward - self.backward) elif command == 'LeftStickX': self.update_turn(value) elif command == 'KeyA': self.left = value self.update_turn(self.right - self.left) elif command == 'KeyD': self.right = value self.update_turn(self.right - self.left) def change_fps(self, value): if self.camera is not None: if value == 1: self.camera.increase_fps() elif value == -1: self.camera.decrease_fps() def update_speed(self, speed): speed = self.map(speed, -1, 1, -100, 100) # print(f'Speed set to {speed}') if not Controller.dev: for motor in [self.left_wheel, self.right_wheel]: if speed >= 0: motor.forward() else: motor.backward() motor.speed = abs(speed) def update_turn(self, turn): turn = self.map(turn, -1, 1, 45, 135) # print(f'Turn set to {turn} ({turn+self.offset})') turn += self.offset if not Controller.dev: self.servo.write(turn) @staticmethod def map(x, in_min, in_max, out_min, out_max): return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) def stop_car(self): self.update_turn(0) self.update_speed(0)
import sys sys.path.append("..") import puka from servo import Servo client = puka.Client("amqp://raspberrypi/") promise = client.connect() client.wait(promise) promise = client.queue_declare(queue='servo') client.wait(promise) print " [*] Waiting for messages. Press CTRL+C to quit." servo = Servo() while True: consume_promise = client.basic_consume(queue='servo') result = client.wait(consume_promise) print " [x] Received message %r" % (result,) servo.position = int(result.get('body', '')) try: servo.update_position() except Exception, e: print "Couldn't update position. %s" % e client.basic_ack(result)
import RPi.GPIO as GPIO from servo import Servo import numpy as np import math #init the arm #rotation 17 #greifer 26 #alpha = 27, beta = 22, gamma = 13, delta = 19 servos = [Servo(27), Servo(22), Servo(13), Servo(17)] x, y, z = 0, 0, 0 def close(): """ cloese every connected GPIO pin """ for servo in servos: servo.close() GPIO.cleanup() def move(*angles): """ move the arm with the given angels NOTE: the given angles are relative to the x-y axis """ prev_angle = math.pi / 2 k = 0 for angle, servo in zip(angles, servos): #if the serve is not a joing the prev_angle should be 90 deg, because only the joints influzes eachothers angles
def __init__(self, bus): super(ServoPlugin, self).__init__(bus) logger.info('Initializing ServoPlugin') self.servo = Servo(PIN_SERVO_H, PIN_SERVO_V)
class Drive2: """controls two servos driving a three wheels platform :param lw_channel: PWM channel for left wheel :param rw_channel: PWM channel for right wheel """ __brakestep = 20 # how much reduce the speed at each cycle __brakedelay = 0.2 # secs between brakestep changes def __init__(self, lw_channel, rw_channel): self.lw = Servo(lw_channel) self.rw = Servo(rw_channel) def onemovefw(self, speed_increment, duration): ldir = -1 rdir = 1 self.__onemove(speed_increment, duration, ldir, rdir) def onemoveback(self, speed_increment, duration): ldir = 1 rdir = -1 self.__onemove(speed_increment, duration, ldir, rdir) def turnleft(self, speed_increment, duration): ldir = 1 rdir = 1 self.__onemove(speed_increment, duration, ldir, rdir) def turnright(self, speed_increment, duration): ldir = -1 rdir = -1 self.__onemove(speed_increment, duration, ldir, rdir) def __onemove(self, speed_increment, duration, ldir, rdir): """smoothly increase and decrease the speed for left and right servos :param speed_increment: how much increase the speed :param duration: how long keep the requested speed :param ldir: left wheel direction (-1: fw, +1: back) :param rdir: right wheel direction (+1: fw, -1: back) """ num_steps = 5 # progressively increase/decrease the speed step_size = abs(speed_increment / num_steps) # speedup for s in range(step_size, speed_increment, step_size): self.lw.step_position(ldir * s) self.rw.step_position(rdir * s) sleep(0.5) # keep top speed for a given time self.lw.step_position(ldir * speed_increment) self.rw.step_position(rdir * speed_increment) sleep(duration) # slowdown for s in range(speed_increment - step_size, step_size - 1, -step_size): dec_factor = -1 # Decrement the speed self.lw.step_position(ldir * s * dec_factor) self.rw.step_position(rdir * s * dec_factor) sleep(0.5) # ensure servos are stopped self.lw.reset_position() self.rw.reset_position() def speedup(self, speed_increment): lret = self.lw.step_position(-speed_increment) rret = self.rw.step_position(speed_increment) def slowdown(self, speed_increment): lret = self.lw.step_position(speed_increment) rret = self.rw.step_position(-speed_increment) def brake(self): lneutral = self.lw.get_neutral_pos() lposition = self.lw.get_current_pos() rneutral = self.rw.get_neutral_pos() rposition = self.rw.get_current_pos() # check direction ldir = -1 rdir = 1 # default to forward direction if (lneutral < lposition or rneutral > rposition): # moving backward ldir = 1 rdir = -1 while (lneutral != lposition and rneutral != rposition): # Slow down left if (lneutral != lposition): # left is still moving lposition = lposition - (ldir * __brakestep) if (abs(lposition - lneutral) < __brakestep): # lposition too close to neutral lposition = lneutral self.lw.step_position(lposition) # Slow down right if (rneutral != rposition): # right is still moving rposition = rposition - (rdir * self.__brakestep) if (abs(rposition - rneutral) < self.__brakestep): # rposition too close to neutral rposition = rneutral self.rw.step_position(rposition) sleep(self.__brakedelay)
import util socket_handler = SocketHandler(config.BACKEND_HOST, config.BACKEND_PORT) def func(): socket_handler.send_login(config.CLIENT_UUID) # socket_handler.close() socket_handler.on_connected = func socket_handler.start() video = cv2.VideoCapture(0) BASE = 100 OFFSET = 45 servo1 = Servo(config.SERVO1) servo2 = Servo(config.SERVO2) print("Reseting servo...") servo1.rotate(BASE) servo2.rotate(BASE) def turn_to_left(): servo1.rotate(BASE - OFFSET) servo2.rotate(BASE + OFFSET) import time time.sleep(4) servo1.rotate(BASE) servo2.rotate(BASE)
def __init__(self, i2c, pca9685, shoulder_pin, foot_pin, name=None): self.shoulder = Servo(i2c=i2c, pca9685=pca9685, name='shoulder', channel=shoulder_pin) self.foot = Servo(i2c=i2c, pca9685=pca9685, name="foot", channel=foot_pin) if name is not None: self.__name = name
class Leg(): __name = "leg" stand = False def __init__(self, i2c, pca9685, shoulder_pin, foot_pin, name=None): self.shoulder = Servo(i2c=i2c, pca9685=pca9685, name='shoulder', channel=shoulder_pin) self.foot = Servo(i2c=i2c, pca9685=pca9685, name="foot", channel=foot_pin) if name is not None: self.__name = name @property def name(self): return self.__name @name.setter def name(self, value): self.__name = value def stand(self): print("standing up leg", self.name) self.shoulder.transition = 'ease_in_sine' self.foot.transition = 'ease_in_sine' self.shoulder.duration_in_seconds = 2 self.foot.duration_in_seconds = 2 self.shoulder.target_angle = 180 self.foot.target_angle = 180 self.shoulder.start_angle = 90 self.shoulder.change_in_value = self.shoulder.target_angle - self.shoulder.start_angle self.foot.start_angle = 90 self.foot.change_in_value = self.foot.target_angle - self.shoulder.start_angle # Start the counters self.shoulder.tick_start() self.foot.tick_start() def sit(self): print("sitting down leg", self.name) self.shoulder.transition = 'ease_in_sine' self.foot.transition = 'ease_in_sine' self.shoulder.duration_in_seconds = 2 self.foot.duration_in_seconds = 2 self.shoulder.target_angle = 0 self.foot.target_angle = 180 self.shoulder.start_angle = 90 self.shoulder.change_in_value = self.shoulder.target_angle - self.shoulder.start_angle self.foot.start_angle = 90 self.foot.change_in_value = self.foot.target_angle - self.shoulder.start_angle # Start the counters self.shoulder.tick_start() self.foot.tick_start() @property def stand_tick(self): if not self.shoulder.tick() or not self.foot.tick(): self.shoulder.tick() self.foot.tick() # show current values # self.foot.show() # self.shoulder.show() return False else: return True @stand_tick.setter def stand_tick(self): if self.shoulder.elapsed_time_in_seconds <= self.shoulder.duration_in_seconds: self.stand_tick = False else: self.stand_tick = True def reset(self): self.shoulder.angle = 90 self.foot.angle = 90 print("Reseting limbs to 90 degrees") return True
sleep(30) # Where am I? Fetch from IP location. g = geocoder.ip('me') lat, lon = g.latlng # lat, lon = 53.3809, -1.4879 me = locations.Location('me', lat, lon, 150) lcd.clear() lcd.set_cursor(0,0) lcd.message("I am at lat, lon\n{:<8.2f}{:>8.2f}".format(lat, lon)) sleep(10) # Set up actuators elevation_actuator = Servo( servo_pin, initial_angle=0, min_angle=-101, max_angle=84, min_allowed=-90) azimuth_actuator = stepMotors(stepper_pins) # Buttons switch = Button(homeswitch_pin) cycle_button = Button( cycle_button_pin, hold_time=5.0, hold_repeat=False, ) cycle_button.when_held = held cycle_button.when_released = released # Home the stepper lcd.clear()
from servo import Servo import time servo = Servo() while True: x = float(input("Type angle: ")) servo.safe_rotate(x) time.sleep(.1)
class Movement(): def __init__(self): self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) def normalize(self, val): scale = 0.5 / 100 speed = val * scale if val >= 0: speed += 0.5 return speed def forward(self, speed = 100): self.left.set_normalized(self.normalize(speed)) self.right.set_normalized(self.normalize(-speed)) def backward(self, speed = 100): self.left.set_normalized(self.normalize(-speed)) self.right.set_normalized(self.normalize(speed)) def turn_left(self, speed = 100): self.left.set_normalized(self.normalize(-speed)) self.right.set_normalized(self.normalize(-speed)) def turn_right(self, speed = 100): self.left.set_normalized(self.normalize(speed)) self.right.set_normalized(self.normalize(speed)) def stop(self): self.left.set_normalized(-1) self.right.set_normalized(-1)
from servo import Servo import time servo = Servo() while True: print(360 - float(servo.get_angle())) time.sleep(.1)
class MovementInterface: def __init__(self, stepper_pins, servo1_pin, servo2_pin): GPIO.setmode(GPIO.BCM) self.servo1 = Servo(servo1_pin) # Серво сверху self.servo2 = Servo(servo2_pin) # Серво снизу self.stepper = Stepper(stepper_pins) def shot(self): self.servo2.set_servo_angle(0) def prepare(self): self.servo2.set_servo_angle(0) self.servo1.set_servo_angle(-20, hold=True) for angle in range(-20, 160): duty_cycle = angle / 20 + 3. self.servo1.servo_pwm.ChangeDutyCycle(duty_cycle) sleep(0.0125) sleep(2) for angle in range(160, 127, -1): duty_cycle = angle / 20 + 3. self.servo1.servo_pwm.ChangeDutyCycle(duty_cycle) sleep(0.0125) self.servo2.set_servo_angle(0) self.servo2.set_servo_angle(-30) self.servo1.set_servo_angle(-20) def turn(self, degree_value): self.stepper.turn_degree(degree_value)
slide.cleanup() home_switch.cleanup() # NEVER DELETE end_switch.cleanup() # NEVER DELETE linear.cleanup() pulley.cleanup() rotate.cleanup() fan.cleanup() if __name__ == "__main__": # initializing classes and pins slide = Stepper(config.slide_pins, slide_circum, slide_step) home_switch = Digital_Io(config.limit_home_pin, "in") # NEVER DELETE end_switch = Digital_Io(config.limit_end_pin, "in") # NEVER DELETE linear = Servo(config.linear_pin) pulley = Servo(config.pulley_pin) rotate = Servo(config.rotation_pin, 180) fan = Digital_Io(config.fan_pin, "out", 0) # initializing pins rotate.start(1.98, 12.85) rotate.update_duty(rotate_neutral_duty) linear.start(10, 5) linear.update_duty(linear_blade_retract_duty) pulley.start(0, 12.59) pulley.update_duty(pulley_off_duty) # setting up gui window window = tk.Tk() smear = Smear(window)
# Photo interrupter Settings IR1_Detect = 15 IR2_Detect = 14 GPIO.setup(IR1_Detect, GPIO.IN) GPIO.setup(IR2_Detect, GPIO.IN) GPIO.add_event_detect(IR1_Detect, GPIO.RISING, callback=get_num_of_pos_1) GPIO.add_event_detect(IR2_Detect, GPIO.RISING, callback=get_num_of_pos_2) # The Servos # Right motor M1 = UDServo(19) # Left motor M2 = Servo(6) #---------------------------------- num_of_pos_1 = 0 num_of_pos_2 = 0 Motor_1_on = 0 Motor_2_on = 0 positions = 0 m1 = 0 m2 = 0 start_time = 0 flag = 0 dt = [] pp = [] # IP of the Remote Computer Running SCRATCH #s = scratch.Scratch(host='192.168.1.21',port=42001)
import sys import random import functools import asyncio from wheel import Wheel from camera import Camera from servo import Servo from gamepad import XboxController driver = Wheel() cam = Camera() serv = Servo() controller = XboxController() controller.register('drive', driver.drive) controller.register('servo0', functools.partial(serv.angle, 0)) #controller.register('servo1', functools.partial(serv.angle, 1)) #controller.register('camera_onoff', cam.onoff) async def main(): print("press Ctl-C to exit") #print("camera initialization") try: task1 = asyncio.create_task(controller.start()) task2 = asyncio.create_task(cam.start()) await task1 await task2 except KeyboardInterrupt as e: print(e) except Exception as e: print(e)
#!/usr/bin/env python # WS server example that synchronizes state across clients import asyncio import json import logging import websockets from servo import Servo servo = Servo() logging.basicConfig() STATE = {"value": 2.5} USERS = set() def state_event(): return json.dumps({"type": "state", **STATE}) def users_event(): return json.dumps({"type": "users", "count": len(USERS)}) async def notify_state(): if USERS: # asyncio.wait doesn't accept an empty list message = state_event() await asyncio.wait([user.send(message) for user in USERS])
# set initial servo position servoInitPos = 65 # create a servoFirmate object per servo servoFirmata1 = servoFirmata(arduino, [2], [servoInitPos]) servoFirmata2 = servoFirmata(arduino, [3], [servoInitPos]) servoFirmata3 = servoFirmata(arduino, [4], [servoInitPos]) #---------------------------create servo objects---------------------------------------------------------------------- # calculate the avarage of servoPos2 and servoPos3 and substract servoPos1. result is direction of servo1 dirServo1 = [(servoPos2[0] + servoPos3[0]) / 2 - servoPos1[0], (servoPos2[1] + servoPos3[1]) / 2 - servoPos1[1]] # create servo1 object servo1 = Servo(dirServo1, servoFirmata1, kp, ki, kd, targetPosition, 52, debug) # calculate the avarage of servoPos1 and servoPos3 and substract servoPos2. result is direction of servo2 dirServo2 = [(servoPos1[0] + servoPos3[0]) / 2 - servoPos2[0], (servoPos1[1] + servoPos3[1]) / 2 - servoPos2[1]] # create servo2 object servo2 = Servo(dirServo2, servoFirmata2, kp, ki, kd, targetPosition, 45, debug) # calculate the avarage of servoPos2 and servoPos1 and substract servoPos3. result is direction of servo3 dirServo3 = [(servoPos2[0] + servoPos1[0]) / 2 - servoPos3[0], (servoPos2[1] + servoPos1[1]) / 2 - servoPos3[1]] # create servo3 object servo3 = Servo(dirServo3, servoFirmata3, kp, ki, kd, targetPosition, 48, debug)
# Regulates servo speed to a specified RPM # (SP_rpm variable) using Propotional Control. # Used for a two wheels robot, to find out the necessary # pulse width for each servo. import RPi.GPIO as GPIO import time from subprocess import check_call from servo import Servo, UDServo # The Normal servo M2 = Servo(6) # GPIO pin of photointerrupter sensor sensor_2 = 14 # The UpSide Down servo M1 = UDServo(19) # GPIO pin of photointerrupter sensor sensor_1 = 15 GPIO.setmode(GPIO.BCM) GPIO.setup(sensor_1, GPIO.IN) GPIO.setup(sensor_2, GPIO.IN) # Number of Encoder's Disk Positions encoder_pos = 20 #encoder_pos = 10 # The initial speed (pulse width in microseconds) # M1 and M2 at 1680 and 1650 respectively #speed_1 = 1650
def __init__(self): ### PREPARE PWM ### self.servo = Servo() self.servo.initialize(26, 7.5)
# # example ESP32 servo multitasking # phil van allen # # thanks to servo library from https://bitbucket.org/thesheep/micropython-servo # import time import machine import _thread as th from servo import Servo pin1 = machine.Pin(4) my_servo = Servo(pin1) delay = 0.025 pwm1_running = True pwm2_running = True def fade(secs, run): steps = int(round(180/(secs / delay / 2))) print(str(secs) + " " + str(steps) + " ") while run(): for i in range(0,180,steps): my_servo.write_angle(i) time.sleep(delay) for i in range(180, -1, (-1 * steps)): my_servo.write_angle(i) time.sleep(delay) def pwm1_run(): return pwm1_running
class Robot: def __init__(self, name, ammo): self.name = name self.current_degree = 0 self.ammunition_count = ammo self.degrees = range(0, 101, 10) self.serial_connection = serial.Serial("/dev/ttyACM0", 9600) self.seven_seg_one = Display(SDI=11, RCLK=12, SRCLK=13) self.seven_seg_two = Display(SDI=33, RCLK=32, SRCLK=35) self.x_axis = Servo(0, "X Axis", self) self.y_axis = Servo(1, "Y Axis", self) self.release= Servo(2, "Release", self) self.sonic = Sonic(16, 18) self.servos = [self.x_axis, self.y_axis] self.motor = Motor(37, 38, 40) def demo(self): self.x_axis.turn(180) self.y_axis.turn(170) self.x_axis.turn(100) self.y_axis.turn(20) def calibrate(self): print "[*]Calibrating" self.x_axis.turn(180, 0.05) self.y_axis.turn(90, 0.05) #self.release.turn(180, 0.05) print "[*]Done" def output_value(self, number): number = str(number) if len(number) == 2: self.seven_seg_one.disp_val(int(number[0])) self.seven_seg_two.disp_val(int(number[1])) elif len(number) == 1: self.seven_seg_one.disp_val(0) self.seven_seg_two.disp_val(int(number[0])) else: print "[-] Attempted to print non-two-digit number: {}".format(number) def main(self): """This is to be the main method. It will do the following: (1) Rotate the main servo in 10 degree segments (2) Stops and scans for an object (3) If an object exists, goes into "Firing Mode" (4) If not, continues on (5) Once it hits 180, it should reverse down to 0 """ while True: for deg in range(0, 181, 10): self.x_axis.turn(deg, 0.005) ob_pres = self.sonic.get_dist() if not (ob_pres is None): self.y_axis.turn(30) print "[!]Preparing to Fire" self.motor.fire() self.y_axis.turn(90) for deg in range(0,181, 10)[::-1]: self.x_axis.turn(deg, 0.005) ob_pres = self.sonic.get_dist() if not (ob_pres is None): self.y_axis.turn(30) print "[!]Preparing to Fire" self.motor.fire() self.y_axis.turn(90)
import time import pwm #Used to enable PWM from servo import Servo #You can use your own method to enable PWM, but this just works pwm.enable(); servo1 = Servo() servo1.attach("P9_14") servo2 = Servo() servo2.attach("P9_16") servo3 = Servo() servo3.attach("P8_13") servo4 = Servo() servo4.attach("P8_19") servo1.detach() servo2.detach() servo3.detach() servo4.detach()
import RPi.GPIO as GPIO from servo import Servo import time device = Servo() try: while True: device.move() time.sleep(0.1) except KeyboardInterrupt: GPIO.cleanup()
class RCCar(object): def __init__(self): GPIO.setmode(GPIO.BCM) self.direction = Servo(27, (-45, 45), (72.5, 162.5)) # board pin: 13 self.direction.trim = -1 self.front_motor = DCMotor(20, 6, 13) # board pin: 38, 31, 33 self.rear_motor = DCMotor(21, 19, 26) # board pin: 40, 35, 37 self.speed = 0 self.w_angle = self.direction.angle self.mov = 'quieto' """ self.rear_sensor = BatSensor(17, 27, rango=(10, 80), continuous=0.2) # board pin: 11, 13 self.front_left_sensor = BatSensor(22, 10, rango=(10, 80), continuous=0.2) # board pin: 15, 19 self.front_right_sensor = BatSensor(9, 11, rango=(10, 80), continuous=0.2) # board pin: 21, 23 """ def stop(self): self.in_use = False # dejar el coche frenando def turn_left(self): if self.survival_instinct(): return -self.direction.angle print("Izquierda:", self.direction.angle) return -self.direction.update(-5) def turn_right(self): if self.survival_instinct(): return -self.direction.angle print("Derecha:", self.direction.angle) return -self.direction.update(5) def forward(self): self.mov = 'adelante' if self.survival_instinct(): return self.speed if self.speed != 1: self.speed = 1 self.front_motor.forward() self.rear_motor.forward() print("Acelero hacia delante") return self.speed def backward(self): self.mov = 'atras' if self.survival_instinct(): return self.speed if self.speed != -1: self.speed = -1 self.front_motor.backward() self.rear_motor.backward() print("Acelero hacia atras") return self.speed def decelerate(self): if self.mov != 'quieto': print("Desacelero") #self.mov = 'quieto' self.front_motor.release() self.rear_motor.release() def survival_instinct(self): return False def survival_instinct_not_in_use(self): """ esta funcion no es ta en uso """ limit = 0.8 * max(self.front_left_sensor._rango) rear = self.rear_sensor.avg left = self.front_left_sensor.avg right = self.front_right_sensor.avg print("sensores R({}), FL({}), FR({})".format(rear, left, right)) if self.mov == 'atras' and rear < limit: self.brake() return True elif self.mov == 'adelante': if right < limit and left < limit: self.brake() return True elif right < limit: self.brake() return True elif left < limit: self.brake() return True return False def straight(self): if self.survival_instinct(): return -self.direction.angle self.direction.update(-self.direction.angle) return -self.direction.angle def brake(self): print("Freno") self.mov = 'quieto' self.front_motor.brake() self.rear_motor.brake()
from servo import Servo device = Servo() try: while True: device.angle = input("Digite um angulo destino ") device.set_angle() except KeyboardInterrupt: GPIO.cleanup()
class Bot: 'Controls bot' def __init__(self, logger = logging): self.logger = logger self.servo = Servo(logger = logger) self.dcMotor = DCMotor(logger = logger) self.config = "" self.setConfig(FINE) self.moveFuncs = { 'W': self.dcMotor.forward, 'S': self.dcMotor.backward, 'A': self.dcMotor.left, 'D': self.dcMotor.right, 'w': self.servo.up, 's': self.servo.down, 'a': self.dcMotor.left, 'd': self.dcMotor.right } self.logger.info('bot created') def start(self): self.servo.start() def stop(self): self.servo.stop() def move(self, ch): self.logger.info('bot move %s' %ch) func = self.moveFuncs.get(ch, lambda: 'Unknown %s' %ch) return func() def setConfig(self, config): if(self.config != config): if(config == COARSE): self.servo.setCoarseMovement() self.dcMotor.setCoarseMovement() self.config = config elif(config == FINE): self.servo.setFineMovement() self.dcMotor.setFineMovement() self.config = config else: self.logger.info('unrecogonized bot config %s' %config) def getConfig(self): return self.config def test(self): 'Testing bot' try: self.start() print 'get config ([fine]:coarse):', config = raw_input() self.setConfig(config) print 'config set to ' + self.getConfig() ch = readchar.readchar() while(ch != 'Q'): self.move(ch) ch = readchar.readchar() finally: self.stop()
from flask import Flask, url_for, render_template from flask import request, Response import cv2 from facedetection import facedetection from numpy import array from camserver import piwebcam from servo import Servo from yolomodel import YoloModel app = Flask(__name__, static_url_path = "", static_folder = "static" ) app.secret_key = 'X456yhj3k510oq' servobase = Servo(27) servoaltura = Servo(17) yolomodel = YoloModel() @app.route("/arm", methods=["GET"]) def move_arm(): angup = None anglebase = None if "anglebase" in request.args: try: anglebase = int(request.args["anglebase"]) servobase.move_angle(anglebase) except: servobase.stop()
class Move: TOO_CLOSE = "Too Close"; def __init__(self, start = (0,0),leftSpeed = 100, rightSpeed = 55, dps = 15.8): self.location = start self.x = start[0] self.y = start[1] self.leftSpeed = leftSpeed self.rightSpeed = rightSpeed self.distancePerSecond = dps self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR) self.sensor = Sensor() self.us = Ultrasound() self.movement = Movement() self.timeSpin = 0 def checkBoundary(self,cmSent): # situation 1 # we send something and we find out that the robot is going to hit the wall if it continues, meaning the distance is greater #than the closest thing away, that the worst possible thing, so lets check for that first. #if the desired distance is greater or equal to the distance away from the closest obstacle cmAwayFromWall = self.sensor.getDistance() if (cmSent > cmAwayFromWall): return "STOP" elif (cmSent > (cmAwayFromWall - 10)): return "STOP" elif (cmSent < (cmAwayFromWall - 10)): return "GOOD" def normalize(self, val): scale = 0.5 / 100 speed = val * scale if val >= 0: speed += 0.5 return speed def turn(self,angle): self.movement.setMotorSpeed(pins.SERVO_RIGHT_MOTOR, 50) self.movement.setMotorSpeed(pins.SERVO_LEFT_MOTOR, -100) time1 = 0.9 if angle == 15: time1 = 0.11 if angle == 90: time1 = 0.84 else: a = angle % 360 time1 = a * 0.11 time.sleep(self.timeSpin)#needs to be replaced with time1 after it is calibrated self.stop() def forward(self, speed = 100): ''' self.left.set_normalized(1.0) time.sleep(0.01) print self.rightSpeed self.right.set_normalized(-self.rightSpeed) ''' self.movement.setMotorSpeed(pins.SERVO_LEFT_MOTOR,100) time.sleep(0.01) self.movement.setMotorSpeed(pins.SERVO_RIGHT_MOTOR, self.rightSpeed) def move(self,distance): status = True result = self.checkBoundary(distance) cmAwayFromWall = self.sensor.getDistance() print cmAwayFromWall if (result == "STOP"): distanceMoved = 0 self.movement.stop() return distanceMoved elif (result =="GOOD"): self.forward(100) time.sleep(distance/self.distancePerSecond) # return status #account for the 5 degrees a second/ every 12 cm of curvature to the left #anglesToTurn = (distance/12) * 5 #spinTime = anglesToTurn*0.0053763408602 # degrees per second # telling the robot to spin back since it turns left on its own #self.timedSpin(spinTime,'right') self.movement.stop() distanceMoved = cmAwayFromWall - self.sensor.getDistance() return distanceMoved def timedSpin(self,spinTime,direction): if (direction == 'left'): self.movement.rotate_left(); elif (direction == 'right'): self.movement.rotate_right() time.sleep(spinTime) self.movement.stop() def turnMe(self,angle): # 90 degrees of rotation comes to approx 0.66 seconds, 2 seconds = 270* of rotation on a smooth surface angle = float(angle) if (angle >0): direction = 'right' else: direction = 'left' angle = abs(angle) if angle == 15: self.timeSpin = 0.0067 elif angle == 90 or angle == -90: self.timeSpin = 0.0083 else: self.timeSpin = 0.0067 print self.timeSpin spinTime = angle * self.timeSpin # degrees per second 0.0053763408602 self.timedSpin(spinTime,direction) def scanHallway(self): start = self.sensor.getDistance() self.sensor.getSensor() print start t1 = time.time() self.forward() while True: try: r = self.sensor.getSensor('r') l = self.sensor.getSensor('l') print r,l if r + l + 8 > 30: self.movement.stop() break except Exception as e: print e t2 = time.time() end = self.sensor.getDistance() distanceMoved = 0 if start >= 100: distanceMoved = (t2-t1) * self.distancePerSecond else: distanceMoved = start - end print end return (distanceMoved, (t2-t1) * self.distancePerSecond) def findDoor(self, side = 'r', distance = 10, maxDistance = 90): start = self.sensor.getDistance() self.sensor.getSensor() print start t1 = time.time() self.forward() while True: try: r = self.sensor.getSensor(side) print r if r > 5 + distance: #if sensor reads a distance greater than 5 (a buffer) + distance its found the door self.movement.stop() break except Exception as e: print e t2 = time.time() distanceMoved = (t2-t1) * self.distancePerSecond if distanceMoved >= maxDistance: return -1 t2 = time.time() end = self.sensor.getDistance() distanceMoved = 0 if start >= 100: distanceMoved = (t2-t1) * self.distancePerSecond else: distanceMoved = start - end print end return (distanceMoved, (t2-t1) * self.distancePerSecond) def stop(self): self.left.set_normalized(-1) self.right.set_normalized(-1)
class Camera(object): _instance = None def __new__(cls, *args, **kwargs): if not isinstance(cls._instance, cls): cls._instance = object.__new__(cls, *args, **kwargs) return cls._instance def __init__(self, config, logger): controller = open(config.fetch('servo_controller'), 'w') self.tilt = Servo( config.fetch('tilt_gpio'), controller, (int(config.fetch('tilt_max')), int(config.fetch('tilt_min')))) self.tilt.cur_pos = config.fetch('tilt_start') self.pan = Servo( config.fetch('pan_gpio'), controller, (int(config.fetch('pan_max')), int(config.fetch('pan_min')))) self.pan.cur_pos = config.fetch('pan_start') self.tilt_q_cur_pos = Queue() self.tilt_q_des_pos = Queue() self.tilt_q_speed = Queue() self.pan_q_cur_pos = Queue() self.pan_q_des_pos = Queue() self.pan_q_speed = Queue() self.cam = cv2.VideoCapture(int(config.fetch('camera_id'))) self.cam.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, float(config.fetch('frame_width'))) self.cam.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, float(config.fetch('frame_height'))) self.config = config self.logger = logger def get_frame(self): return self.cam.read()[1] def get_frames(self, count): frames = [] for i in range(count): frames.append(self.get_frame()) return frames def up(self, distance, speed): cur_pos = int(self.tilt.cur_pos) desired_pos = cur_pos - int(distance) self.logger.info('[UP] {0} -> {1}'.format(cur_pos, desired_pos)) self.tilt.move(desired_pos) self.tilt.cur_pos = desired_pos return if not self.tilt_q_cur_pos.empty(): cur_pos = self.tilt_q_cur_pos.get() desired_pos = self.tilt.assure_max(int(cur_pos) - int(distance)) self.tilt_q_des_pos.put(desired_pos) self.tilt_q_speed.put(speed) def down(self, distance, speed): cur_pos = int(self.tilt.cur_pos) desired_pos = cur_pos + int(distance) self.logger.info('[DOWN] {0} -> {1}'.format(cur_pos, desired_pos)) self.tilt.move(desired_pos) self.tilt.cur_pos = desired_pos return if not self.tilt_q_cur_pos.empty(): cur_pos = self.tilt_q_cur_pos.get() desired_pos = self.tilt.assure_min(int(cur_pos) + int(distance)) self.tilt_q_des_pos.put(desired_pos) self.tilt_q_speed.put(speed) def left(self, distance, speed): cur_pos = int(self.pan.cur_pos) desired_pos = cur_pos - int(distance) self.logger.info('[LEFT] {0} -> {1}'.format(cur_pos, desired_pos)) self.pan.move(desired_pos) self.pan.cur_pos = int(desired_pos) return if not self.pan_q_cur_pos.empty(): cur_pos = self.pan_q_cur_pos.get() desired_pos = self.pan.assure_max(int(cur_pos) - int(distance)) self.pan_q_des_pos.put(desired_pos) self.pan_q_speed.put(speed) def right(self, distance, speed): cur_pos = int(self.pan.cur_pos) desired_pos = cur_pos + int(distance) self.logger.info('[RIGHT] {0} -> {1}'.format(cur_pos, desired_pos)) self.pan.move(desired_pos) self.pan.cur_pos = desired_pos return if not self.pan_q_cur_pos.empty(): cur_pos = self.pan_q_cur_pos.get() desired_pos = self.pan.assure_min(int(cur_pos) + int(distance)) self.pan_q_des_pos.put(desired_pos) self.pan_q_speed.put(speed)
def setUp(self): self.serial = Mock() self.channel,self.min_pulse,self.min_angle,self.max_pulse,self.max_angle = 1,500,-90,2500,90 self.servo = Servo(self.serial,self.channel,self.min_pulse,self.min_angle,self.max_pulse,self.max_angle)
def __init__(self, pin, pull_duty, standby_duty, push_duty): Servo.__init__(self, pin, pull_duty) self.pull_duty = pull_duty self.standby_duty = standby_duty self.push_duty = push_duty
from servo import Servo servo = Servo() _ = input("Remove servo assembly from hob body. Press enter to continue _") print("Aligning servo...") servo.rotate(350) print("Rotate hob knob anticlockwise to 0, then rotate forward aprrox 10 degrees") _ = input(" Press enter to continue _") _ = input("Reattatch servo assembly to hob. Press enter to finish _")
class Pantilt(object): servo = None #Para controle dos servos cen_posTILT = None #Centro do Tilt cen_posPAN = None #Centro do Pan min_posTILT = None #Min do Tilt min_posPAN = None #Min do Pan max_posTILT = None #Max do Tilt max_posPAN = None #Max do Pan __Config = None #Leitura arquivo __args = None # Ganhos __p_pan = None __i_pan = None __d_pan = None __p_tilt = None __i_tilt = None __d_tilt = None __p_speed = None __min_speed = None # Find __list_find = None __old_list_find = None __pos_find = 0 __lost = 1 # Controlador __ControllerPan = None __ControllerTilt = None # DEFINES SERVOS __VAL_MIN = 6 __VAL_MAX = 8 __SERVO_PAN = 19 __SERVO_TILT = 20 __STATUS = 24 __GOAL_POS = 30 __SPEED = 32 __PRESENT_POS = 36 #---------------------------------------------------------------------------------------------------------------------------------- def __init__(self, args, Config): self.__Config = Config self.__readConfig() self.__args = args # Abrindo servos e configurando if args.withoutservo == False: self.servo = Servo(self.cen_posPAN, self.cen_posTILT) self.min_posPAN = self.servo.readWord(self.__SERVO_PAN, self.__VAL_MIN) self.max_posPAN = self.servo.readWord(self.__SERVO_PAN, self.__VAL_MAX) self.min_posTILT = self.servo.readWord(self.__SERVO_TILT, self.__VAL_MIN) self.max_posTILT = self.servo.readWord(self.__SERVO_TILT, self.__VAL_MAX) # Criando controlador self.__ControllerPan = PID(self.__p_pan, self.__i_pan, self.__d_pan) self.__ControllerPan.setPoint(0) self.__ControllerTilt = PID(self.__p_tilt, self.__i_tilt, self.__d_tilt) self.__ControllerTilt.setPoint(0) self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, self.__min_speed) self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, self.__min_speed) if args.head == True: self.__setVarredura() cv2.namedWindow('Video - Bola') cv2.createTrackbar('P Pan', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('I Pan', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('D Pan', 'Video - Bola', 0, 100, self.__nothing) cv2.createTrackbar('P Tilt', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('I Tilt', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('D Tilt', 'Video - Bola', 0, 100, self.__nothing) cv2.createTrackbar('Min vel', 'Video - Bola', 0, 1023, self.__nothing) cv2.createTrackbar('Mod', 'Video - Bola', 0, 3, self.__nothing) #Setando valoeres iniciais cv2.setTrackbarPos('P Pan', 'Video - Bola', int(self.__p_pan * 100)) cv2.setTrackbarPos('I Pan', 'Video - Bola', int(self.__i_pan * 100)) cv2.setTrackbarPos('D Pan', 'Video - Bola', int(self.__d_pan * 1000)) cv2.setTrackbarPos('P Tilt', 'Video - Bola', int(self.__p_tilt * 100)) cv2.setTrackbarPos('I Tilt', 'Video - Bola', int(self.__i_tilt * 100)) cv2.setTrackbarPos('D Tilt', 'Video - Bola', int(self.__d_tilt * 1000)) cv2.setTrackbarPos('Min vel', 'Video - Bola', self.__min_speed) cv2.setTrackbarPos('Mod', 'Video - Bola', 0) self.lastmod = -1 #---------------------------------------------------------------------------------------------------------------------------------- def mov(self, status, posHead, Mem): if self.__args.head == True: self.__p_pan = cv2.getTrackbarPos('P Pan', 'Video - Bola') / 100.0 self.__i_pan = cv2.getTrackbarPos('I Pan', 'Video - Bola') / 100.0 self.__d_pan = cv2.getTrackbarPos('D Pan', 'Video - Bola') / 1000.0 self.__p_tilt = cv2.getTrackbarPos('P Tilt', 'Video - Bola') / 100.0 self.__i_tilt = cv2.getTrackbarPos('I Tilt', 'Video - Bola') / 100.0 self.__d_tilt = cv2.getTrackbarPos('D Tilt', 'Video - Bola') / 1000.0 self.__min_speed = cv2.getTrackbarPos('Min vel', 'Video - Bola') mod = cv2.getTrackbarPos('Mod', 'Video - Bola') if self.__min_speed == 0: self.__min_speed = 1 cv2.setTrackbarPos('Min vel', 'Video - Bola', 1) self.__ControllerPan.setKp(self.__p_pan) self.__ControllerPan.setKi(self.__i_pan) self.__ControllerPan.setKd(self.__d_pan) self.__ControllerTilt.setKp(self.__p_tilt) self.__ControllerTilt.setKi(self.__i_tilt) self.__ControllerTilt.setKd(self.__d_tilt) if self.lastmod != mod: if mod == 0: self.servo.writeByte(self.__SERVO_PAN, self.__STATUS, 0) self.servo.writeByte(self.__SERVO_TILT, self.__STATUS, 0) elif mod == 2: self.servo.writeByte(self.__SERVO_PAN, self.__STATUS, 0) self.servo.writeByte(self.__SERVO_TILT, self.__STATUS, 1) self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, self.__min_speed) elif mod == 1: self.servo.writeByte(self.__SERVO_PAN, self.__STATUS, 1) self.servo.writeByte(self.__SERVO_TILT, self.__STATUS, 0) self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, self.__min_speed) elif mod == 3: self.servo.writeByte(self.__SERVO_PAN, self.__STATUS, 1) self.servo.writeByte(self.__SERVO_TILT, self.__STATUS, 1) self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, self.__min_speed) self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, self.__min_speed) self.__ControllerPan.setIntegrator(0) self.__ControllerPan.setDerivator(0) self.__ControllerTilt.setIntegrator(0) self.__ControllerTilt.setDerivator(0) self.lastmod = mod if status[0] == 2: if status[1] != 0 and status[2] != 0 and self.__lost == 0: posHead = self.__segue(status, Mem) else: if self.__args.head == False or self.servo.readByte( self.__SERVO_PAN, self.__STATUS) == 1: self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, posHead[0]) if self.__args.head == False or self.servo.readByte( self.__SERVO_TILT, self.__STATUS) == 1: self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, posHead[1]) self.__lost = 0 else: self.__lost = 1 self.__ControllerPan.setIntegrator(0) self.__ControllerPan.setDerivator(0) self.__ControllerTilt.setIntegrator(0) self.__ControllerTilt.setDerivator(0) posHead = self.__find(status) return posHead #---------------------------------------------------------------------------------------------------------------------------------- def __readConfig(self): while True: if 'Offset' not in self.__Config.sections(): print "Offset inexistentes, crinado valores padrao" self.__Config.add_section('Offset') self.__Config.set('Offset', 'ID_19', str(430) + '\t;Offset Tilt') self.__Config.set( 'Offset', 'ID_20', str(540) + '\t;Offset Pan\n;Valores para o robo olhando para frente') with open('../Data/config.ini', 'wb') as configfile: self.__Config.write(configfile) self.__Config.read('../Data/config.ini') else: self.cen_posTILT = self.__Config.getint('Offset', 'ID_19') self.cen_posPAN = int( self.__Config.get('Offset', 'ID_20').rpartition(';')[0]) break while True: if 'Head' not in self.__Config.sections(): print "Head inexistente, crinado valores padrao" self.__Config.add_section('Head') self.__Config.set( 'Head', 'p_pan', str(0.0) + '\t;Ganho proporcinal para controle da posicao no pan') self.__Config.set( 'Head', 'i_pan', str(0.0) + '\t;Ganho integral para controle da posicao no pan') self.__Config.set( 'Head', 'd_pan', str(0.0) + '\t;Ganho derivativo para controle da posicao no pan\n') self.__Config.set( 'Head', 'p_tilt', str(0.0) + '\t;Ganho proporcinal para controle da posicao no tilt') self.__Config.set( 'Head', 'i_tilt', str(0.0) + '\t;Ganho integral para controle da posicao no tilt') self.__Config.set( 'Head', 'd_tilt', str(0.0) + '\t;Ganho derivativo para controle da posicao no tilt\n;Ganho PID para a posicao\n' ) self.__Config.set( 'Head', 'min_vel', str(1) + '\t;Velocidade minima\n;Ganho P para a velocidade e velocidade minima' ) with open('../Data/config.ini', 'wb') as configfile: self.__Config.write(configfile) self.__Config.read('../Data/config.ini') else: self.__p_pan = self.__Config.getfloat('Head', 'p_pan') self.__i_pan = self.__Config.getfloat('Head', 'i_pan') self.__d_pan = self.__Config.getfloat('Head', 'i_pan') self.__p_tilt = self.__Config.getfloat('Head', 'p_tilt') self.__i_tilt = self.__Config.getfloat('Head', 'i_tilt') self.__d_tilt = float( self.__Config.get('Head', 'd_tilt').rpartition(';')[0]) self.__min_speed = int( self.__Config.get('Head', 'min_vel').rpartition(';')[0]) break #---------------------------------------------------------------------------------------------------------------------------------- def __find(self, status): self.__jump_find = 75 self.__list_find = [[self.min_posPAN, self.max_posTILT], [self.max_posPAN, self.max_posTILT]] # Indo para bola if abs( self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS) - self.servo.readWord(self.__SERVO_PAN, self.__GOAL_POS)) < 10: # Procura qual e o maior valor da distancia dis_pan = abs( self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS) - self.__list_find[self.__pos_find % 2][0]) * 1.0 dis_tilt = abs( self.servo.readWord(self.__SERVO_TILT, self.__PRESENT_POS) - self.__list_find[self.__pos_find % 2][1] + self.__jump_find * (self.__pos_find / 2)) * 1.0 if dis_pan > dis_tilt: max_dis = dis_pan else: max_dis = dis_tilt # print("dis_pan: " + str(dis_pan)) # print("dis_tilt: " + str(dis_tilt)) # raw_input("max_dis: " + str(max_dis)) # print(99*(dis_pan/max_dis)) # raw_input(99*(dis_tilt/max_dis)) if self.__args.head == False or self.servo.readByte( self.__SERVO_PAN, self.__STATUS) == 1: self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, 1 + int(99 * (dis_pan / max_dis))) self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.__list_find[self.__pos_find % 2][0]) if self.__args.head == False or self.servo.readByte( self.__SERVO_TILT, self.__STATUS) == 1: self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, 1 + int(99 * (dis_tilt / max_dis))) self.servo.writeWord( self.__SERVO_TILT, self.__GOAL_POS, self.__list_find[self.__pos_find % 2][1] + (self.__jump_find * (self.__pos_find / 2))) self.__pos_find += 1 if self.__list_find[self.__pos_find % 2][1] - ( self.__jump_find * (self.__pos_find / 2)) <= self.min_posTILT: self.__pos_find = 0 return [ self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS), self.servo.readWord(self.__SERVO_TILT, self.__PRESENT_POS) ] #---------------------------------------------------------------------------------------------------------------------------------- def __segue(self, status, Mem): # Pan # # Posicao self.__pos_find = 0 if self.__args.head == False or self.servo.readByte( self.__SERVO_PAN, self.__STATUS) == 1: self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, self.__min_speed) self.servo.writeWord( self.__SERVO_PAN, self.__GOAL_POS, int( self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS) + self.__ControllerPan.update(status[1]))) # Tilt # Posicao if self.__args.head == False or self.servo.readByte( self.__SERVO_TILT, self.__STATUS) == 1: self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, self.__min_speed) self.servo.writeWord( self.__SERVO_TILT, self.__GOAL_POS, int( self.servo.readWord(self.__SERVO_TILT, self.__PRESENT_POS) - self.__ControllerTilt.update(status[2]))) bkb.write_float( Mem, 'VISION_TILT_DEG', (self.servo.readWord(self.__SERVO_TILT, self.__PRESENT_POS) - self.max_posTILT) * 0.29) bkb.write_float( Mem, 'VISION_PAN_DEG', (self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS) - self.cen_posPAN) * 0.29) print bkb.read_float(Mem, 'VISION_PAN_DEG') # bkb.write_int('VISION_MOTOR1_ANGLE', self.servo.readWord(self.__SERVO_TILT, self.__PRESENT_POS)) # bkb.write_int('VISION_MOTOR2_ANGLE', self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS)) return [ self.servo.readWord(self.__SERVO_PAN, self.__PRESENT_POS), self.servo.readWord(self.__SERVO_TILT, self.__PRESENT_POS) ] #---------------------------------------------------------------------------------------------------------------------------------- def __setVarredura(self): # Crinado e setando cv2.namedWindow('Ajustar varredura - Head') cv2.createTrackbar('Val dir', 'Ajustar varredura - Head', 0, 1023, self.__nothing) cv2.createTrackbar('Val centro', 'Ajustar varredura - Head', 0, 1023, self.__nothing) cv2.createTrackbar('Val esq', 'Ajustar varredura - Head', 0, 1023, self.__nothing) cv2.createTrackbar('Val cima', 'Ajustar varredura - Head', 0, 1023, self.__nothing) cv2.createTrackbar('Val meio', 'Ajustar varredura - Head', 0, 1023, self.__nothing) cv2.createTrackbar('Val baixo', 'Ajustar varredura - Head', 0, 1023, self.__nothing) cv2.setTrackbarPos('Val dir', 'Ajustar varredura - Head', self.min_posPAN) cv2.setTrackbarPos('Val centro', 'Ajustar varredura - Head', self.cen_posPAN) cv2.setTrackbarPos('Val esq', 'Ajustar varredura - Head', self.max_posPAN) cv2.setTrackbarPos('Val cima', 'Ajustar varredura - Head', self.min_posTILT) cv2.setTrackbarPos('Val meio', 'Ajustar varredura - Head', self.cen_posTILT) cv2.setTrackbarPos('Val baixo', 'Ajustar varredura - Head', self.max_posTILT) self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, 0) self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, 0) cap = cv2.VideoCapture(0) while True: ret, frame = cap.read() if self.min_posPAN != cv2.getTrackbarPos( 'Val dir', 'Ajustar varredura - Head'): self.min_posPAN = cv2.getTrackbarPos( 'Val dir', 'Ajustar varredura - Head') self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, self.cen_posTILT) self.servo.writeWord(self.__SERVO_PAN, self.__VAL_MIN, self.min_posPAN) self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.min_posPAN) if self.cen_posPAN != cv2.getTrackbarPos( 'Val centro', 'Ajustar varredura - Head'): self.cen_posPAN = cv2.getTrackbarPos( 'Val centro', 'Ajustar varredura - Head') self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.cen_posPAN) self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, self.cen_posTILT) if self.max_posPAN != cv2.getTrackbarPos( 'Val esq', 'Ajustar varredura - Head'): self.max_posPAN = cv2.getTrackbarPos( 'Val esq', 'Ajustar varredura - Head') self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, self.cen_posTILT) self.servo.writeWord(self.__SERVO_PAN, self.__VAL_MAX, self.max_posPAN) self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.max_posPAN) if self.min_posTILT != cv2.getTrackbarPos( 'Val cima', 'Ajustar varredura - Head'): self.min_posTILT = cv2.getTrackbarPos( 'Val cima', 'Ajustar varredura - Head') self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.cen_posPAN) self.servo.writeWord(self.__SERVO_TILT, self.__VAL_MIN, self.min_posTILT) self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, self.min_posTILT) if self.cen_posTILT != cv2.getTrackbarPos( 'Val meio', 'Ajustar varredura - Head'): self.cen_posTILT = cv2.getTrackbarPos( 'Val meio', 'Ajustar varredura - Head') self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.cen_posPAN) self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, self.cen_posTILT) if self.max_posTILT != cv2.getTrackbarPos( 'Val baixo', 'Ajustar varredura - Head'): self.max_posTILT = cv2.getTrackbarPos( 'Val baixo', 'Ajustar varredura - Head') self.servo.writeWord(self.__SERVO_PAN, self.__GOAL_POS, self.cen_posPAN) self.servo.writeWord(self.__SERVO_TILT, self.__VAL_MAX, self.max_posTILT) self.servo.writeWord(self.__SERVO_TILT, self.__GOAL_POS, self.max_posTILT) cv2.imshow('Ajustar varredura - Head', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break cv2.destroyAllWindows() cap.release() #---------------------------------------------------------------------------------------------------------------------------------- def finalize(self): self.__Config.set('Offset', 'ID_19', str(self.cen_posTILT) + '\t;Offset Tilt') self.__Config.set( 'Offset', 'ID_20', str(self.cen_posPAN) + '\t;Offset Pan\n;Valores para o robo olhando para frente') self.__Config.set( 'Head', 'p_pan', str(self.__p_pan) + '\t;Ganho proporcinal para controle da posicao no pan') self.__Config.set( 'Head', 'i_pan', str(self.__i_pan) + '\t;Ganho integral para controle da posicao no pan') self.__Config.set( 'Head', 'd_pan', str(self.__d_pan) + '\t;Ganho derivativo para controle da posicao no pan\n') self.__Config.set( 'Head', 'p_tilt', str(self.__p_tilt) + '\t;Ganho proporcinal para controle da posicao no tilt') self.__Config.set( 'Head', 'i_tilt', str(self.__i_tilt) + '\t;Ganho integral para controle da posicao no tilt') self.__Config.set( 'Head', 'd_tilt', str(self.__d_tilt) + '\t;Ganho derivativo para controle da posicao no tilt\n;Ganho PID para a posicao\n' ) self.__Config.set( 'Head', 'min_vel', str(self.__min_speed) + '\t;Velocidade minima\n;Ganho P para a velocidade e velocidade minima' ) #---------------------------------------------------------------------------------------------------------------------------------- def __nothing(x, y): pass
def __init__(self): self.left = Servo(pins.SERVO_LEFT_MOTOR) self.right = Servo(pins.SERVO_RIGHT_MOTOR)
def __init__(self, args, Config): self.__Config = Config self.__readConfig() self.__args = args # Abrindo servos e configurando if args.withoutservo == False: self.servo = Servo(self.cen_posPAN, self.cen_posTILT) self.min_posPAN = self.servo.readWord(self.__SERVO_PAN, self.__VAL_MIN) self.max_posPAN = self.servo.readWord(self.__SERVO_PAN, self.__VAL_MAX) self.min_posTILT = self.servo.readWord(self.__SERVO_TILT, self.__VAL_MIN) self.max_posTILT = self.servo.readWord(self.__SERVO_TILT, self.__VAL_MAX) # Criando controlador self.__ControllerPan = PID(self.__p_pan, self.__i_pan, self.__d_pan) self.__ControllerPan.setPoint(0) self.__ControllerTilt = PID(self.__p_tilt, self.__i_tilt, self.__d_tilt) self.__ControllerTilt.setPoint(0) self.servo.writeWord(self.__SERVO_PAN, self.__SPEED, self.__min_speed) self.servo.writeWord(self.__SERVO_TILT, self.__SPEED, self.__min_speed) if args.head == True: self.__setVarredura() cv2.namedWindow('Video - Bola') cv2.createTrackbar('P Pan', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('I Pan', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('D Pan', 'Video - Bola', 0, 100, self.__nothing) cv2.createTrackbar('P Tilt', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('I Tilt', 'Video - Bola', 0, 1000, self.__nothing) cv2.createTrackbar('D Tilt', 'Video - Bola', 0, 100, self.__nothing) cv2.createTrackbar('Min vel', 'Video - Bola', 0, 1023, self.__nothing) cv2.createTrackbar('Mod', 'Video - Bola', 0, 3, self.__nothing) #Setando valoeres iniciais cv2.setTrackbarPos('P Pan', 'Video - Bola', int(self.__p_pan * 100)) cv2.setTrackbarPos('I Pan', 'Video - Bola', int(self.__i_pan * 100)) cv2.setTrackbarPos('D Pan', 'Video - Bola', int(self.__d_pan * 1000)) cv2.setTrackbarPos('P Tilt', 'Video - Bola', int(self.__p_tilt * 100)) cv2.setTrackbarPos('I Tilt', 'Video - Bola', int(self.__i_tilt * 100)) cv2.setTrackbarPos('D Tilt', 'Video - Bola', int(self.__d_tilt * 1000)) cv2.setTrackbarPos('Min vel', 'Video - Bola', self.__min_speed) cv2.setTrackbarPos('Mod', 'Video - Bola', 0) self.lastmod = -1
from camera2D import Camera from servo import Servo from sparkfun_board import Sparkfun from development_board import DevBoard from google_cloud import GoogleCloud SERVO_PIN = 0 #14 # or 9 SERVO_ANG = 90 import os # get board type car = Sparkfun() #DevBoard() #Sparkfun() car.enable_en() camera = Camera() cloud = GoogleCloud() servo = Servo() #servo.attach(SERVO_PIN) servo_ang = SERVO_ANG servo.write(SERVO_ANG) class HTTPRequestHandler(BaseHTTPServer.BaseHTTPRequestHandler): def __init__(self, request, client_address, server): self.html = ["Error loading main.html"] with open("main.html", "r") as html: self.html = html.readlines() with open("favicon.ico", "rb") as favicon: self.favicon = favicon.read() BaseHTTPServer.BaseHTTPRequestHandler.__init__(self, request, client_address, server)
def __init__(self, lw_channel, rw_channel): self.lw = Servo(lw_channel) self.rw = Servo(rw_channel)