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)
