def scissors():
servo.enable()
print('scissors')
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(1.5)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.5)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.5)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(-1.5)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.01)
clck.start()
srvo5.set(0.2)
print('scissors')
time.sleep(0.5)
servo.disable()
return
def main(duty):
# # exit if no options
# if len(sys.argv) < 2:
# usage()
# sys.exit(2)
# # Parse command line
# try:
# opts, args = getopt.getopt(sys.argv[1:], "hst:d:c:", ["help"])
# except getopt.GetoptError as err:
# # print help information and exit:
# print('rcpy_test_servos: illegal option {}'.format(sys.argv[1:]))
# usage()
# sys.exit(2)
# defaults
# duty = 1.5
period = 0.02
channel = 0
sweep = False
brk = False
free = False
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(channel)
srvo.set(duty)
clck = clock.Clock(srvo, period)
servo.enable()
clck.start()
def oneaction():
datalen = len(data['poses'])
servo.enable()
for i in range(0, datalen):
srvo[i].set(data['poses'][i]['position']['z'])
time.sleep(0.2)
return
def first():
print('first')
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(-1.5)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.5)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(-0.8)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(-0.8)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.02)
clck.start()
srvo5.set(0)
time.sleep(3)
print('second')
srvo1.set(-1.5)
srvo2.set(1.2)
srvo3.set(1.5)
srvo4.set(-0.5)
srvo5.set(-0.5)
time.sleep(3);
print('third')
srvo1.set(0.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-0.3)
srvo5.set(-0.8)
time.sleep(3)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1)
time.sleep(1000)
servo.disable()
return
def init_steering(init_data):
global steering_servo
# Setup steering servo
rcpy.set_state(rcpy.RUNNING)
servo.enable()
steering_servo = servo.Servo(init_data["channel"])
steering_servo.start(init_data["pwm_period"])
print("Steering initialized. YOU LOVELY PERSON!")
def start(self):
servo.enable()
for i in self.clocks:
i.start()
mpu9250.initialize(enable_dmp=True,
dmp_sample_rate=100,
enable_fusion=True,
enable_magnetometer=True)
def rock():
for i in range(0, 4):
servo.enable()
print(data['poses'][i]['position']['x'])
srvo[i] = servo.Servo(data['poses'][i]['position']['x'])
clck = clock.Clock(srvo[i], data['poses'][i]['position']['y'])
print(srvo)
clck.start()
srvo[i].set(data['poses'][i]['position']['z'])
time.sleep(0.5)
return
def reset():
with open('open_1.json', 'r') as f:
data = json.load(f)
#print('open start')
servo.enable()
datalen = len(data['poses'])
for i in range(0, datalen):
print(i, data['poses'][i]['position']['x'],
data['poses'][i]['position']['z'])
srvo[i].set(data['poses'][i]['position']['z'])
#print('open finish')
time.sleep(1)
return
def close():
for i in range(0, 6):
servo.enable()
print(i, data['poses'][i]['position']['x'])
srvo[i] = servo.Servo(data['poses'][i]['position']['x'])
clck = clock.Clock(srvo[i], data['poses'][i]['position']['y'])
clck.start()
srvo[i].set(data['poses'][i]['position']['z'])
srvo[4].set(data['poses'][10]['position']['z'])
time.sleep(0.5)
print('close')
return
def sweep(self, angle):
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(self.channel)
duty = self.angle_to_duty(angle)
clck = clock.Clock(srvo, self.period)
try:
servo.enable()
clck.start()
d = 0
direction = 1
duty = math.fabs(duty)
delta = duty / 100
while rcpy.get_state() != rcpy.EXITING:
# increment duty
d = d + direction * delta
# end of range?
if d > duty or d < -duty:
direction = direction * -1
if d > duty:
d = duty
else:
d = -duty
srvo.set(d)
self.duty = d
msg = servolcm()
msg.duty = self.duty
msg.angle = (self.duty)
msg.timestamp = datetime.strftime(datetime.now(),
datetime_format)
self.lc.publish("servo_data", msg.encode())
time.sleep(.02)
# handle ctrl c exit
except KeyboardInterrupt:
pass
finally:
clck.stop()
servo.disable()
def initial():
with open('open.json', 'r') as f:
data = json.load(f)
#print(data)
datalen = len(data['poses']) #6
servo.enable()
for i in range(0, datalen):
#print(data['poses'][i]['position']['x'],data['poses'][i]['position']['z'])
srvo[i] = servo.Servo(data['poses'][i]['position']['x'])
clck = clock.Clock(srvo[i], data['poses'][i]['position']['y'])
clck.start()
srvo[i].set(data['poses'][i]['position']['z'])
#print(i)
time.sleep(1)
return
def __init__(self):
super().__init__()
self.running = False
self.reload()
period = self.period
self.bbb_servos = [
servo.Servo(1),
servo.Servo(2),
servo.Servo(3),
servo.Servo(4),
servo.Servo(5),
servo.Servo(6),
servo.Servo(7),
servo.Servo(8)
]
self.clcks = [
clock.Clock(self.bbb_servos[0], period),
clock.Clock(self.bbb_servos[1], period),
clock.Clock(self.bbb_servos[2], period),
clock.Clock(self.bbb_servos[3], period),
clock.Clock(self.bbb_servos[4], period),
clock.Clock(self.bbb_servos[5], period),
clock.Clock(self.bbb_servos[6], period),
clock.Clock(self.bbb_servos[7], period)
]
self.motors = [
motor.Motor(1),
motor.Motor(2),
motor.Motor(3),
motor.Motor(4)
]
# Boot
GPIO.cleanup()
rcpy.set_state(rcpy.RUNNING)
# disable servos
servo.enable()
# start clock
for i in range(0, 8):
self.clcks[i].start()
self.tts_engine = pyttsx3.init()
self.tts_engine.setProperty('rate', 150)
self.running = True
def main():
global s_command, t_command, new_command
servo.enable()
steering_servo.start(CONFIG["PWM_PERIOD"])
esc8.start(CONFIG["PWM_PERIOD"])
while True:
try:
if new_command:
apply_steering_command(steering_servo, s_command)
apply_throttle_command(t_command)
new_command = False
time.sleep(0.01)
except KeyboardInterrupt:
client.loop_stop()
client.disconnect()
servo.disable()
print("car_client, F*****g off...")
break
def run(self, direction):
if self.duty != 0:
if not self.sweep:
print('Setting servo {} to {} duty'.format(self.channel, self.duty))
self.internal.set(self.duty)
else:
print('Sweeping servo {} to {} duty'.format(self.channel, self.duty))
else:
self.sweep = False
try:
# enable servos
servo.enable()
# start clock
self.clck.start()
# sweep
if self.sweep:
self.servo_sweep(direction)
else:
# keep running
while rcpy.get_state() != rcpy.EXITING:
time.sleep(1)
except KeyboardInterrupt:
# stop clock
self.clck.stop()
# disable servos
servo.disable()
finally:
# stop clock
self.clck.stop()
# disable servos
servo.disable()
# say bye
print("\nBye Beaglebone!")
def handshake():
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(-1.2)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.2)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.2)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(1.2)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.01)
clck.start()
srvo5.set(1.2)
time.sleep(3)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1.4)
time.sleep(0.5)
servo.disable()
return
def set_angle(self, angle):
rcpy.set_state(rcpy.RUNNING)
srvo = servo.Servo(self.channel)
duty = self.angle_to_duty(angle)
print(duty)
srvo.set(duty)
clck = clock.Clock(srvo, self.period)
servo.enable()
clck.start()
clck.stop()
servo.disable()
self.duty = duty
msg = servolcm()
msg.duty = self.duty
msg.timestamp = datetime.strftime(datetime.now(), datetime_format)
self.lc.publish("servo_data", msg.encode())
def runPWM(number):
global number1, number2, pi
# Raspberry Pi direct PWM output
if os.uname()[1] == "raspberrypi":
gpio = 0
if number == 1:
gpio = 17
n = number1
elif number == 2:
gpio = 27
n = number2
value = int(1000 + (n + 1) * 500)
value = min(max(value, 1000), 2000)
#print( gpio, value )
pi.set_servo_pulsewidth(gpio, value)
if False:
# Stop pulsing
pi.set_PWM_dutycycle(17, 0)
if False:
# Start pulsing
pi.set_PWM_dutycycle(17, 50)
# If Beagle Bone Blue robotic cape support
if rcpy:
try:
servo.enable()
if number == 1:
esc1.run()
elif number == 2:
servo2.run()
except:
pass
elif pi is None:
# Else output to Betaflight style motor controller
if number == 1: # Only send if the first motor is pulsed, as we send for all motors in one message
sendMotorCommands([0, 100.0 * number1, 100.0 * number2], False)
def setup(self):
# # Set the two LEDs as outputs:
# GPIO.setup(self.inputpin, GPIO.OUT)
# GPIO.setup(self.inputpin, GPIO.OUT)
#
# # Start one high and one low:
# GPIO.output(self.inputpin, GPIO.HIGH)
#GPIO.output("SERVO_PWR", GPIO.HIGH)
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
self.rcpy_state = rcpy.RUNNING
# set servo duty (only one option at a time)
#Enable Servos
servo.enable()
self.srvo = servo.Servo(self.num)
if self.duty != 0:
print('Setting servo {} to {} duty'.format(self.num, self.duty))
self.srvo.set(self.duty)
self.currentState = True
self.clk = clock.Clock(self.srvo, self.period)
# start clock
self.clk.start()
#print('open start')
servo.enable()
datalen = len(data['poses'])
for i in range(0, datalen):
print(i, data['poses'][i]['position']['x'],
data['poses'][i]['position']['z'])
srvo[i].set(data['poses'][i]['position']['z'])
#print('open finish')
time.sleep(1)
return
initial()
while (True):
servo.enable()
print("Receiving...\n")
rcpy.set_state(rcpy.RUNNING)
result = ws.recv()
print("\n" + result + "\n")
mm = json.loads(result)
print(mm['msg'])
print(mm['msg']['goal']['order'])
revnum = mm['msg']['goal']['order']
for i in range(3):
object = ['button', 'bottle', 'phone']
print(object[revnum])
jsname = object[revnum]
jsfile = str(jsname) + ".json"
print(jsfile)
time.sleep(1)
import rcpy.servo as servo
import rcpy.clock as clock # For PWM period for servos
# INITIALIZE DEFAULT VARS
duty = 1.5 # Duty cycle (-1.5,1.5 is the typical range)
period = 0.02 # recommended servo period: 20ms (this is the interval of commands)
ch1 = 1 # select channel (1 thru 8 are available)
ch2 = 2 # selection of 0 performs output on all channels
rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING
srvo1 = servo.Servo(ch1) # Create servo object
srvo2 = servo.Servo(ch2)
clck1 = clock.Clock(srvo1, period) # Set PWM period for servos
clck2 = clock.Clock(srvo2, period)
servo.enable() # Enables 6v rail on beaglebone blue
clck1.start() # Starts PWM
clck2.start()
def move1(angle):
srvo1.set(angle)
def move2(angle):
srvo2.set(angle)
# THE SECTION BELOW WILL ONLY RUN IF L1_SERVO.PY IS CALLED DIRECTLY
if __name__ == "__main__":
while True:
def count():
print('one')
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(1)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(1)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.5)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(-1)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.018)
clck.start()
srvo5.set(0.7)
time.sleep(2)
print('two')
srvo1.set(1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-1.5)
srvo5.set(0.7)
time.sleep(2)
print('three')
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(-1.5)
srvo4.set(-1.5)
srvo5.set(0.7)
time.sleep(2)
print('four')
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-1.5)
srvo5.set(0.7)
time.sleep(2)
print('five')
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1)
time.sleep(1000)
servo.disable()
return
import rcpy.clock as clock # For PWM period for servos
# defaults
duty = 1.5 # Duty cycle (-1.5,1.5)
period = 0.02 # Set servo period to 20ms
channel = 1 # Which channel (1-8), 0 outputs to all channels
channel2 = 2
rcpy.set_state(rcpy.RUNNING) # set state to rcpy.RUNNING
srvo = servo.Servo(channel) # Create servo object
srvo2 = servo.Servo(channel2)
clck = clock.Clock(srvo, period) # Set PWM period for servos
clck2 = clock.Clock(srvo2, period)
#try:
servo.enable() # Enables 6v rail
clck.start() # Starts PWM
clck2.start()
def move(angle):
srvo.set(angle)
def move2(angle):
srvo2.set(angle)
# while rcpy.get_state() != rcpy.EXITING: # keep running
# srvo.set(duty) # Set servo duty
def main():
# exit if no options
if len(sys.argv) < 2:
usage()
sys.exit(2)
# Parse command line
try:
opts, args = getopt.getopt(sys.argv[1:], "hst:d:c:", ["help"])
except getopt.GetoptError as err:
# print help information and exit:
print('rcpy_test_servos: illegal option {}'.format(sys.argv[1:]))
usage()
sys.exit(2)
# defaults
duty = 1.5
period = 0.02
channel = 0
sweep = False
brk = False
free = False
for o, a in opts:
if o in ("-h", "--help"):
usage()
sys.exit()
elif o in "-d":
duty = float(a)
elif o in "-t":
period = float(a)
elif o in "-c":
channel = int(a)
elif o == "-s":
sweep = True
else:
assert False, "Unhandled option"
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# set servo duty (only one option at a time)
srvo = servo.Servo(channel)
if duty != 0:
if not sweep:
print('Setting servo {} to {} duty'.format(channel, duty))
srvo.set(duty)
else:
print('Sweeping servo {} to {} duty'.format(channel, duty))
else:
sweep = False
# message
print("Press Ctrl-C to exit")
clck = clock.Clock(srvo, period)
try:
# enable servos
servo.enable()
# start clock
clck.start()
# sweep
if sweep:
d = 0
direction = 1
duty = math.fabs(duty)
delta = duty / 100
# keep running
while rcpy.get_state() != rcpy.EXITING:
# increment duty
d = d + direction * delta
# end of range?
if d > duty or d < -duty:
direction = direction * -1
if d > duty:
d = duty
else:
d = -duty
srvo.set(d)
# sleep some
time.sleep(.02)
# or do nothing
else:
# keep running
while rcpy.get_state() != rcpy.EXITING:
# sleep some
time.sleep(1)
except KeyboardInterrupt:
# handle what to do when Ctrl-C was pressed
pass
finally:
# stop clock
clck.stop()
# disable servos
servo.disable()
# say bye
print("\nBye Beaglebone!")
f_m_minus = int(bins[m - 1]) # left
f_m = int(bins[m]) # center
f_m_plus = int(bins[m + 1]) # right
fbank[m - 1, :f_m_minus] = 0
fbank[m - 1, f_m_plus:] = 0
for k in range(f_m_minus, f_m):
fbank[m - 1, k] = (k - f_m_minus) / (f_m - f_m_minus)
for k in range(f_m, f_m_plus):
fbank[m - 1, k] = (f_m_plus - k) / (f_m_plus - f_m)
#%%
hitme_melcep = np.load('hitme_melcep.npy')
left_melcep = np.load('left_melcep.npy')
right_melcep = np.load('right_melcep.npy')
up_melcep = np.load('up_melcep.npy')
down_melcep = np.load('down_melcep.npy')
garbo_melcep = np.load('garbo_melcep.npy')
Nsamps = hitme_melcep.shape[1]
Nsampsgarbo = garbo_melcep.shape[1]
melceps = (hitme_melcep, left_melcep, right_melcep, up_melcep, down_melcep,
garbo_melcep)
servo.enable() #allow electrons to flow to the Mbed
command_predictor = train(1, melceps, Nsamps,
Nsampsgarbo) #train on the recorded samples
time.sleep(2)
#%%
cont_voice_recog(command_predictor) #start listening for voice commands
def setDuty(duty):
srvo.set(duty)
servo.enable()
clck = clock.Clock(srvo, period)
clck.start()
def bye():
servo.enable()
srvo1 = servo.Servo(2)
clck = clock.Clock(srvo1, 0.01)
clck.start()
srvo1.set(-1.5)
srvo2 = servo.Servo(4)
clck = clock.Clock(srvo2, 0.01)
clck.start()
srvo2.set(-1.5)
srvo3 = servo.Servo(6)
clck = clock.Clock(srvo3, 0.01)
clck.start()
srvo3.set(1.5)
srvo4 = servo.Servo(8)
clck = clock.Clock(srvo4, 0.01)
clck.start()
srvo4.set(1.5)
srvo5 = servo.Servo(1)
clck = clock.Clock(srvo5, 0.02)
clck.start()
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(0.5)
srvo2.set(0.5)
srvo3.set(-0.5)
srvo4.set(-0.5)
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(-0.5)
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(0.5)
srvo2.set(0.5)
srvo3.set(-0.5)
srvo4.set(-0.5)
srvo5.set(1.4)
time.sleep(0.5)
srvo1.set(-1.5)
srvo2.set(-1.5)
srvo3.set(1.5)
srvo4.set(1.5)
srvo5.set(1.4)
time.sleep(.5)
servo.disable()
