def __init__(
self,
dev_name,
servo_id,
baudrate=1000000,
offset=0,
min_angle=0,
max_angle=299,
max_rpm=30,
flipped=1,
type=None,
):
""" dev_name - name of serial device of the servo controller (e.g. '/dev/ttyUSB0')
servo_id - 1,2,3,4 ... (range: 1 to 253)
baudrate - for the servo controller. (default: 1000000)
offset - offset of robotis 0 angle to input 0 angle (default: 0, range: 0-300)
min_angle - min valid input angle. (default: 0, range: 0-300)
max_angle - max valid input angle. (default: 300, range: 0-300)
max_rpm - max valid rpm input speed (default: 113, range: 0-113)
flipped - Set -1 if you wish to reverse angle orienation (default: 1)
"""
# offset within hardware angle bounds
# min is less then max
# min is above or equal the hardware lower bound
# max is below or equal the hardware upper bound
# max rpm falls within hardware bounds
if (
offset >= 0
and offset < 300
and min_angle >= 0
and min_angle < 360
and max_angle >= 0
and max_angle < 360
and min_angle < max_angle
and max_rpm >= 0
and max_rpm <= 113
and (flipped == 1 or flipped == -1)
):
limits = [(min_angle - offset) * flipped + offset, (max_angle - offset) * flipped + offset]
self.offset = offset
self.min_angle = min(limits)
self.max_angle = max(limits)
self.max_rpm = max_rpm
self.flipped = flipped
else:
print "WARNING: parameters are invalid. Halting operation."
raise ValueError("invalid servo parameters")
if type == "EX-106":
self.servo = servo.servo_ex106(dev_name, servo_id)
else:
self.servo = servo.servo(dev_name, servo_id)
self.servo_write("speed", self.max_rpm)
self.softBlockingTolerance = 3
self.id = servo_id
def __init__(self, cfg, mqtt_client = None):
"""Class Constructor ... called when object instance created.
Arguments:
self - object being created
cfg - pointer to config.YAMLConfig class instance
"""
self.logger = logging.getLogger(self.__class__.__name__)
self.mqtt_client = mqtt_client
self.cfg = cfg
# setup RPi GPIO
# set up GPIO using BCM numbering
GPIO.setmode(GPIO.BCM)
for pin in cfg["Pins"]:
if (cfg["Pins"][pin]["type"] == "IN"):
GPIO.setup(pin, GPIO.IN,
pull_up_down = getattr(GPIO, cfg["Pins"][pin]["pull_up_down"]))
GPIO.add_event_detect(pin, GPIO.BOTH, callback=self.inputStateChange,
bouncetime=100)
elif (cfg["Pins"][pin]["type"] == "OUT"):
GPIO.setup(pin, GPIO.OUT)
GPIO.output(pin, True)
time.sleep(1)
GPIO.output(pin,False)
elif (cfg["Pins"][pin]["type"] == "SERVO"):
cfg["Pins"][pin]["__SERVO"] = servo.servo(pin, cfg.getBoolean("Pins."+str(pin)+".continuous"))
self.logger.info("Setting up pin %d as %s", pin, cfg["Pins"][pin]["type"])
def main(): while True: queue = Queue() camt0 = camera_task(queue, 0, 0, 'len.jpeg') camt0.daemon = True #camt = camera_task(queue, 0, 1) #camt.daemon = True camt0.start() #camt.start() s = queue.get() print s lk = lockObject() sv = servo() ret = lk.draw_qrcode(s) if ret != False: print 'ok!!! you have picked your stuff successfully!!!!!' lp = subprocess.call(['sudo', './light']) # require a light execute time.sleep(2) sv.open() lk.remove_item(ret) else: print 'no!!! Is that the right qrcode b**ch????' #camt.stop() camt0.stop() raw_input()
def __init__(self, conf_path):
config = ConfigParser.ConfigParser()
config.read(conf_path)
self.dev_id = int(config.get("signalConfig", "devID"))
self.mgmt_sleep = int(config.get("signalConfig", "mgmtSleep"))
self.op_sleep = int(config.get("signalConfig", "opSleep"))
self.server_ip = config.get("signalConfig", "serverIP")
self.server_port = str(int(config.get("signalConfig", "serverPort")))
self.dev_type = config.get("signalConfig", "devType")
self.rest_addr = self.server_ip + ":" + self.server_port
self.url = "http://" + self.rest_addr + "/dev/" + str(self.dev_id)
init_pos = config.get("servoConfig", "servoXY")
init_pos = init_pos.split(",")
init_pos = [int(init_pos[0]), int(init_pos[1])]
self.des_queue_x = Queue()
self.des_queue_y = Queue()
self.cur_queue_x = Queue()
self.cur_queue_y = Queue()
self.blaster = None
# debug **
# self.debug_count = 10
# self.debug_url = "http://"+"127.0.0.1:8000"+"/dev/"+str(self.dev_id)
# self.debug_report_url = self.debug_url + "/report"
# debug **
lock = Lock()
self.servo_x = None
self.servo_y = None
self.step = 0
if (self.dev_type == "pi"):
self.blaster = open('/dev/servoblaster', 'w')
self.servo_x = servo(config, 0, self.blaster, lock,
self.des_queue_x, self.cur_queue_x)
self.servo_y = servo(config, 1, self.blaster, lock,
self.des_queue_y, self.cur_queue_y)
self.step = int(config.get("servoConfig", "step"))
self.process = Process(target=self.signal_channel,
args=(self.dev_type, init_pos,))
self.process.start()
def makeJoints(self, dev_name):
self.joints = [self.makeJoint(dev_name, 0, offset=150, min_angle=0, max_angle=300, max_rpm=30),
self.makeJoint(dev_name, 1, offset=127.1, min_angle=61, max_angle=233.6, max_rpm=30, type='EX-106'),
self.makeJoint(dev_name, 2, offset=150, min_angle=0, max_angle=300, max_rpm=30),
self.makeJoint(dev_name, 3, offset=150, min_angle=54.25, max_angle=262, max_rpm=30),
self.makeJoint(dev_name, 4, offset=150, min_angle=0, max_angle=300, max_rpm=30),
self.makeJoint(dev_name, 5, offset=150, min_angle=35.2, max_angle=262, max_rpm=30),
self.makeJoint(dev_name, 6, offset=150, min_angle=0, max_angle=300, max_rpm=30),
self.makeJoint(dev_name, 7, offset=60, min_angle=45, max_angle=120, max_rpm=30)]
self.all = servo.servo(dev_name,254)
if set(self.joints) == set([None]):
raise RuntimeError('Arm: no servos found')
self.claw = len(self.joints)-1
self.ts=.03
self.torqued=False
# self.set_torque_all(True)
# time.sleep(.1)
self.set_rpm_all(6)
self.set_torque_all(False)
def do_GET(s):
"""Respond to a GET request."""
ulk = lockObject()
sv = servo()
s.send_response(200)
s.send_header("Content-type", "text/html")
s.end_headers()
gv = s.path.split('?')
print gv
if (len(gv) > 1):
print "gv len=" + str(len(gv))
gvd = urlparse.parse_qs(gv[1])
if 'uid' in gvd and 'bid' in gvd:
print gvd['uid'][0], gvd['bid'][0]
qr = ulk.add_item(gvd['uid'][0], gvd['bid'][0])
if qr != False:
print "http://lo.wows.tech/qrcode/?data=" + qr
sim = simcard_task(qr, ulk.users[gvd['uid'][0]]['phone'])
sim.start()
sv.close()
print "add item succeed"
s.wfile.write(index)
def __init__(self, dev_name='/dev/ttyUSB0'):
''' dev_name - name of serial device of the servo controller (default '/dev/ttyUSB0')
'''
self.joints = [makeJoint(dev_name, 0, offset=150, min_angle=0, max_angle=300, max_rpm=30),
makeJoint(dev_name, 1, offset=127.1, min_angle=61, max_angle=233.6, max_rpm=30, type='EX-106'),
makeJoint(dev_name, 2, offset=150, min_angle=0, max_angle=300, max_rpm=30),
makeJoint(dev_name, 3, offset=150, min_angle=54.25, max_angle=262, max_rpm=30),
makeJoint(dev_name, 4, offset=150, min_angle=0, max_angle=300, max_rpm=30),
makeJoint(dev_name, 5, offset=150, min_angle=35.2, max_angle=262, max_rpm=30),
makeJoint(dev_name, 6, offset=150, min_angle=0, max_angle=300, max_rpm=30),
makeJoint(dev_name, 7, offset=60, min_angle=45, max_angle=120, max_rpm=30)]
self.all = servo.servo(dev_name,254)
if set(self.joints) == set([None]):
raise RuntimeError('PdArm: no servos found')
self.claw = len(self.joints)-1
self.ts=.03
self.torqued=False
# self.set_torque_all(True)
# time.sleep(.1)
self.set_rpm_all(6)
self.set_torque_all(False)
__author__ = 'glalonde'
import xml.etree.ElementTree as ET
import servo
tree = ET.parse('configurations/servos.xml')
root = tree.getroot()
servos = []
for child in root:
new_servo = servo.servo()
servos.append(new_servo.load_from_xml(child))
print new_servo.angular_speed
print servos
from servo import servo
import RPi.GPIO as GPIO
from time import sleep
import argparse
# argument parsing
parser = argparse.ArgumentParser()
parser.add_argument("rotation", help="rotation", type=float, default=90)
parser.add_argument("elevation", help="elevation", type=float, default=84)
args = parser.parse_args()
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
srv_r = servo(12)
srv_l = servo(13)
srv_r.rotate_to(args.rotation)
srv_l.rotate_to(args.elevation)
sleep(0.6)
srv_r.stop()
if False:
try:
pass
except KeyboardInterrupt:
pass
GPIO.cleanup()
def pandtiltn ():
servo.servo('down', 'hold')
def pandtiltd ():
servo.servo('down', 'down')
def pandtiltu ():
servo.servo('down', 'up')
def panutiltn ():
servo.servo('up', 'hold')
accAngX1 = 0.0
CFangleX1 = 0.0
counter = 0
Speed = 0
Steer = 0
#
PIDo = 0.0
eInteg = 0.0 # previous Integration
ePrev = 0.0 # previous error
FIX = -12.89
ZLoop = True
#-------------System Initialization --------------------
bus = smbus.SMBus(1) #Init I2C module
now = time.time()
m = MOTOR.servo() # Start Servoblaster deamon and reset servos
wi_net = Server() # Start net service for remote control
wi_net.start()
time0 = now
sensor = MPU6050(bus, address, "MPU6050") #Init IMU module
sensor.read_raw_data() # Reads current data from the sensor
k = Kalman()
#=========================================================
def PID_L(current, target):
global eInteg
global ePrev
def panntiltu ():
servo.servo('hold', 'up')
def dontmove ():
servo.servo('hold', 'hold')
from airspeed import airspeed
os.chdir('..')
#except:
# print("Err : error importing libraries")
# --------------------------------------------------------------------------
# Setup
# --------------------------------------------------------------------------
# RC control pins
roll, pitch, throttle, yaw = 35, 34, 33, 32
VRA, VRB = 39, 36
receiver = rc.receiver([roll, pitch, throttle, yaw, VRA, VRB])
# Servo control pins
pitch_servo = servo(23)
roll_servo = servo(18, offset=-7)
yaw_servo = servo(19)
throttle_servo = servo(5)
# I2C
I2C_bus = I2C(scl=Pin(22), sda=Pin(21))
# bmp180
bmp180 = BMP180(I2C_bus)
bmp180.oversample_sett = 2
bmp180.baseline = 101325
alt_init = bmp180.altitude
# BNO055
bno055 = BNO055(I2C_bus)
# Right leg rHipRot = 22 rHip = 27 rKnee = 17 rBase = 4 # Right Hand rshld = 16 relb = 12 # Left Hand lshld = 21 lelb = 20 rshldServo = servo(rshld, "rshld") relbServo = servo(relb, "relb") lshldServo = servo(lshld, "lshld") lelbServo = servo(lelb, "lelb") lHipRotServo = servo(lHipRot, "lHipRot") lHipServo = servo(lHip, "lHip") lKneeServo = servo(lKnee, "lKnee") lBaseServo = servo(lBase, "lBase") rHipRotServo = servo(rHipRot, "rHipRot") rHipServo = servo(rHip, "rHip") rKneeServo = servo(rKnee, "rKnee") rBaseServo = servo(rBase, "rBase")
import network
sta_if = network.WLAN(network.STA_IF)
if not sta_if.isconnected():
print('connecting to network...')
sta_if.active(True)
sta_if.connect('test1234', '12345678')
while not sta_if.isconnected():
pass
print('network config:', sta_if.ifconfig())
# Write data to text window
print('Start BusTimes')
# Initialize all
sv = servo.servo()
# Try to connect to Network
#do_connect()
# Get time from internet
settime()
print('Time is set')
print(time.localtime())
lastUpdate = time.time()
while True:
# Trigger garbage collector
gc.collect()
print("il faut etre admin (sudo)")
robot.my_lcd.write("not root")
quit()
thread_1 = timer.timer()
thread_2 = timer.timer()
#timer.start_robot_daemon()
robot.my_lcd.write("go 2019!")
timer.start_robot_daemon()
robot.my_lcd.write("daemon start")
servo.open_servo()
time.sleep(0.5)
servo.close_servo()
servo.servo(0,0)
robot.com("-X -x500 -y500 -m0")
robot.com("-D -x0.0001 -y0.0004 -t-0.000 -m0")
robot.com("-D -x0.5 -y-0.2 -t-0.000 -m1")
boot.init_robot()
thread_1.start()
thread_2.start()
#thread_2 = timer.unbreak()
#thread_2.start()
#depart:450;250;pi
score = 0
if(robot.color=="jaune"):
#sortie
def panntiltd ():
servo.servo('hold', 'down')
#Connections #MPU6050 - Raspberry pi #VCC - 5V (2 or 4 Board) #GND - GND (6 - Board) #SCL - SCL (5 - Board) #SDA - SDA (3 - Board) from servo import servo from Kalman import KalmanAngle import smbus #import SMBus module of I2C import time import math servo = servo() kalmanX = KalmanAngle() kalmanY = KalmanAngle() RestrictPitch = False #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf radToDeg = 57.2957786 kalAngleX = 0 kalAngleY = 0 #some MPU6050 Registers and their Address PWR_MGMT_1 = 0x6B SMPLRT_DIV = 0x19 CONFIG = 0x1A GYRO_CONFIG = 0x1B INT_ENABLE = 0x38 ACCEL_XOUT_H = 0x3B ACCEL_YOUT_H = 0x3D ACCEL_ZOUT_H = 0x3F GYRO_XOUT_H = 0x43
def panutiltd ():
servo.servo('up', 'down')
def panutiltu ():
servo.servo('up', 'up')
__author__ = 'glalonde'
import xml.etree.ElementTree as ET
import servo
tree = ET.parse('configurations/servos.xml')
root = tree.getroot()
servos = []
for child in root:
new_servo = servo.servo()
servos.append(new_servo.load_from_xml(child))
print new_servo.angular_speed
print servos
class Arm():
# initiate used variables
#
# connections
#
TX_PIN = 16
RX_PIN = 17
SUCTION_PIN = 33
DROP_ANGLES = [1.87, 145, 47.62, 10, 103.38]
# objects
#
CONNECTION = servo(TX_PIN, RX_PIN)
JOINTS = Joints()
SUCTION = Pin(SUCTION_PIN, Pin.OUT)
#
#
def __init__(self):
sleep(0.5)
self.IDLE()
#
#
def IDLE(self):
self.drop()
# TODO: reorder joint movements
#
def moveJoints(self, angles=[], flag=0):
if (flag == 0):
# if no angles ware passed set to IDLE
#
if len(angles) == 0:
self.IDLE()
return
# send each joint the corresbonding angle
#
# for (id,angle) in enumerate(angles):
# position = self.conv_angle(angle)
# self.CONNECTION.move(ID=id, position=position, time=500)
# sleep(0.5)
self.CONNECTION.move(ID=self.JOINTS.base,
position=self.conv_angle(angles[0]),
time=700)
sleep(0.05)
for id in range(4, 0, -1):
position = self.conv_angle(angles[id])
self.CONNECTION.move(ID=id, position=position, time=700)
sleep(0.5)
else:
print("Going backwards")
for id in range(1, 5):
position = self.conv_angle(angles[id])
self.CONNECTION.move(ID=id, position=position, time=700)
sleep(0.5)
self.CONNECTION.move(ID=self.JOINTS.base,
position=self.conv_angle(angles[0]),
time=700)
return 1
# this function converts angles to possitoins
#
def conv_angle(self, angle):
CONV = .240
pos = angle / .240
return int(pos)
# turn on the suction cup
#
def pick(self, angles):
# move the middle joint to make the suction on top of the mushrom
#
# turn on the pump
#
self.SUCTION.value(0)
sleep(3)
# move elevate arm to z=10.5
#
self.moveJoints(angles=angles, flag=1)
# move to drop basket and turn off the suction cup
#
def drop(self):
# move the arm to go on top of the basket
#
self.moveJoints(self.DROP_ANGLES)
# turn off suction
#
self.SUCTION.value(1)
# this method was for demo purposes in the first presentation
#
def demo(self):
self.CONNECTION.move(self.JOINTS.higher_elbow, 0, 500)
self.CONNECTION.move(self.JOINTS.mid_arm, 600, 500)
self.CONNECTION.move(self.JOINTS.wrist, 700, 500)
self.CONNECTION.move(self.JOINTS.base, 1000, 500)
sleep(1)
self.CONNECTION.move(self.JOINTS.base, 600, 500)
sleep(1)
self.IDLE()
sleep(2)
self.CONNECTION.move(self.JOINTS.higher_elbow, 300, 500)
self.CONNECTION.move(self.JOINTS.mid_arm, 500, 500)
self.CONNECTION.move(self.JOINTS.wrist, 500, 500)
self.CONNECTION.move(self.JOINTS.base, 1000, 500)
sleep(1)
self.CONNECTION.move(self.JOINTS.base, 600, 500)
sleep(1)
self.IDLE()
sleep(2)
self.CONNECTION.move(self.JOINTS.lower_elbow, 350, 500)
self.CONNECTION.move(self.JOINTS.higher_elbow, 150, 500)
self.CONNECTION.move(self.JOINTS.mid_arm, 830, 500)
self.CONNECTION.move(self.JOINTS.wrist, 550, 500)
self.CONNECTION.move(self.JOINTS.base, 1000, 500)
sleep(1)
self.CONNECTION.move(self.JOINTS.base, 600, 500)
sleep(1)
self.IDLE()
sleep(2)
