def pub_angles():
pub = rospy.Publisher(
'angles_topic', angles_msg,
queue_size=10) #node is publishing on the topic, angles_topic
rospy.init_node(
'pub_angles',
anonymous=True) #pub_angles is the name of the publishing node
rate = rospy.Rate(50) # 40hz
msg = angles_msg()
x.set_servo_angle(10, 200, 4)
time.sleep(0.007)
y.set_servo_angle(10, 200, 4)
time.sleep(0.007)
while not rospy.is_shutdown():
print "b"
while time.time() < (timer + delay):
#if time.time() < (timer + delay):
#break
msg.pitch = herkulex.servo(20).get_servo_angle()
time.sleep(0.0014)
print "a"
msg.yaw = herkulex.servo(19).get_servo_angle()
time.sleep(0.0014)
rospy.loginfo(msg)
print "c"
pub.publish(msg)
#10ms #100Hz
herkulex.close()
def talker(): for i in range(0,4) : for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(hand_in[y]+zero_offset[y],50,4) time.sleep(0.0007) time.sleep(0.5) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(hand_out[y]+zero_offset[y],50,4) time.sleep(0.0007) time.sleep(0.5)
def talker(): for y in range (0,10): herkulex.servo(motor_id[y]).set_servo_angle(right[y],50,4) time.sleep(0.0007) time.sleep(5) for y in range (0,10): herkulex.servo(motor_id[y]).set_servo_angle(left[y],50,4) time.sleep(0.0007) time.sleep(5) #for y in range (0,10): #print herkulex.servo(motor_id[y]).get_servo_angle() ###################### herkulex.close()
def talker():
while 1:
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(
down[y] + zero_offset[y], 100, 4)
time.sleep(0.0007)
time.sleep(1)
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(
up[y] + zero_offset[y], 100, 4)
time.sleep(0.0007)
time.sleep(1)
#for y in range (0,10):
#print herkulex.servo(motor_id[y]).get_servo_angle()
######################
herkulex.close()
def talker():
while 1:
if (r_ankle_pitch.get_servo_status() != 0):
r_knee_pitch.torque_off()
elif (r_ankle_roll.get_servo_status() != 0):
r_knee_pitch.torque_off()
elif (r_knee_pitch.get_servo_status() != 0):
r_knee_pitch.torque_off()
elif (r_hip_roll.get_servo_status() != 0):
r_hip_roll.torque_off()
elif (r_hip_pitch.get_servo_status() != 0):
r_hip_pitch.torque_off()
elif (l_hip_roll.get_servo_status() != 0):
l_hip_roll.torque_off()
elif (l_hip_pitch.get_servo_status() != 0):
l_hip_pitch.torque_off()
elif (l_knee_pitch.get_servo_status() != 0):
l_knee_pitch.torque_off()
elif (l_ankle_roll.get_servo_status() != 0):
l_ankle_roll.torque_off()
elif (l_ankle_pitch.get_servo_status() != 0):
l_ankle_pitch.torque_off()
motor_id = [4, 2, 10, 5, 7, 12, 8, 1, 11, 6]
#l_hip_roll_id ,l_hip_pitch_id ,l_knee_pitch_id ,l_ankle_pitch_id, l_ankle_roll_id,
#r_hip_roll_id , r_hip_pitch_id, r_knee_pitch_id, r_ankle_pitch_id, r_ankle_roll_id
###########################################
for c in range(0, 5): #for five steps
time.sleep(0.056)
for x in range(0, 260):
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(
angles[x][y], 2, 4)
print("angle received by")
print(motor_id[y])
time.sleep(0.0007)
time.sleep(0.013)
if c == 4:
break
###########################################
herkulex.close()
def talker():
while 1:
if (r_ankle_pitch.get_servo_status()!=0):
r_knee_pitch.torque_off()
elif (r_ankle_roll.get_servo_status()!=0):
r_knee_pitch.torque_off()
elif (r_knee_pitch.get_servo_status()!=0):
r_knee_pitch.torque_off()
elif (r_hip_roll.get_servo_status()!=0):
r_hip_roll.torque_off()
elif (r_hip_pitch.get_servo_status()!=0):
r_hip_pitch.torque_off()
elif (l_hip_roll.get_servo_status()!=0):
l_hip_roll.torque_off()
elif (l_hip_pitch.get_servo_status()!=0):
l_hip_pitch.torque_off()
elif (l_knee_pitch.get_servo_status()!=0):
l_knee_pitch.torque_off()
elif (l_ankle_roll.get_servo_status()!=0):
l_ankle_roll.torque_off()
elif (l_ankle_pitch.get_servo_status()!=0):
l_ankle_pitch.torque_off()
motor_id = [4, 2, 10, 5, 7, 12, 8, 1, 11, 6]
#l_hip_roll_id ,l_hip_pitch_id ,l_knee_pitch_id ,l_ankle_pitch_id, l_ankle_roll_id,
#r_hip_roll_id , r_hip_pitch_id, r_knee_pitch_id, r_ankle_pitch_id, r_ankle_roll_id
###########################################
for c in range (0, 5):#for five steps
time.sleep(0.056)
for x in range (0, 260):
for y in range (0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y],2,4)
print("angle received by")
print (motor_id[y])
time.sleep(0.0007)
time.sleep(0.013)
if c == 4:
break
###########################################
herkulex.close()
def main():
global servo1
rospy.init_node('servo_controller', anonymous=True)
rate = rospy.Rate(10) # 10hz
#connect to the serial port
herkulex.connect("/dev/ttyUSB0", 115200)
#scan for servos, it returns a tuple with servo id & model number
servos = herkulex.scan_servos()
print servos
servo1 = servo(servos.servoid)
servo_status = servo1.get_servo_status()
print "servo status: ", servo_status
rospy.Subscriber('/servo/cmdpos', Int16, moveServo_cb)
pub = rospy.Publisher('/servo/pos', Int16, queue_size=10)
servo_position = servo1.get_servo_position()
print "servo pos: ", servo_position
pub.publish(servo_position)
rospy.spin()
def talker():
for y in range(0, motor_no):
herkulex.servo(motor_id[y]).set_servo_angle(init[y], 200, 4)
time.sleep(0.0007)
time.sleep(3)
while 1:
for y in range(0, motor_no):
herkulex.servo(motor_id[y]).set_servo_angle(right_bend[y], 150, 4)
time.sleep(0.0007)
time.sleep(2)
for y in range(0, motor_no):
herkulex.servo(motor_id[y]).set_servo_angle(left_bend[y], 150, 4)
time.sleep(0.0007)
time.sleep(2)
######################
herkulex.close()
#!/usr/bin/env python
# license removed for brevity
from beginner_tutorials.msg import angles_msg
import rospy
import serial
import herkulex
import time
herkulex.connect('/dev/ttyUSB0', 115200)
x = herkulex.servo(20)
y = herkulex.servo(19)
angles = [20, 19]
## MOTOR_TORQUE_ON ##
x.torque_on()
y.torque_on()
angles_arr = ['pitch', 'yaw']
timer = time.time()
delay = 1.5 * 60
def pub_angles():
pub = rospy.Publisher(
'angles_topic', angles_msg,
queue_size=10) #node is publishing on the topic, angles_topic
import herkulex
import time
herkulex.connect("/dev/ttyAMA0", 115200)
DROP = herkulex.servo(50)
ALL = herkulex.servo(0xfe)
#power = 1000
power = 1000
init_a = -512
end_a = 512
ALL.torque_on()
DROP.set_servo_angle(0, 1, 0x08)
time.sleep(1)
DROP.set_servo_angle(-95, 1, 0x08)
time.sleep(1)
DROP.set_servo_speed(1, 0x06)
ALL.set_led(0x06)
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
herkulex.connect('/dev/ttyUSB0',115200)
#biped
l_hip_roll = herkulex.servo(4)
l_hip_pitch = herkulex.servo(2)
l_knee_pitch = herkulex.servo(10)
l_ankle_pitch = herkulex.servo(5)
l_ankle_roll = herkulex.servo(7)
r_hip_roll = herkulex.servo(12)
r_hip_pitch = herkulex.servo(8)
r_knee_pitch = herkulex.servo(1)
r_ankle_pitch = herkulex.servo(11)
r_ankle_roll = herkulex.servo(6)
l_low_arm_roll = herkulex.servo(18)
l_arm_roll = herkulex.servo(17)
l_shoulder_pitch = herkulex.servo(16)
r_shoulder_pitch = herkulex.servo(13)
r_arm_roll = herkulex.servo(14)
r_low_arm_roll = herkulex.servo(15)
#new=herkulex.servo(10)
#torso
def talker():
while 1:
for y in range (0,10): #bend
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y],200,4)
time.sleep(0.0007)
time.sleep(2.5)
for x in range (18,49): #shift
for y in range (0,10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y],3,4)
#print motor_id[y]
time.sleep(0.0007)
time.sleep(0.025)
for y in range (0,10): #swing_max
herkulex.servo(motor_id[y]).set_servo_angle(angles[107][y],150,4)
time.sleep(0.0007)
time.sleep(delay)
for y in range (0,10): #swing_min
herkulex.servo(motor_id[y]).set_servo_angle(angles[158][y],100,4)
time.sleep(0.0007)
time.sleep(delay)
## second_step ##
for y in range (0,10): #zero_second_step
herkulex.servo(motor_id[y]).set_servo_angle(angles[193][y],80,4)
time.sleep(0.0007)
time.sleep(0.8)
for x in range (194,213): #shift2
for y in range (0,10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y],3,4)
time.sleep(0.0007)
time.sleep(0.025)
for y in range (0,10): #swing_max2
herkulex.servo(motor_id[y]).set_servo_angle(angles[264][y],150,4)
time.sleep(0.0007)
time.sleep(delay)
for y in range (0,10): #swing_min2
herkulex.servo(motor_id[y]).set_servo_angle(angles[313][y],150,4)
time.sleep(0.0007)
time.sleep(delay)
for y in range (0,10): #zero_second step
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y],80,4)
time.sleep(0.0007)
time.sleep(0.8)
######################
herkulex.close()
## Date of attempt1: 12|07|2016
## STATIC WALKING ##
## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ##
## frequency = 40Hz (=2X20Hz(20Hz is the assumed bandwidth for the motor, So the nyquist criteria is applied to get a minimal sampling frequency of 40Hz between two consecutive angles to a motor)) ##
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
import xlrd
herkulex.connect('/dev/ttyUSB0',115200)
## MOTOR_ID ##
#biped
l_hip_roll = herkulex.servo(4)
l_hip_pitch = herkulex.servo(2)
l_knee_pitch = herkulex.servo(10)
l_ankle_pitch = herkulex.servo(5)
l_ankle_roll = herkulex.servo(7)
r_hip_roll = herkulex.servo(12)
r_hip_pitch = herkulex.servo(8)
r_knee_pitch = herkulex.servo(1)
r_ankle_pitch = herkulex.servo(11)
r_ankle_roll = herkulex.servo(6)
#torso
l_hand_pitch = herkulex.servo(13)
l_hand_roll = herkulex.servo(14)
l_hand_yaw = herkulex.servo(15)
r_hand_pitch = herkulex.servo(16)
r_hand_roll = herkulex.servo(17)
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
herkulex.connect('/dev/ttyUSB0', 115200)
#biped
l_hip_roll = herkulex.servo(4)
l_hip_pitch = herkulex.servo(2)
l_knee_pitch = herkulex.servo(10)
l_ankle_pitch = herkulex.servo(5)
l_ankle_roll = herkulex.servo(7)
r_hip_roll = herkulex.servo(12)
r_hip_pitch = herkulex.servo(8)
r_knee_pitch = herkulex.servo(1)
r_ankle_pitch = herkulex.servo(11)
r_ankle_roll = herkulex.servo(6)
l_low_arm_roll = herkulex.servo(18)
l_arm_roll = herkulex.servo(17)
l_shoulder_pitch = herkulex.servo(16)
r_shoulder_pitch = herkulex.servo(13)
r_arm_roll = herkulex.servo(14)
r_low_arm_roll = herkulex.servo(15)
#new=herkulex.servo(10)
#torso
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
herkulex.connect('/dev/ttyUSB0', 115200)
l_low_arm_roll = herkulex.servo(18)
l_arm_roll = herkulex.servo(17)
l_shoulder_pitch = herkulex.servo(16)
r_shoulder_pitch = herkulex.servo(13)
r_arm_roll = herkulex.servo(14)
r_low_arm_roll = herkulex.servo(15)
motor_id = [18, 17, 16, 13, 14, 15]
motor_no = 6
## MOTOR_TORQUE_ON ##
l_low_arm_roll.torque_on()
l_arm_roll.torque_on()
l_shoulder_pitch.torque_on()
r_shoulder_pitch.torque_on()
r_arm_roll.torque_on()
r_low_arm_roll.torque_on()
init = [0, 90, 80, -80, -90, 0]
right_bend = [0, 90, 80, -70, 7, -70]
left_bend = [70, -27, 70, -80, -90, 0]
## Date of attempt1: 12|07|2016
## STATIC WALKING ##
## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ##
## frequency = 40Hz (=2X20Hz(20Hz is the assumed bandwidth for the motor, So the nyquist criteria is applied to get a minimal sampling frequency of 40Hz between two consecutive angles to a motor)) ##
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
import xlrd
herkulex.connect('/dev/ttyUSB0', 115200)
## MOTOR_ID ##
#biped
l_hip_roll = herkulex.servo(4)
l_hip_pitch = herkulex.servo(2)
l_knee_pitch = herkulex.servo(10)
l_ankle_pitch = herkulex.servo(5)
l_ankle_roll = herkulex.servo(7)
r_hip_roll = herkulex.servo(12)
r_hip_pitch = herkulex.servo(8)
r_knee_pitch = herkulex.servo(1)
r_ankle_pitch = herkulex.servo(11)
r_ankle_roll = herkulex.servo(6)
#torso
l_hand_pitch = herkulex.servo(13)
l_hand_roll = herkulex.servo(14)
l_hand_yaw = herkulex.servo(15)
r_hand_pitch = herkulex.servo(16)
r_hand_roll = herkulex.servo(17)
## Date : 12|08|2016 ##
## Author : Surabhi Verma ##
## TORSO_STATIC_WALKING ##
## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ##
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
import xlrd
import math
herkulex.connect('/dev/ttyUSB0',115200)
#biped
l_hip_roll = herkulex.servo(4)
l_hip_pitch = herkulex.servo(2)
l_knee_pitch = herkulex.servo(10)
l_ankle_pitch = herkulex.servo(5)
l_ankle_roll = herkulex.servo(7)
r_hip_roll = herkulex.servo(12)
r_hip_pitch = herkulex.servo(8)
r_knee_pitch = herkulex.servo(1)
r_ankle_pitch = herkulex.servo(11)
r_ankle_roll = herkulex.servo(6)
#torso
#l_hand_pitch = herkulex.servo(13)
#l_hand_roll = herkulex.servo(14)
#l_hand_yaw = herkulex.servo(15)
#r_hand_pitch = herkulex.servo(16)
#r_hand_roll = herkulex.servo(17)
## Date : 12|08|2016 ##
## Author : Surabhi Verma ##
## TORSO_STATIC_WALKING ##
## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ##
import rospy
from std_msgs.msg import String
import serial
import herkulex
import time
import xlrd
import math
herkulex.connect('/dev/ttyUSB0', 115200)
#biped
l_hip_roll = herkulex.servo(4)
l_hip_pitch = herkulex.servo(2)
l_knee_pitch = herkulex.servo(10)
l_ankle_pitch = herkulex.servo(5)
l_ankle_roll = herkulex.servo(7)
r_hip_roll = herkulex.servo(12)
r_hip_pitch = herkulex.servo(8)
r_knee_pitch = herkulex.servo(1)
r_ankle_pitch = herkulex.servo(11)
r_ankle_roll = herkulex.servo(6)
#torso
#l_hand_pitch = herkulex.servo(13)
#l_hand_roll = herkulex.servo(14)
#l_hand_yaw = herkulex.servo(15)
#r_hand_pitch = herkulex.servo(16)
#r_hand_roll = herkulex.servo(17)
import herkulex
import time
herkulex.connect("/dev/ttyAMA0", 115200)
FR = herkulex.servo(0xfd)
FL = herkulex.servo(16)
BR = herkulex.servo(10)
BL = herkulex.servo(20)
ALL = herkulex.servo(0xfe)
#power = 1000
power = 1000
rtime = 1.54
#rtime = 20
calib = 0
calib = -50
corrpow = -300
ALL.torque_on()
FR.set_servo_speed(-power + corrpow, 0x06)
FL.set_servo_speed(power + corrpow + calib, 0x06)
BR.set_servo_speed(-power - corrpow, 0x06)
BL.set_servo_speed(power - corrpow + calib, 0x06)
time.sleep(rtime)
ALL.set_servo_speed(1, 0x06)
ALL.set_led(0x06)
peltierpin2 = 20 #Cold Block
heaterpin = 16 #HotEnd
peltierfanpin1 = 5 #D5 AC
peltierfanpin2 = 3 #D3 Cold block
motor_dir_pin = 26 #Stepper motor
motor_step_pin = 19
motor_enable_pin = 13
buzzerpin = 4 #buzzer
ledcirclepin = 6 #led circular
#Servo Motor Configuration
herkulex.connect("/dev/ttyS0", 115200)
herkulex.clear_errors()
#servos = herkulex.scan_servos(0x01,0x02) #min and max range of ServoID
#print(servos)
servo1=servo(0x01,0x02) #ServoID, Model
servo2=servo(0x02,0x02) #ServoID, Model
servo3=servo(0x03,0x02) #ServoID, Model
#servo1.set_servo_angle(50, 1, 0x00) #goaltime is 1 to 255
time.sleep(0.1)
#Configuration of Pin IO
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False) #disable warning
#Stepper Motor
GPIO.setup(motor_step_pin, GPIO.OUT)
GPIO.setup(motor_dir_pin, GPIO.OUT)
GPIO.setup(motor_enable_pin, GPIO.OUT)
#Aircon
GPIO.setup(peltierpin1, GPIO.OUT)
peltier1 = GPIO.PWM(peltierpin1, 50)
peltier1.start(0)
def talker(): for i in range(0,4): for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(l_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(both[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(l_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(straight[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(r_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(both[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(r_hand_up[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1) for y in range (0,motor_no): herkulex.servo(motor_id[y]).set_servo_angle(straight[y]+zero_offset[y],100,4) time.sleep(0.0007) time.sleep(1)
import herkulex
import time
herkulex.connect("/dev/ttyAMA0", 115200)
FR = herkulex.servo(0xfd)
FL = herkulex.servo(16)
BR = herkulex.servo(10)
BL = herkulex.servo(20)
ALL = herkulex.servo(0xfe)
#power = 1000
power = 1000
#rtime = 1.54
rtime = 20
calib = 0
calib = -50
ALL.torque_on()
FR.set_servo_speed(-power, 0x06)
FL.set_servo_speed(power + calib, 0x06)
BR.set_servo_speed(-power, 0x06)
BL.set_servo_speed(power + calib, 0x06)
time.sleep(rtime)
ALL.set_servo_speed(1, 0x06)
ALL.set_led(0x06)
## Date : 12|08|2016 ##
## Author : Surabhi Verma ##
## TORSO_STATIC_WALKING ##
## INPUT FROM EXCEL SHEET, ANGLES PRECALCULATED IN MATLAB ##
import rospy
from std_msgs.msg import Float64
import serial
import herkulex
import time
import xlrd
import math
herkulex.connect('/dev/ttyUSB4', 115200)
l_hand_roll = herkulex.servo(17)
l_hand_pitch = herkulex.servo(16)
l_low_arm = herkulex.servo(18)
r_hand_roll = herkulex.servo(14)
r_hand_pitch = herkulex.servo(13)
r_low_arm = herkulex.servo(15)
motor_id = [4, 2, 10, 5, 7, 12, 8, 1, 11, 6, 13, 14, 15, 16, 17, 18]
delay = 1.5
############################
## MOTOR_TORQUE_ON ##
l_hand_pitch.torque_on()
def talker():
while 1:
for y in range(0, 10): #bend
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y], 200, 4)
time.sleep(0.0007)
time.sleep(2.5)
for x in range(18, 49): #shift
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y], 3, 4)
#print motor_id[y]
time.sleep(0.0007)
time.sleep(0.025)
for y in range(0, 10): #swing_max
herkulex.servo(motor_id[y]).set_servo_angle(angles[107][y], 150, 4)
time.sleep(0.0007)
time.sleep(delay)
for y in range(0, 10): #swing_min
herkulex.servo(motor_id[y]).set_servo_angle(angles[158][y], 100, 4)
time.sleep(0.0007)
time.sleep(delay)
## second_step ##
for y in range(0, 10): #zero_second_step
herkulex.servo(motor_id[y]).set_servo_angle(angles[193][y], 80, 4)
time.sleep(0.0007)
time.sleep(0.8)
for x in range(194, 213): #shift2
for y in range(0, 10):
herkulex.servo(motor_id[y]).set_servo_angle(angles[x][y], 3, 4)
time.sleep(0.0007)
time.sleep(0.025)
for y in range(0, 10): #swing_max2
herkulex.servo(motor_id[y]).set_servo_angle(angles[264][y], 150, 4)
time.sleep(0.0007)
time.sleep(delay)
for y in range(0, 10): #swing_min2
herkulex.servo(motor_id[y]).set_servo_angle(angles[313][y], 150, 4)
time.sleep(0.0007)
time.sleep(delay)
for y in range(0, 10): #zero_second step
herkulex.servo(motor_id[y]).set_servo_angle(angles[33][y], 80, 4)
time.sleep(0.0007)
time.sleep(0.8)
######################
herkulex.close()
import herkulex
import time
herkulex.connect("/dev/ttyAMA0", 115200)
DROP = herkulex.servo(50)
ALL = herkulex.servo(0xfe)
#power = 1000
power = 1000
init_a = -512
end_a = 512
ALL.torque_on()
DROP.set_servo_angle(0, 1, 0x08)
time.sleep(1)
DROP.set_servo_angle(-95, 1, 0x08)
time.sleep(1)
DROP.set_servo_speed(1, 0x06)
ALL.set_led(0x06)
