def turnAround(): Motors.still() Servo.turn_right() Motors.forward() time.sleep(6) Motors.still() Servo.center()
def straighten_up(self, xShift, yShift, current_speed, adjust_speed,
cameraPos):
"""If landmark or target is detected on the camera, but the servo is not aligned along
the x-axis, align the servo motor while keeping the landmark centered.
Args:
xShift (int): Displacement of landmark from center of frame along x-axis.
yShift (int): Displacement of landmark from center of frame along y-axis.
current_speed (int): Current speed of motors.
adjust_speed (int): Small change of speed to turn while moving.
cameraPos (int): Position of servos holding camera.
Returns:
cameraPos (int): Position of servos holding camera.
"""
camShift = self.x_servo_center - cameraPos[0]
print('sign xShift', np.sign(xShift))
print('sign servo center', np.sign(camShift))
if np.sign(xShift) == -np.sign(camShift) \
and abs(xShift) > self.thresh:
servo_shift = [int(xShift), int(yShift)]
cameraPos = Servo.moveToCenter(servo_shift, cameraPos, 2, 3)
else:
servo_shift = [0, int(yShift)]
cameraPos = Servo.vert_center(servo_shift, cameraPos, 3)
if abs(camShift) > self.servo_thresh:
motor.turn(current_speed, adjust_speed, np.sign(camShift))
else:
motor.turn(current_speed, adjust_speed, np.sign(xShift))
time.sleep(0.2)
motor.brake()
print('yshift', yShift)
print('xShift', xShift)
return cameraPos
def __init__(self, config_file=None):
''' setup channels and basic stuff '''
if self._DEBUG:
print self._DEBUG_INFO, "Debug on"
#self.db = filedb.fileDB()
#self.turning_offset = self.db.get('turning_offset', default_value=0)
self.wheel = Servo(self.STEER_SERVO_ID)
self.wheel.set_base_position(self.STRAIGHT_ANGLE)
self.wheel.set_min_limit(self.MAX_STEER_ANGLE)
self.wheel.set_max_limit(self.MAX_STEER_ANGLE)
if self._DEBUG:
print self._DEBUG_INFO, 'Front wheel PEM channel:', self.STEER_SERVO_ID
print self._DEBUG_INFO, 'Front wheel base value:', self.STRAIGHT_ANGLE
self.angle = {
"left": self.MAX_STEER_ANGLE,
"straight": self.STRAIGHT_ANGLE,
"right": self.MAX_STEER_ANGLE
}
if self._DEBUG:
print self._DEBUG_INFO, 'left angle: %s, straight angle: %s, right angle: %s' % (
self.angle["left"], self.angle["straight"],
self.angle["right"])
def turnLeft(): Motors.still() Servo.turn_left() Motors.forward() time.sleep(3) Motors.still() Servo.center()
def __init__(self):
self.glz_name = rospy.get_param('GLZ_NAME', 'GLZ01')
self.pin_config = {
"GPIO_FIRE_PIN": rospy.get_param('cannon_move_py/GPIO_FIRE_PIN',
4),
"GPIO_YAW_PIN": rospy.get_param('cannon_move_py/GPIO_YAW_PIN', 12),
"GPIO_PITCH_PIN": rospy.get_param('cannon_move_py/GPIO_PITCH_PIN',
13)
}
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.pin_config["GPIO_FIRE_PIN"], GPIO.OUT)
self.yaw_servo = Servo(PIN=self.pin_config["GPIO_YAW_PIN"],
mode="HARDWARE")
self.pitch_servo = Servo(PIN=self.pin_config["GPIO_PITCH_PIN"],
mode="HARDWARE")
rospy.init_node('cannon_move_listener', anonymous=True)
rospy.Subscriber("/" + self.glz_name + "/cannon/move", Twist,
self.cannon_callback)
rospy.Subscriber("/" + self.glz_name + "/cannon/fire", Bool,
self.fire_callback)
rospy.loginfo("Node cannon_move_listener has started")
rospy.spin()
self.stop()
GPIO.cleanup()
def __init__(self):
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
self.GPIO_4 = 4
GPIO.setup(self.GPIO_4, GPIO.OUT)
GPIO.output(self.GPIO_4, False)
self.imu = IMU()
self.servo = Servo()
self.move_flag = 0x01
self.relax_flag = False
self.pid = Incremental_PID(0.500, 0.00, 0.0025)
self.flag = 0x00
self.timeout = 0
self.height = -25
self.body_point = [[137.1, 189.4, self.height], [225, 0, self.height],
[137.1, -189.4, self.height],
[-137.1, -189.4,
self.height], [-225, 0, self.height],
[-137.1, 189.4, self.height]]
self.calibration_leg_point = self.readFromTxt('point')
self.leg_point = [[140, 0, 0], [140, 0, 0], [140, 0, 0], [140, 0, 0],
[140, 0, 0], [140, 0, 0]]
self.calibration_angle = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0]]
self.angle = [[90, 0, 0], [90, 0, 0], [90, 0, 0], [90, 0, 0],
[90, 0, 0], [90, 0, 0]]
self.order = ['', '', '', '', '', '']
self.calibration()
self.setLegAngle()
self.Thread_conditiona = threading.Thread(target=self.condition)
def right():
Motors.still()
Servo.turn_right()
Motors.forward()
time.sleep(3)
Motors.still()
Servo.center()
def setup():
global xAxis, yAxis, minPulse, midPulse, maxPulse
minPulse = 150
midPulse = 350
maxPulse = 600
xAxis = Servo(0, minPulse, maxPulse, 60)
yAxis = Servo(1, minPulse, maxPulse, 60)
cleanStart()
def __init__(self):
self.tcp_flag = False
self.led = Led()
self.adc = ADS7830()
self.servo = Servo()
self.buzzer = Buzzer()
self.control = Control()
self.sonic = Ultrasonic()
self.control.Thread_conditiona.start()
def servoturn():
servo = Servo("s1")
servo.attach(3)
time.sleep(0.05)
# From 0 to 180 degrees
for angle in range(0,180):
servo.write(angle)
time.sleep(0.05)
def __init__(self):
"""init Scanner und Servo"""
self.sonar1 = Sonar()
self.run_scan = 0
self.scan_list = []
self.servo = Servo()
self.servo_step = 1
self.lidar = Lidar_Lite()
self.lidar.connect(1)
print("Init Scanner, Servo")
def __init__(self):
SimpleCatapult.__init__(self) #Run the superclass initialization method
self.TENSION_SERVO_ADDRESS = 0
self.ANGLE_SERVO_ADDRESS = 1
self.YAW_SERVO_ADDRESS = 2
self.LOCK_SERVO_ADDRESS = 3
#Define the controller
self.controller = ServoController(0x41)
#Make reference to servos
self.tension_servo = Servo(self.controller,self.TENSION_SERVO_ADDRESS)
self.angle_servo = Servo(self.controller,self.ANGLE_SERVO_ADDRESS)
self.yaw_servo = Servo(self.controller,self.YAW_SERVO_ADDRESS)
self.lock_servo = Servo(self.controller,self.LOCK_SERVO_ADDRESS)
#Servo thresholds
self.TENSION_MIN = 60
self.TENSION_MAX = 90
self.ANGLE_MIN = 0
self.ANGLE_MAX = 50
self.YAW_MIN = 12
self.YAW_MAX = 79
#Servo presets
self.NOTENSION = 0
self.LOCKED = 0
self.UNLOCKED = 50
self.POST_LOCK_WAIT = 0.25
self.POST_UNLOCK_WAIT = 0.5
self.ANGLE_EASE_OFF_WAIT = 0.5
self.ANGLE_EASE_OFF = 25
self.TENSION_REMOVE_WAIT = 1.5
#Defaults
self.angle = (self.ANGLE_MIN + self.ANGLE_MAX)/2
self.yaw = (self.YAW_MIN + self.YAW_MAX)/2
self.tension = self.TENSION_MIN
#Set initial servo position
self.removeTension()
self.applyAngle()
self.applyYaw()
self.removeLock()
#Timed Disarm Thread
seconds = 60
self.count = seconds * 10
self.timedDisarmPreExec = None #should be set externally
self.timedDisarmPostExec = None #should be set externally
self.timedDisarmBreak = False
def __init__(self, globalGPIO, config):
self._gpioLeftServoControl = config.gripper.gpioLeftServoControl #27
self._gpioRightServoControl = config.gripper.gpioRightServoControl #22
self._leftServo = Servo.ServoDS3218_180(globalGPIO,
self._gpioLeftServoControl)
self._rightServo = Servo.ServoDS3218_180(globalGPIO,
self._gpioRightServoControl)
self._rightServoCloseAngle = config.gripper.rightServoCloseAngle #106
self._rightServoOpenAngle = config.gripper.rightServoOpenAngle #100
self._rightServoCurrentAngle = -1
self._leftServoCloseAngle = config.gripper.leftServoCloseAngle #73
self._leftServoOpenAngle = config.gripper.leftServoOpenAngle #79
self._leftServoCurrentAngle = -1
def moveServo(self):
if self.rotation == "clockwise":
self.position += self.index
if self.position <= s.ubAngle:
print("current position: ", self.position)
else:
self.rotation = "counterClockwise"
self.position = s.ubAngle
else:
if self.position >= s.lbAngle:
self.position -= self.index
print("current position: ", self.position)
Servo.baseServo(s.baseservoPin, self.position)
def main():
#print('Left Sensor ', LeftSensor.getLastEvent(), '\t Center Sensor ', CenterSensor.getLastEvent(), '\t Right Sensor ', RightSensor.getLastEvent())
#time.sleep(1)
Motors.forward()
if LeftSensor.getLastEvent() >= 500 and RightSensor.getLastEvent() >= 500:
#Execute ppark
Motors.still()
Motors.forward()
time.sleep(1)
Motors.still()
Servo.turn_left()
Motors.backward()
time.sleep(3)
Servo.center()
Servo.turn_right()
Motors.backward()
time.sleep(2)
Motors.still()
Motors.forward()
time.sleep(2)
Motors.still()
Servo.center()
os.system("flite -t 'Delta Task ended'")
LeftSensor.stop()
CenterSensor.stop()
RightSensor.stop()
def __init__(self):
# Servos connected to PWM pins of the Beagle Bone Black.
# Edit this according to the plane connections.
# Maximum number of servos allowed = 4
self.Ailerons_Right = Servo("P8_13")
self.Ailerons_Left = Servo("P9_14")
self.Flaps = Servo("P9_21")
self.Elevators = Servo("P9_42")
#self.Radar = Servo("")
self.Ailerons_Right.StartServo()
self.Ailerons_Left.StartServo()
self.Flaps.StartServo()
self.Elevators.StartServo()
# Set makimum titl angles of all servos.
# Edit this portion according to the plane being used
self.alireons_max = 30
self.falps_max = 30
self.elevators_max = 45
self.radar_max = 45
# Exponential componenet of the movement.
# 1 - Linear,
self.alireons_exp = 2
self.elevators_exp = 2
self.radar_exp = 3
# Throttle specific data
self.engines = 1
def __init__(self):
self.offset = 15
self.position = 90
self.max_right = 135
self.max_left = 45
self.servo = Servo.Servo(0)
self.servo.setup()
def main() :
a = Servo.servo()
b = degree.acc()
global count
#f = open("data.txt", 'w')
que = []
#timecheck_list = []
every5sec()
every1sec()
#timecheck_list.append(time.time())
while(True):
a.servo_1(pwm_1)
a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
#timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
#timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, b.pitch())
f.write(data)
"""
que.append(b.pitch())
if(len(que) == 10):
print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, sum(que,0.0)/len(que), count)
count += 1
que.pop(0)
def __init__(self,
debug=False,
db="config",
bus_number=1,
channel=FRONT_WHEEL_CHANNEL):
''' setup channels and basic stuff '''
self.db = filedb.fileDB(db=db)
self._channel = channel
self._straight_angle = 90
self.turning_max = 20
self._turning_offset = int(
self.db.get('turning_offset', default_value=0))
self.wheel = Servo.Servo(self._channel,
bus_number=bus_number,
offset=self.turning_offset)
self.debug = debug
if self._DEBUG:
print self._DEBUG_INFO, 'Front wheel PWM channel:', self._channel
print self._DEBUG_INFO, 'Front wheel offset value:', self.turning_offset
self._angle = {
"left": self._min_angle,
"straight": self._straight_angle,
"right": self._max_angle
}
if self._DEBUG:
print self._DEBUG_INFO, 'left angle: %s, straight angle: %s, right angle: %s' % (
self._angle["left"], self._angle["straight"],
self._angle["right"])
def main():
a = Servo.servo()
b = degree.acc()
f = open("data.txt", 'w')
count = 0
que = []
que_time = []
every5sec()
while (True):
a.servo_1(pwm_1)
a.servo_2(pwm_2)
que.append(b.pitch())
que_time.append(time.time())
if (count >= 1):
print "sensor time == ", que_time[count] - que_time[count - 1]
count += 1
if (len(que) == 100):
average_pitch = sum(que, 0.0) / len(que)
print "pwm_v1 = %s pwm_v2 = %s degree = %s " % (pwm_1, pwm_2,
average_pitch)
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2,
average_pitch)
f.write(data)
#que.pop(0)
que[0:10] = []
def read_config(self, filename):
if self.verbose:
print "We are in config\n"
frame_data = []
current_frame_list = []
output_servo_list = []
with open(filename, 'r') as config:
line = config.readline()
while not ((line == '!end') or (line == '!END')):
self.debug(line)
datum = line.split()
self.debug(datum)
if len(datum) > 0:
if (datum[0] == '#'):
label = datum[1]
tag = int(datum[2])
temp_servo = S.Servo(self.device, label, tag,
int(datum[3]), int(datum[4]),
int(datum[5]), int(datum[6]))
output_servo_list.append(temp_servo)
if (datum[0] == '!'):
if self.trigger:
timer = 0
else:
timer = int(datum[6])
frame_data = [
int(datum[1]),
int(datum[2]),
int(datum[3]),
int(datum[4]),
int(datum[5]), timer
]
current_frame_list.append(frame_data)
line = config.readline()
return output_servo_list, current_frame_list
def drone_play(mainDQN):
global init_pwm_1
global init_pwm_2
a = Servo.servo()
pwm_left = init_pwm_1
pwm_right = init_pwm_2
while True:
memory_semaphore.acquire(10)
degree = memory_degree.read()
acc_gyro_pitch = float(degree.rstrip('\x00'))
ang_vel = memory_ang_vel.read()
p_ang_vel = float(ang_vel.rstrip('\x00'))
acc_degree = memory_acc_degree.read()
acc_pitch = float(acc_degree.rstrip('\x00'))
memory_semaphore.release()
state = np.array([acc_gyro_pitch, p_ang_vel, pwm_left, pwm_right])
print "\t\t\t<state> degree: %s, \tangular velocity: %s" % (state[0],
state[1])
action = np.argmax(mainDQN.predict(state))
pwm_left, pwm_right = step_action(action, pwm_left, pwm_right)
print "\t\t\t\t\t\t\t\t\t\t<action-motor> left: %s, right: %s <= %s" % (
pwm_left, pwm_right, action_print(action))
a.servo_1(pwm_left)
a.servo_2(pwm_right)
time.sleep(0.01)
class Mode:
y = x.Ax12()
servo = Servo.Servo()
def __init__(self):
self.md = 0
pass
def init_modes(self, key, value):
"""__________________DRIVING MODE_______________"""
if self.md == '0':
self.servo.reset_servos(
) # go to the start position(maybe not needed to call this every time)
self.servo.move_all_servos(key, value)
"""_________________DANCING MODE_________________"""
if self.md == '3':
string = self.ser.readline()
lowValue = string[1]
middleValue = string[2]
highValue = string[3]
"""________________LINE DANCE MODE_______________"""
if self.md == '1':
pass
"""________________LINE DETECTION________________"""
if self.md == '4':
LineDetection().send_value_canon()
"""_____________TRANSPORT AND REBUILD____________"""
if self.md == '5':
pass # kinematics and Camera here
def change_mode(self, value):
self.md = value
def conServo():
a = Servo.servo()
global pitch_gyro
global acc_pitch
global acc_gyro_pitch
if (acc_gyro_pitch <= 180 and acc_gyro_pitch > 5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s " % (
pwm_1 + (1.0 / 81000.0) * pow(acc_gyro_pitch, 2), pwm_2,
acc_gyro_pitch, pitch_gyro, acc_pitch)
#print "180down"
elif (acc_gyro_pitch > 180 and acc_gyro_pitch < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360 - acc_gyro_pitch, 2))
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s " % (
pwm_1, pwm_2 + (7.0 / 648000.0) * pow(360 - acc_gyro_pitch, 2),
acc_gyro_pitch, pitch_gyro, acc_pitch)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s \t\t degree = C: %s\t<-\tG: %s vs A: %s " % (
pwm_1, pwm_2, acc_gyro_pitch, pitch_gyro, acc_pitch)
def main() :
a = Servo.servo()
b = degree.acc()
global count
#f = open("data.txt", 'w')
#que = []
timecheck_list = []
every5sec()
every1sec()
timecheck_list.append(time.time())
while(True):
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
timecheck_list.pop(0)
count += 1
"""
def __init__(self, servo):
self.Servo = servo
self.Buffer = Sequencer_Buffer(self)
self.Thread = Sequencer_Thread(self)
self.ServoSeq = Servo.Servo_Sequencer(self.Servo)
self.ServoSeq.daemon = True;
self.ServoSeq.start()
def main():
try:
loop()
except KeyboardInterrupt: #when 'Ctrl+C' is pressed, the program will exit
DataQueue.Quit()
MySql.Quit()
Servo.Quit()
Anova.Quit()
def configLegs(connexion=Connexion()):
Leg.a = 72
Leg.b = 100
Leg.a1 = 0
Leg.a2 = 0
Leg.ab0 = 0
Leg.ab1 = 0
Leg.ab2 = 0
Leg.liftTime = 5.0
Leg.forwardTime = 5.0
Leg.pullTime = 5.0
#define SERVO_BR1 0
#define SERVO_BR2 1
#define SERVO_BL1 2
#define SERVO_BL2 3
#define SERVO_FM1 4
#define SERVO_FR1 5
#define SERVO_FR2 6
#define SERVO_FR3 7
#define SERVO_FR4 8
#define SERVO_FL1 9
#define SERVO_FL2 10
#define SERVO_FL3 11
BR1 = 0
BR2 = 1
BL1 = 2
BL2 = 3
FM1 = 4
FR1 = 5
FR2 = 6
FR3 = 7
FR4 = 8
FL1 = 9
FL2 = 10
FL3 = 11
return (Leg(
(Servo(connexion, BR1, offset=90), Servo(connexion, BR2, offset=-90)),
LegType.BACK),
Leg((Servo(connexion, BL1, offset=90), Servo(connexion, BL2)),
LegType.BACK))
"""
def __init__(self, debug=False, bus_number=1, db="config", address=0x40):
''' Init the servo channel '''
self.db = filedb.fileDB(db=db)
self.pan_offset = int(self.db.get('pan_offset', default_value=0))
self.tilt_offset = int(self.db.get('tilt_offset', default_value=0))
self.pan_servo = Servo.Servo(self.pan_channel, bus_number=bus_number, offset=self.pan_offset, address= address )
self.tilt_servo = Servo.Servo(self.tilt_channel, bus_number=bus_number, offset=self.tilt_offset, address = address)
self.debug = debug
if self._DEBUG:
print self._DEBUG_INFO, 'Pan servo channel:', self.pan_channel
print self._DEBUG_INFO, 'Tilt servo channel:', self.tilt_channel
print self._DEBUG_INFO, 'Pan offset value:', self.pan_offset
print self._DEBUG_INFO, 'Tilt offset value:', self.tilt_offset
self.current_pan = 0
self.current_tilt = 0
self.ready()
def getPlateInfo():
try:
plate = request.form['plate']
data = firestore.getInfoByPlate(plate)
Servo.servo()
LCD.LCD(plate)
response = app.response_class(status=200,
response=json.dumps(data),
mimetype='application/json')
return response
except Exception as identifier:
LCD.LCD('Ban chua dang ki')
print("get errors")
print(identifier)
return app.response_class(status=500,
response=json.dumps(
{'message': 'Internal server error'}),
mimetype='application/json')
def main():
pitch_aver = 180
a = Servo.servo()
b = degree.acc()
global count
#f = open("data.txt", 'w')
que = []
#timecheck_list = []
# every5sec()
every1sec()
#timecheck_list.append(time.time())
while (True):
#a.servo_1(pwm_1)
#a.servo_2(pwm_2)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, b.pitch(), count)
#timecheck_list.append(time.time())
#print "loop time : %s" % (timecheck_list[1] - timecheck_list[0])
#timecheck_list.pop(0)
#count += 1
"""
data = "pwm_v1 = %s pwm_v2 = %s degree = %s \n" % (pwm_1, pwm_2, b.pitch())
f.write(data)
"""
que.append(b.pitch())
if (len(que) == 100):
pitch_aver = sum(que, 0.0) / len(que)
#print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (pwm_1, pwm_2, pitch_aver, count)
count += 1
que.pop(0)
if (pitch_aver <= 180 and pitch_aver > 5):
a.servo_1(pwm_1 + (1.0 / 81000.0) * pow(pitch_aver, 2))
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (
pwm_1 +
(1.0 / 81000.0) * pow(pitch_aver, 2), pwm_2, pitch_aver, count)
#print "180down"
elif (pitch_aver > 180 and pitch_aver < 355):
a.servo_1(pwm_1)
a.servo_2(pwm_2 + (7.0 / 648000.0) * pow(360 - pitch_aver, 2))
print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (
pwm_1, pwm_2 +
(7.0 / 648000.0) * pow(360 - pitch_aver, 2), pitch_aver, count)
#print "180up"
else:
a.servo_1(pwm_1)
a.servo_2(pwm_2)
print "pwm_v1 = %s pwm_v2 = %s degree = %s ---- count : %s" % (
pwm_1, pwm_2, pitch_aver, count)
def facial_rec():
out_dir = None
servo = Servo("s1")
servo.attach(3) #attaches to pin 3 in pwm
print("New picture has been found in library")
# if len(sys.argv) < 2:
# print "USAGE: facerec_demo.py </path/to/images> [</path/to/store/images/at>]"
# sys.exit()
# Now read in the image data. This must be a valid path!
[X,y] = read_images(sys.argv[1], (200, 100))
face_cascade = cv2.CascadeClassifier("haarcascade_frontalface_default.xml")
img = cv2.imread(os.path.join(sys.argv[1], 'face.png'))
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)
for (p,q,r,s) in faces:
cv2.rectangle(gray,(p,q),(p+r,q+s),(150,125,0),2)#drawing a rectangle indicating face
sample = gray[q:q+s, p:p+r]
# resize to given size (if given)
sample = cv2.resize(gray, (200,100))
sIm = np.frombuffer(sample, dtype=np.uint8)
y = np.asarray(y, dtype=np.int32)
if len(sys.argv) == 3:
out_dir = sys.argv[2]
model = cv2.createFisherFaceRecognizer()
model.train(np.asarray(X), np.asarray(y))
[p_label, p_confidence] = model.predict(np.asarray(sIm)) #replace sIm with another
print "Predicted label = %d (confidence=%.2f)" % (p_label, p_confidence)
if(p_confidence > 0):
print "Face does not match"
blinkLed = mraa.Gpio(LED_GPIO3) # Get the LED pin object
blink(blinkLed)
else:
print "Face Matches!!!!!"
servoturn()
if out_dir is None:
cv2.waitKey(0)
class CannonMove():
def __init__(self):
self.glz_name = rospy.get_param('GLZ_NAME', 'GLZ01')
self.pin_config = {
"GPIO_FIRE_PIN": rospy.get_param('cannon_move_py/GPIO_FIRE_PIN',
4),
"GPIO_YAW_PIN": rospy.get_param('cannon_move_py/GPIO_YAW_PIN', 12),
"GPIO_PITCH_PIN": rospy.get_param('cannon_move_py/GPIO_PITCH_PIN',
13)
}
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.pin_config["GPIO_FIRE_PIN"], GPIO.OUT)
self.yaw_servo = Servo(PIN=self.pin_config["GPIO_YAW_PIN"],
mode="HARDWARE")
self.pitch_servo = Servo(PIN=self.pin_config["GPIO_PITCH_PIN"],
mode="HARDWARE")
rospy.init_node('cannon_move_listener', anonymous=True)
rospy.Subscriber("/" + self.glz_name + "/cannon/move", Twist,
self.cannon_callback)
rospy.Subscriber("/" + self.glz_name + "/cannon/fire", Bool,
self.fire_callback)
rospy.loginfo("Node cannon_move_listener has started")
rospy.spin()
self.stop()
GPIO.cleanup()
def cannon_callback(self, data):
# rospy.loginfo(data)
if (0 <= data.angular.y <= 180):
self.yaw_servo.write(data.angular.y)
if (0 <= data.angular.z <= 80):
self.pitch_servo.write(data.angular.z)
def fire_callback(self, data):
# rospy.loginfo(data)
if (data.data):
GPIO.output(self.pin_config["GPIO_FIRE_PIN"], GPIO.HIGH)
else:
GPIO.output(self.pin_config["GPIO_FIRE_PIN"], GPIO.LOW)
def stop(self):
self.yaw_servo.stop()
self.pitch_servo.stop()
print("Servo Stop")
rospy.loginfo("Servo Stop")
def takePics():
'''
Returns a list of pictures taken at positions specified in teh global parameter CAMERA_SERVOS
'''
# Create servo objects
servo1 = Servo(1)
servo2 = Servo(2)
# Initialize picture list
pics = []
# Take 5 pictures, moving camera between
for i in range(params.p['CAMERA_SERVOS']):
servo1.setServo(params.p['CAMERA_SERVOS'][i][0])
servo2.setServo(params.p['CAMERA_SERVOS'][i][1])
pics.append(camera.capture(rawCapture, format="rgb"))
# Return array of images
return pics
# -*- coding: utf-8 -*-
from Servo import *
import time
import socket
HOST = '192.168.1.35' # Endereco IP do Servidor
PORT = 5000 # Porta que o Servidor esta
udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
orig = (HOST, PORT)
udp.bind(orig)
# Create a new servo object with a reference name
myServo = Servo("First Servo")
# Attaches the servo to pin 3 in Arduino Expansion board
myServo.attach(3)
angle = myServo.read()
def left(angle):
for i in range (angle, angle-45, -1):
myServo.write(i)
return(angle-45)
def right(angle):
for i in range (angle, angle+45):
myServo.write(i)
myLCD.setCursor(0, 0)
myLCD.write("FaceLock")
myLCD.setCursor(1, 0)
myLCD.write("Press button")
servo.write(0)
BUTTON_PIN = 2
TRAINING_BUTTON_PIN = 8
button = mraa.Gpio(BUTTON_PIN)
training_button = mraa.Gpio(TRAINING_BUTTON_PIN)
button.dir(mraa.DIR_IN)
training_button.dir(mraa.DIR_IN)
## The LCD can only display 16 characters per line
myLCD = lcd.Jhd1313m1(0, 0x3E, 0x62)
servo = Servo("Lock")
servo.attach(3)
greeting()
while True:
if button.read():
authenticate()
if training_button.read():
training_interface.begin_training()
greeting()
time.sleep(0.1)
This example code is in the public domain.
Revision History
----------------------------------------------------------
Author Date Description
----------------------------------------------------------
Diego Villalobos 02-12-2015 Example created
"""
# Libraries required
from Servo import *
import time
# Create a new servo object with a reference name
myServo = Servo("First Servo")
# Attaches the servo to pin 3 in Arduino Expansion board
myServo.attach(3)
# Print servo settings
print ""
print "*** Servo Initial Settings ***"
print myServo
print ""
try:
# Sweeps the servo motor forever
while True:
# From 0 to 180 degrees
for angle in range(0,180):
a candy as reward.
It is driven by opencv. The programme take pictures of the token and
match them to a pre-defined set(done by combiLock_train.py).
'''
import imageRecognizer as recon
from Servo import *
import mraa
import time
led_pin_green=4
led_pin_red=5
led_pin_light=7
btn_pin=6
myServo = Servo("The lock")
myServo.attach(3)
iCorrect=True
def unlock():
myServo.write(90)
time.sleep(2)
myServo.write(0)
if __name__=="__main__":
led_green=mraa.Gpio(led_pin_green)
led_green.dir(mraa.DIR_OUT)
led_green.write(0)
def main():
servo1 = Servo("First Servo")
servo1.attach(5)
servo2 = Servo("Second Servo")
servo2.attach(3)
servo1.write(86)
servo2.write(85)
servo3 = Servo("Third Servo")
servo3.attach(6)
servo3.write(87)
oldDoor = "False"
while 1:
# print "shoot"
# shoot(servo1,servo2)
# time.sleep(2)
print "start"
try:
match, ready, confidence, door = callApi()
print "good"
print "door", door
if door != oldDoor:
print door,
print oldDoor
oldDoor = door
moveDoor(door, servo3)
if ready == "True":
if match == "False" or confidence < 0.58:
shoot(servo1, servo2)
print "shoot"
except:
print "bad"
""" test PWM """ from bbio import * from Servo import * import time servo1 = Servo(PWM1A) servo2 = Servo(PWM2A) servo1.write(90) servo2.write(90) time.sleep(2) servo1.detach() servo2.detach()
class ServoCatapult(SimpleCatapult):
def __init__(self):
SimpleCatapult.__init__(self) #Run the superclass initialization method
self.TENSION_SERVO_ADDRESS = 0
self.ANGLE_SERVO_ADDRESS = 1
self.YAW_SERVO_ADDRESS = 2
self.LOCK_SERVO_ADDRESS = 3
#Define the controller
self.controller = ServoController(0x41)
#Make reference to servos
self.tension_servo = Servo(self.controller,self.TENSION_SERVO_ADDRESS)
self.angle_servo = Servo(self.controller,self.ANGLE_SERVO_ADDRESS)
self.yaw_servo = Servo(self.controller,self.YAW_SERVO_ADDRESS)
self.lock_servo = Servo(self.controller,self.LOCK_SERVO_ADDRESS)
#Servo thresholds
self.TENSION_MIN = 60
self.TENSION_MAX = 90
self.ANGLE_MIN = 0
self.ANGLE_MAX = 50
self.YAW_MIN = 12
self.YAW_MAX = 79
#Servo presets
self.NOTENSION = 0
self.LOCKED = 0
self.UNLOCKED = 50
self.POST_LOCK_WAIT = 0.25
self.POST_UNLOCK_WAIT = 0.5
self.ANGLE_EASE_OFF_WAIT = 0.5
self.ANGLE_EASE_OFF = 25
self.TENSION_REMOVE_WAIT = 1.5
#Defaults
self.angle = (self.ANGLE_MIN + self.ANGLE_MAX)/2
self.yaw = (self.YAW_MIN + self.YAW_MAX)/2
self.tension = self.TENSION_MIN
#Set initial servo position
self.removeTension()
self.applyAngle()
self.applyYaw()
self.removeLock()
#Timed Disarm Thread
seconds = 60
self.count = seconds * 10
self.timedDisarmPreExec = None #should be set externally
self.timedDisarmPostExec = None #should be set externally
self.timedDisarmBreak = False
#override
def __repr__(self):
return (SimpleCatapult.__repr__(self))
#override
def applyAngle(self):
if self.angle < self.ANGLE_MIN:
self.angle = self.ANGLE_MIN
elif self.angle > self.ANGLE_MAX:
self.angle = self.ANGLE_MAX
SimpleCatapult.applyAngle(self)
self.angle_servo.changePositionPercent(self.angle)
#override
def applyYaw(self):
if self.yaw < self.YAW_MIN:
self.yaw = self.YAW_MIN
elif self.yaw > self.YAW_MAX:
self.yaw = self.YAW_MAX
SimpleCatapult.applyYaw(self)
self.yaw_servo.changePositionPercent(self.yaw)
#override
def applyTension(self):
if self.tension < self.TENSION_MIN:
self.tension = self.TENSION_MIN
elif self.tension > self.TENSION_MAX:
self.tension = self.TENSION_MAX
SimpleCatapult.applyTension(self)
self.tension_servo.changePositionPercent(self.tension)
#override
def removeTension(self):
angle = self.angle
SimpleCatapult.removeTension(self)
if self.angle > self.ANGLE_EASE_OFF:
self.angle = self.ANGLE_EASE_OFF
self.applyAngle()
sleep(self.ANGLE_EASE_OFF_WAIT)
self.tension_servo.changePositionPercent(self.NOTENSION)
self.angle = angle
self.applyAngle()
sleep(self.TENSION_REMOVE_WAIT)
#override
def applyLock(self):
SimpleCatapult.applyLock(self)
self.lock_servo.changePositionPercent(self.LOCKED)
sleep(self.POST_LOCK_WAIT)
self.timedDisarmBreak = False
thread.start_new_thread(self.timedDisarm, (None,))
#override
def removeLock(self):
self.timedDisarmBreak = True
SimpleCatapult.removeLock(self)
self.lock_servo.changePositionPercent(self.UNLOCKED)
sleep(self.POST_UNLOCK_WAIT)
#This method is used to disarm the catapult if it has been armed for
#the time threshold of self.count * 0.1 seconds. The purpose of this
#method is to reduce the stress on the tension servo by only allowing
#it to be in a tense position for a short duration. The catapult is
#disarmed after the time threshold is reached.
#The method also has provision to call a user defined pre-method and
#post-method to allow for handling of the disarm. For example, these
#pre and post methods can be used by an UI to lock specific UI actions
#until the disarm method is complete.
def timedDisarm(self, junk):
count = 0
while count <= self.count:
if self.timedDisarmBreak:
if self.DEBUG:
print("SimpleCatapult.timedDisarm received exit signal.")
return
count += 1
time.sleep(0.1)
try:
self.timedDisarmPreExec()
except:
if self.DEBUG:
print("Warning: ServoCatapult.timedDisarm.PreExec not callable.")
self.disarm()
try:
self.timedDisarmPostExec()
except:
if self.DEBUG:
print("Warning: ServoCatapult.timedDisarm.PostExec not callable.")