def __init__(self,*args,**kwds):
servoControlFrame.__init__(self,*args,**kwds)
self.servoCtrl = Servo()
self.initGrid()
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER,self.onTick,self.timer)
self.timer.Start(200)
def __init__(self, horizontalControlPin: int, verticalControlPin: int) -> ArmControl:
self.horizontalControlPin = horizontalControlPin
self.verticalControlPin = verticalControlPin
self.horizontalServo = Servo(horizontalControlPin)
self.verticalServo = Servo(verticalControlPin)
self.tracking = False
class CandyCrush:
def __init__(self, config):
self.config = config
self.setup_servo(config)
self.setup_apis(config)
# External APIs
def setup_apis(self, config):
toggl_token = config.get('API Tokens', 'toggl')
self.toggl = Toggl(access_token=toggl_token)
# Servo control
def setup_servo(self, config):
physical_pin = config.getint('Servo', 'physical_pin')
servo_speed_180 = config.getfloat('Servo', 'speed_180')
duty_min = config.getfloat('Servo', 'duty_min')
duty_max = config.getfloat('Servo', 'duty_max')
self.servo = Servo(physical_pin, servo_speed_180, duty_min, duty_max)
def dispense_candy(self):
servo = self.servo
if not servo.position == 180:
servo.set_position(180)
time.sleep(1)
servo.set_position(90, 1.5)
servo.set_position(0, 0.5)
time.sleep(1)
servo.set_position(180, 1.5)
# Run!
def main(self):
self.servo.set_position(180)
self.dispense_candy()
def __init__(self, printer):
super(DualServoPlugin, self).__init__(printer)
logging.debug('Activating ' + __PLUGIN_NAME__ + ' plugin...')
# Add the servo
channel = self.printer.config.get(type(self).__name__, 'servo_channel')
pulse_min = self.printer.config.getfloat(type(self).__name__, 'pulse_min')
pulse_max = self.printer.config.getfloat(type(self).__name__, 'pulse_max')
angle_min = self.printer.config.getfloat(type(self).__name__, 'angle_min')
angle_max = self.printer.config.getfloat(type(self).__name__, 'angle_max')
angle_init = self.printer.config.getfloat(type(self).__name__, 'extruder_0_angle')
# Disable the end-stop on this channel
if channel == "P9_14":
self.printer.end_stops["X2"].active = False
self.printer.end_stops["X2"].stop()
elif channel == "P9_16":
self.printer.end_stops["Y2"].active = False
self.printer.end_stops["Y2"].stop()
self.head_servo = Servo(channel, pulse_min, pulse_max, angle_min, angle_max, angle_init)
# Load the config for angles
self.t0_angle = float(self.printer.config.getfloat(type(self).__name__, 'extruder_0_angle'))
self.t1_angle = float(self.printer.config.get(type(self).__name__, 'extruder_1_angle'))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_DualServo(self.printer))
self.printer.processor.override_command('T1', T1_DualServo(self.printer))
def __init__(self, printer):
super(HPX2MaxPlugin, self).__init__(printer)
logging.debug('Activating ' + __PLUGIN_NAME__ + ' plugin...')
# Add the servo
channel = self.printer.config.get(type(self).__name__, 'servo_channel')
pulse_min = self.printer.config.getfloat(
type(self).__name__, 'pulse_min')
pulse_max = self.printer.config.getfloat(
type(self).__name__, 'pulse_max')
angle_min = self.printer.config.getfloat(
type(self).__name__, 'angle_min')
angle_max = self.printer.config.getfloat(
type(self).__name__, 'angle_max')
angle_init = self.printer.config.getfloat(
type(self).__name__, 'extruder_0_angle')
self.head_servo = Servo(channel, pulse_min, pulse_max, angle_min,
angle_max, angle_init)
# Load the config for angles
self.t0_angle = float(
self.printer.config.getfloat(
type(self).__name__, 'extruder_0_angle'))
self.t1_angle = float(
self.printer.config.get(type(self).__name__, 'extruder_1_angle'))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_HPX2Max(self.printer))
self.printer.processor.override_command('T1', T1_HPX2Max(self.printer))
def main(argv):
GPIO.setmode(GPIO.BOARD)
angle = int(argv[0])
my_servo = Servo(11, "Sg90")
my_servo.write_angle(angle)
print("Moving: ", angle)
time.sleep(1)
def main(argv):
frame = cv2.imread("/home/pi/Downloads/IMG_4644.JPG")
frame = imutils.resize(frame, width=400)
# Converting captured frame to monochrome
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Blurring the image using the GaussianBlur() method of the opencv object
blur = cv2.GaussianBlur(gray, (9, 9), 0)
# Using an opencv method to identify the threshold intensities and locations
(darkest_value, brightest_value, darkest_loc, brightest_loc) = cv2.minMaxLoc(blur)
print "Brightest Value:",brightest_value
# Threshold the blurred frame accordingly
# First argument is the source image, which is the grayscale image. Second argument is the threshold value
# which is used to classify the pixel values. Third argument is the maxVal which represents the value to be given
# if pixel value is more than (sometimes less than) the threshold value
out2, threshold2 = cv2.threshold(blur, brightest_value - 10, 230, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
out, threshold = cv2.threshold(blur, brightest_value - 10, 230, cv2.THRESH_BINARY)
thr = threshold.copy()
print "out value:",out2
# Resize frame for ease
# cv2.resize(thr, (300, 300))
# Find contours in thresholded frame
edged = cv2.Canny(threshold, 50, 150)
# First one is source image, second is contour retrieval mode, third is contour approximation method. And it outputs
# the contours and hierarchy. Contours is a Python list of all the contours in the image. Each individual contour
# is a Numpy array of (x,y) coordinates of boundary points of the object.
lightcontours, hierarchy = cv2.findContours(edged, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# Checking if the list of contours is greater than 0 and if any circles are detected
if (len(lightcontours)):
# Finding the maxmimum contour, this is assumed to be the light beam
maxcontour = max(lightcontours, key=cv2.contourArea)
# Avoiding random spots of brightness by making sure the contour is reasonably sized
if cv2.contourArea(maxcontour):
(x, final_y), radius = cv2.minEnclosingCircle(maxcontour)
print "x value:",x,"y value:",final_y
t=Stepper(x,final_y)
s = Servo(final_y)
s.servo_control()
t.change_position()
cv2.circle(frame, (int(x), int(final_y)), int(radius), (0, 255, 0), 4)
cv2.rectangle(frame, (int(x) - 5, int(final_y) - 5), (int(x) + 5, int(final_y) + 5), (0, 128, 255), -1)
# Display frames and exit
cv2.imshow('frame', frame)
cv2.waitKey(0)
key = cv2.waitKey(1)
cv2.destroyAllWindows()
class HPX2MaxPlugin(AbstractPlugin):
@staticmethod
def get_description():
return "Plugin to use a HPX2-Max extruder in redeem (single motor - two hotends)"
def __init__(self, printer):
super(HPX2MaxPlugin, self).__init__(printer)
logging.debug('Activating ' + __PLUGIN_NAME__ + ' plugin...')
# Add the servo
self.head_servo = Servo(
int(
self.printer.config.get(
type(self).__name__, 'servo_channel', 1)), 500, 750, 90,
10)
# Load the config for angles
self.t0_angle = float(
self.printer.config.get(
type(self).__name__, 'extruder_0_angle', 20))
self.t1_angle = float(
self.printer.config.get(
type(self).__name__, 'extruder_1_angle', 175))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_HPX2Max(self.printer))
self.printer.processor.override_command('T1', T1_HPX2Max(self.printer))
def exit(self):
self.head_servo.stop()
def path_planner_initialized(self, path_planner):
# Configure the path planner so that the motor direction
# is reversed when selecting T1 or T0 (depending on the angle)
# 3 axis, plus 2 extruders
assert Path.NUM_AXES >= 5
for i in range(2):
e = self.printer.path_planner.native_planner.getExtruder(i)
# FIXME: We have hardcoded the motor to be used here.
# It is always the Extruder E. Patch welcome.
e.setStepperCommandPosition(3)
# If extruder 1 angle is > 90, then we invert the motor for
# him, otherwise for the other
# This is how the HPX2 Max is built.
if i == 0:
e.setDirectionInverted(True if self.t0_angle <= 90 else False)
else:
e.setDirectionInverted(True if self.t1_angle <= 90 else False)
# Select tool 0 as this is the default tool
self.head_servo.set_angle(
self.printer.plugins[__PLUGIN_NAME__].t0_angle, asynchronous=True)
class DualServoPlugin(AbstractPlugin):
@staticmethod
def get_description():
return "Plugin to change between two hot ends that is tilted with a servo"
def __init__(self, printer):
super(DualServoPlugin, self).__init__(printer)
logging.debug('Activating '+__PLUGIN_NAME__+' plugin...')
# Add the servo
channel = self.printer.config.get(type(self).__name__, 'servo_channel')
pulse_min = self.printer.config.getfloat(type(self).__name__, 'pulse_min')
pulse_max = self.printer.config.getfloat(type(self).__name__, 'pulse_max')
angle_min = self.printer.config.getfloat(type(self).__name__, 'angle_min')
angle_max = self.printer.config.getfloat(type(self).__name__, 'angle_max')
angle_init = self.printer.config.getfloat(type(self).__name__, 'extruder_0_angle')
self.head_servo = Servo(channel, pulse_min, pulse_max, angle_min, angle_max, angle_init)
# Load the config for angles
self.t0_angle = float(self.printer.config.getfloat(type(self).__name__, 'extruder_0_angle'))
self.t1_angle = float(self.printer.config.get(type(self).__name__, 'extruder_1_angle'))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_DualServo(self.printer))
self.printer.processor.override_command('T1', T1_DualServo(self.printer))
def exit(self):
self.head_servo.stop()
def path_planner_initialized(self, path_planner):
# Configure the path planner so that the motor direction
# is reversed when selecting T1 or T0 (depending on the angle)
# 3 axis, plus 2 extruders
assert Printer.NUM_AXES >= 5
#for i in range(2):
# e = self.printer.path_planner.native_planner.getExtruder(i)
# FIXME: We have hardcoded the motor to be used here.
# It is always the Extruder E. Patch welcome.
# e.setStepperCommandPosition(3)
# If extruder 1 angle is > 90, then we invert the motor for
# him, otherwise for the other
# This is how the HPX2 Max is built.
# if i == 0:
# e.setDirectionInverted(True if self.t0_angle <= 90 else False)
# else:
# e.setDirectionInverted(True if self.t1_angle <= 90 else False)
# Select tool 0 as this is the default tool
self.head_servo.set_angle(self.printer.plugins[__PLUGIN_NAME__].t0_angle, asynchronous=True)
def main(): s0 = Servo(0) s1 = Servo(1) s2 = Servo(2) s0.all_stop() for i in range(0, 2): loop(s0, s1, s2) s0.all_stop()
def __init__(self, leg_number, pwm):
#Servo controller
self.pwm = pwm
#last leg position
self.last_x = 0
self.last_y = 0
self.last_z = 0
#normal leg position
self.normal_x = 0
self.normal_y = 130
self.angle_afwijking = 0
self.angle_afwijking_knee = 0
#leg number
self.leg_number = leg_number
if leg_number == 1:
self.hip = Servo(0, pwm)
self.height = Servo(1, pwm)
self.knee = Servo(2, pwm)
if leg_number == 3:
self.hip = Servo(13, pwm)
self.height = Servo(1, pwm)
self.knee = Servo(2, pwm)
elif leg_number == 2 or leg_number == 4:
self.hip = Servo(6, pwm)
self.height = Servo(7, pwm)
self.knee = Servo(15, pwm)
class Leg:
#Length in mm
COXA = 46.13
FEMUR = 70.61
TIBIA = 146.0
def __init__(self, leg_number, pwm):
#Servo controller
self.pwm = pwm
#last leg position
self.last_x = 0
self.last_y = 0
self.last_z = 0
#normal leg position
self.normal_x = 0
self.normal_y = 130
self.angle_afwijking = 0
self.angle_afwijking_knee = 0
#leg number
self.leg_number = leg_number
if leg_number == 1:
self.hip = Servo(0, pwm)
self.height = Servo(1, pwm)
self.knee = Servo(2, pwm)
if leg_number == 3:
self.hip = Servo(13, pwm)
self.height = Servo(1, pwm)
self.knee = Servo(2, pwm)
elif leg_number == 2 or leg_number == 4:
self.hip = Servo(6, pwm)
self.height = Servo(7, pwm)
self.knee = Servo(15, pwm)
#set hip
def set_hip(self, degree):
self.hip.set_servo(degree - self.angle_afwijking)
#get hip
def get_hip(self):
return self.hip.get_servo()
#set height
def set_height(self, degree):
self.height.set_servo(degree)
#get height
def get_height(self):
return self.height.get_servo()
#set knee
def set_knee(self, degree):
self.knee.set_servo(degree + self.angle_afwijking_knee)
#get knee
def get_knee(self):
return self.knee.get_servo()
def __init__(self): #ServoBlaster is what we use to control servo motors #Upper Limit for servos self._ServoXul = 250 self._ServoYul = 230 #Lower limit for servos self._ServoXll = 75 self._ServoYll = 75 self.servo_X = Servo(0, self._ServoXul, self._ServoXll) self.servo_Y = Servo(1, self._ServoYul, self._ServoYll)
def __init__(self, **kwargs):
Controller.__init__(self, **kwargs)
self.L3X = 0
self.L3Y = 0
self.R3X = 0
self.R3Y = 0
self.leftMotorPWM = 0
self.rightMotorPWM = 0
self.arrow = "None"
self.motor = Motor()
self.servo = Servo()
self.motor.InitMotorHw()
self.servo.InitServoHw()
self.BASESTEPSIZE = 5
def __init__(self,
name,
pin,
minAngle=0,
maxAngle=180,
angle=0,
minPWM=1000,
maxPWM=2000,
simulation=True):
self.setAngleLimit(minAngle, maxAngle)
self.angle = angle
Servo.__init__(self, name, pin, minPWM, maxPWM, simulation)
def __init__(self, a):
# Open the serial device
self.dyn_chain = a
self.servos = Servo()
self.mik = MachineIK()
# self.labeler = Labeler()
self.INIT_TEST = False
self.VIEW_POS_RT = False
self.curr_joints = [
p for key, p in self.dyn_chain.get_position([1, 2, 4, 6]).items()
]
self.curr_pos = self.get_arm_pose()
class HPX2MaxPlugin(AbstractPlugin):
@staticmethod
def get_description():
return "Plugin to use a HPX2-Max extruder in redeem (single motor - two hotends)"
def __init__(self, printer):
super(HPX2MaxPlugin, self).__init__(printer)
logging.debug('Activating '+__PLUGIN_NAME__+' plugin...')
# Add the servo
self.head_servo = Servo(int(self.printer.config.get(type(self).__name__, 'servo_channel', 1)),500,750,90,10)
# Load the config for angles
self.t0_angle = float(self.printer.config.get(type(self).__name__, 'extruder_0_angle', 20))
self.t1_angle = float(self.printer.config.get(type(self).__name__, 'extruder_1_angle', 175))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_HPX2Max(self.printer))
self.printer.processor.override_command('T1', T1_HPX2Max(self.printer))
def exit(self):
self.head_servo.stop()
def path_planner_initialized(self, path_planner):
# Configure the path planner so that the motor direction
# is reversed when selecting T1 or T0 (depending on the angle)
# 3 axis, plus 2 extruders
assert Path.NUM_AXES >= 5
for i in range(2):
e = self.printer.path_planner.native_planner.getExtruder(i)
# FIXME: We have hardcoded the motor to be used here.
# It is always the Extruder E. Patch welcome.
e.setStepperCommandPosition(3)
# If extruder 1 angle is > 90, then we invert the motor for
# him, otherwise for the other
# This is how the HPX2 Max is built.
if i == 0:
e.setDirectionInverted(True if self.t0_angle <= 90 else False)
else:
e.setDirectionInverted(True if self.t1_angle <= 90 else False)
# Select tool 0 as this is the default tool
self.head_servo.set_angle(self.printer.plugins[__PLUGIN_NAME__].t0_angle, asynchronous=True)
def __init__(self, AddressArray, ChannelArray, DefaultPulseArray, inverse=False, offset=[0,0,0]):
self._AddressArray = AddressArray
self._ChannelArray = ChannelArray
self._DefaultPulseArray = DefaultPulseArray
self._inverse = inverse
self._offset = offset
# Receiving variables, this will be set into arrays. If there aren't 3 input variables of the address
# it will print an Error.
if len(self._AddressArray) == 3:
self._Hip = Servo(self._AddressArray[0], self._ChannelArray[0], self._DefaultPulseArray[0], self._inverse, self._offset[0])
self._Knee = Servo(self._AddressArray[1], self._ChannelArray[1], self._DefaultPulseArray[1], self._inverse, self._offset[1])
self._Ankle = Servo(self._AddressArray[2], self._ChannelArray[2],self._DefaultPulseArray[2], self._inverse, self._offset[2]) #3 keer nul? aangepast naar 0,1,2
else:
print "-----------Error Creating leg-----------"
def __init__(self):
# ServoBlaster is what we use to control servo motors
# Upper Limit for servos
self._ServoXul = 250
self._ServoYul = 230
# Lower limit for servos
self._ServoXll = 75
self._ServoYll = 75
"""
Servos are default to Servo X-axis is assigned (servo-0) GPIO 4
Servo-Y axis is assigned (servo-1) GPIO 17
"""
self.servo_X = Servo(0, self._ServoXul, self._ServoXll)
self.servo_Y = Servo(1, self._ServoYul, self._ServoYll)
def __init__(self):
'''
小车功能初始化
'''
Fire.__init__(self, Fire_Pin=37)
Hc_Sr501.__init__(self, Hc_Sr501_Pin=32)
Moter.__init__(self,
Left_Pin=33,
Left_Pwm_Pin=35,
Right_Pin=29,
Right_Pwm_Pin=31)
Tem_Hum.__init__(self, Tem_Hum_Pin=36)
Servo.__init__(self, Servo_Pin=40)
Mq_2.__init__(self, Mq_2_Pin=38)
Network.Get_Socket(self, Socket_Ip="10.4.250.237", Sockeet_Port=2001)
def main():
car = Car()
ultrasound = Ultrasound()
font_servo = Servo(0)
time.sleep(2)
safe_distance = 70
while True:
# 1. 是否符合前进条件
dis = ultrasound.test()
if dis < safe_distance:
# 移动
d1, d2, d3, d4 = probe(ultrasound, font_servo)
if max(d1, d2, d3, d4) < 100:
car.back(0.2)
if d1 == max(d1, d2, d3, d4):
car.right(0.5)
if d2 == max(d1, d2, d3, d4):
car.right(0.25)
if d3 == max(d1, d2, d3, d4):
car.left(0.25)
if d4 == max(d1, d2, d3, d4):
car.left(0.5)
time.sleep(1)
# 向前移动
# 重新获取距离
distance = ultrasound.test()
while distance > safe_distance:
car.run()
time.sleep(0.1)
distance = ultrasound.test()
else:
car.stop()
def MoveServo(self, Servo, Angle):
if Servo.getAngle() != Angle:
Servo.SetAngle(Angle)
#voor TestDoeleiden
return "Moved servo to: " + Angle
def __init__(self):
if MovementController.__instance is not None: # if the constructor of this class is called more than once
raise Exception("This class is a singleton!")
else:
# puts the created instance in the "__instance" variable
MovementController.__instance = self
# array that holds the pins of all the motors
pinArray = [[20, 23, 24], [16, 5, 6]]
# creates instances of the Motor class to control the physical motors
self.leftMotor = Motor(pinArray[0])
self.rightMotor = Motor(pinArray[1])
self.servos = AX12.Ax12() # list of all servos
# creates instances of the Servo class to control the physical servos
self.armServo = Servo(self.servos, 3, 426, 576, 100)
self.gripServo = Servo(self.servos, 4, 280, 574, 512)
def __init__(self, driver_board, left_or_right, front_or_back, hip_knee_indices: HipKneeIndices, **kwargs):
assert left_or_right in LeftOrRight, 'Leg side must be in LeftOrRight! Val : {}'.format(left_or_right)
assert front_or_back in FrontOrBack, 'front_or_back must be in FrontOrBack! Val : {}'.format(front_or_back)
self.left_or_right = left_or_right
self.front_or_back = front_or_back
self.driver_board = driver_board
self.hip_servo_index = hip_knee_indices.hip
self.knee_servo_index = hip_knee_indices.knee
self.incr_pause = kwargs.get('incr_pause', 0.005)
self.phase_incr = 0.05 * 2 * pi
self.direction = 1 # should determine this from left_or_right etc
# I have to think about this side/direction stuff. Maybe it's a bad
# way of thinking about it, when it's really just 1D anyway (so really
# just diff phases, which I already have).
self.hip = Servo(
self.driver_board,
self.hip_servo_index,
direction=1,
servo_type='hip',
**kwargs
)
self.knee = Servo(
self.driver_board,
self.knee_servo_index,
direction=1,
servo_type='knee',
**kwargs
)
# Also, I DEFINITELY need to set their phases separately here. I think
# it'll probably be another top-down thing where the leg gets a phase,
# and then uses that to set the phases of the servos.
self.servos = [self.hip, self.knee]
self.phase = 0.
self.hip_phase_offset = 0.
if self.front_or_back == FrontOrBack.FRONT:
self.knee_phase_offset = 1 * pi/2
else:
self.knee_phase_offset = 3 * pi/2
def Servo(*args, **kwargs):
try:
from Servo import Servo as Servo
return Servo(*args, **kwargs)
except Exception as e:
print "Failed to Initialize Servo"
print "Error: %s" % e.message
print "Using Mock Servo"
from Servo_Mock import Servo as Servo_Mock
return Servo_Mock()
class Leg(object):
"""
Setting the addresses, channels and default position of the 3 servos of the leg.
"""
def __init__(self, AddressArray, ChannelArray, DefaultPulseArray, inverse=False, offset=[0,0,0]):
self._AddressArray = AddressArray
self._ChannelArray = ChannelArray
self._DefaultPulseArray = DefaultPulseArray
self._inverse = inverse
self._offset = offset
# Receiving variables, this will be set into arrays. If there aren't 3 input variables of the address
# it will print an Error.
if len(self._AddressArray) == 3:
self._Hip = Servo(self._AddressArray[0], self._ChannelArray[0], self._DefaultPulseArray[0], self._inverse, self._offset[0])
self._Knee = Servo(self._AddressArray[1], self._ChannelArray[1], self._DefaultPulseArray[1], self._inverse, self._offset[1])
self._Ankle = Servo(self._AddressArray[2], self._ChannelArray[2],self._DefaultPulseArray[2], self._inverse, self._offset[2]) #3 keer nul? aangepast naar 0,1,2
else:
print "-----------Error Creating leg-----------"
# Setting the Hip to the position of the given pulse
def moveHip(self, Pulse):
self._Hip.setPulse(Pulse)
# Setting the Knee to the position of the given pulse
def moveKnee(self, Pulse):
self._Knee.setPulse(Pulse)
# Setting the Ankle to the position of the given pulse
def moveAnkle(self, Pulse):
self._Ankle.setPulse(Pulse)
# Setting the full Leg to the starting position of 375 pulses
def defaultPos(self):
self.moveHip(self._Hip, self._DefaultPulseArray[0])
self.moveKnee(self._Knee, self._DefaultPulseArray[1])
self.moveAnkle(self._Ankle, self._DefaultPulseArray[2])
def getHip(self):
return self._Hip.getPulse()
def getKnee(self):
return self._Knee.getPulse()
def getAnkle(self):
return self._Ankle.getPulse()
def test_full_fk_ik(c=[0, 1, 2]):
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = c
leg = Leg(length, channels)
servorange = [[-90, 90], [-90, 90], [-180, 0]]
for s in range(0, 3):
leg.servos[s].setServoRangeAngle(*servorange[s])
for i in range(1, 3):
for a in range(-70, 70, 10):
angles = [0, 0, -10]
if i == 2: a -= 90
angles[i] = a
pts = leg.fk(*angles)
angles2 = leg.ik(*pts)
pts2 = leg.fk(*angles2)
angle_error = np.linalg.norm(np.array(angles) - np.array(angles2))
pos_error = np.linalg.norm(pts - pts2)
# print(angle_error, pos_error)
if angle_error > 0.0001:
print('Angle Error')
printError(pts, pts2, angles, angles2)
exit()
elif pos_error > 0.0001:
print('Position Error')
printError(pts, pts2, angles, angles2)
exit()
else:
print('Good: {} {} {} error(deg,mm): {} {}'.format(angles[0], angles[1], angles[2], angle_error, pos_error))
leg.move(*pts)
time.sleep(0.1)
Servo.all_stop()
def parse(obj):
if "body_part" not in obj:
raise Exception("Could not parse JSON object")
if "disabled" in obj:
if obj["disabled"] is True:
return
servos.append(
Servo(obj["id"], obj["min_pulse"], obj["max_pulse"], obj["min_degree"],
obj["max_degree"], obj["default_angle"], obj["body_part"]))
def __init__(self, debug=False, db="config", bus_number=1, channel=FRONT_WHEEL_CHANNEL):
''' setup channels and basic stuff '''
self.db = fileDB(db=db)
self.channel = channel
self.straight_angle = 90
self.turning_max = 45
self.DEBUG_INFO = 'DEBUG "front_wheels.py":'
# self.turning_offset = int(self.db.get('turning_offset', default_value=0))
self.turning_offset = 25
self.wheel = Servo(self.channel, bus_number=bus_number, offset=self.turning_offset)
self.DEBUG = debug
self.set_turning_max(45)
self.min_angle=30
self.max_angle=150.
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 initPeriph(self):
"""
Initialise tout les périphériques servant à l'application (Laser, Servos, Manette nunchuk...) et instancie les objets globals.
"""
try:
# GPIOs
Methods.writeFile("/sys/class/gpio/export", "66", "a")
Methods.writeFile("/sys/class/gpio/export", "69", "a")
Methods.writeFile("/sys/class/gpio/export", "45", "a")
# PWMs
Methods.writeFile("/sys/devices/bone_capemgr.8/slots", "am33xx_pwm", "a")
# UART
Methods.writeFile("/sys/devices/bone_capemgr.8/slots", "BB-UART1", "a")
sleep(2)
except IOError:
print "La configuration des périphériques a déjà été faites"
# Création du laser
self.laser = Laser(45)
# Création de la gestion d'indication des modes
self.modeObj = Mode(66, 69)
# Création des servos
self.verticalServo = Servo("P9_14", "10", False)
self.horizontalServo = Servo("P9_22", "11", True)
# Création de l'UART
self.uart = UART()
# Création du Nunchuk
try:
self.nunchuk = Nunchuk()
self.nunchukIsConnected = True
except IOError:
print "Erreur de connexion avec la manette \"Nunchuk\""
self.nunchukIsConnected = False
def SetServos(self):
servos = []
servos.append(Servo('Deg0', 'Deg180', 2)) #SRV1
servos.append(Servo('Deg180', 'Deg90', 3)) #SRV2
servos.append(Servo('Deg90', 'Deg180', 4)) #SRV3
servos.append(Servo('Deg180', 'Deg90', 14)) #SRV4
servos.append(Servo('Deg180', 'Deg90', 15)) #SRV5
servos.append(Servo('Deg90', 'Deg180', 18)) #SRV6
servos.append(Servo('Deg180', 'Deg90', 23)) #SRV7
return servos
def __init__(self):
#Initiate servo objects
baseServo = Servo(self.BasePanIndex, self.BaseHomeAngle,
self.BaseMinAngle, self.BaseMaxAngle)
shoulderServo = Servo(self.ShoulderIndex, self.ShoulderHomeAngle,
self.ShoulderMinAngle, self.ShoulderMaxAngle)
elbowServo = Servo(self.ElbowIndex, self.ElbowHomeAngle,
self.ElbowMinAngle, self.ElbowMaxAngle)
wristPanServo = Servo(self.WristPanIndex, self.WristPanHomeAngle,
self.WristPanMinAngle, self.WristPanMaxAngle)
wristServo = Servo(self.WristIndex, self.WristHomeAngle,
self.WristMinAngle, self.WristMaxAngle)
gripperServo = Servo(self.GripperIndex, self.GripperHomeAngle,
self.GripperMinAngle, self.GripperMaxAngle)
#Populate servo list
self.m_servoList = [
baseServo, shoulderServo, elbowServo, wristPanServo, wristServo,
gripperServo
]
#set current servo index
self.m_currentServoIndex = 0
#Move to home position
self.HomePosition()
def __init__(self, printer):
super(HPX2MaxPlugin, self).__init__(printer)
logging.debug('Activating ' + __PLUGIN_NAME__ + ' plugin...')
# Add the servo
self.head_servo = Servo(
int(
self.printer.config.get(
type(self).__name__, 'servo_channel', 1)), 500, 750, 90,
10)
# Load the config for angles
self.t0_angle = float(
self.printer.config.get(
type(self).__name__, 'extruder_0_angle', 20))
self.t1_angle = float(
self.printer.config.get(
type(self).__name__, 'extruder_1_angle', 175))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_HPX2Max(self.printer))
self.printer.processor.override_command('T1', T1_HPX2Max(self.printer))
def __init__(self):
_servo_type = {"Base": 0, "Leg": 2, "Arm": 4, "Neck": 6, "Head": 8}
self.pwm = PWM.PCA9685(
) # Initialise the PCA9685 using the default address (0x40).
self.pwm.set_pwm_freq(50)
# Create all the servos
self.servos = {}
for i in _servo_type.keys():
self.servos[i] = Servo(self.pwm, _servo_type[i])
print 'Servo "' + i + '" on channel ' + str(_servo_type[i])
self.scanThread = None
self.scan_event = threading.Event()
self.angryLock = threading.Lock()
self.angry = False
class Head(object):
configuration = conf("Settings.ini")
jaw = Servo(
channel = int(configuration.get_setting("Jaw","channel")),
frequency = int(configuration.get_setting("Jaw","frequency")),
minPulseLength = int(configuration.get_setting("Jaw","minPulseLength")),
maxPulseLength = int(configuration.get_setting("Jaw","maxPulseLength"))
)
left_antenna = Servo(
channel = int(configuration.get_setting("LeftAntenna","channel")),
frequency = int(configuration.get_setting("LeftAntenna","frequency")),
minPulseLength = int(configuration.get_setting("LeftAntenna","minPulseLength")),
maxPulseLength = int(configuration.get_setting("LeftAntenna","maxPulseLength"))
)
right_antenna = Servo(
channel = int(configuration.get_setting("RightAntenna","channel")),
frequency = int(configuration.get_setting("RightAntenna","frequency")),
minPulseLength = int(configuration.get_setting("RightAntenna","minPulseLength")),
maxPulseLength = int(configuration.get_setting("RightAntenna","maxPulseLength"))
)
left_brow_Vertical = Servo(
channel = int(configuration.get_setting("LeftBrowVertical","channel")),
frequency = int(configuration.get_setting("LeftBrowVertical","frequency")),
minPulseLength = int(configuration.get_setting("LeftBrowVertical","minPulseLength")),
maxPulseLength = int(configuration.get_setting("LeftBrowVertical","maxPulseLength"))
)
right_brow_vertical = Servo(
channel = int(configuration.get_setting("RightBrowVertical","channel")),
frequency = int(configuration.get_setting("RightBrowVertical","frequency")),
minPulseLength = int(configuration.get_setting("RightBrowVertical","minPulseLength")),
maxPulseLength = int(configuration.get_setting("RightBrowVertical","maxPulseLength"))
)
left_brow_horizontal = Servo(
channel = int(configuration.get_setting("LeftBrowHorizontal","channel")),
frequency = int(configuration.get_setting("LeftBrowHorizontal","frequency")),
minPulseLength = int(configuration.get_setting("LeftBrowHorizontal","minPulseLength")),
maxPulseLength = int(configuration.get_setting("LeftBrowHorizontal","maxPulseLength"))
)
right_brow_horizontal = Servo(
channel = int(configuration.get_setting("RightBrowHorizontal","channel")),
frequency = int(configuration.get_setting("RightBrowHorizontal","frequency")),
minPulseLength = int(configuration.get_setting("RightBrowHorizontal","minPulseLength")),
maxPulseLength = int(configuration.get_setting("RightBrowHorizontal","maxPulseLength"))
)
class Leg:
# constructor
def __init__(self, name, shoulder_pin, knee_pin, ankle_pin):
self.name = name
self.shoulder = Servo(shoulder_pin)
self.knee = Servo(knee_pin)
self.ankle = Servo(ankle_pin)
# step(self)
# step forward
def step(self):
shoulder.setPosition(180)
knee.setPosition(180)
ankle.setPosition(180)
print("Stepping {} forward".format(name))
# pull(self)
# pull forward
def pull(self):
shoulder.setPosition(90)
knee.setPosition(90)
ankle.setPosition(90)
print("Pulling {} backward".format(name))
# driveMode(self)
# set leg to drive position
def driveMode(self):
# for now, use name of leg to determine if in
# front or back. Eventually should use Leg
# Manager for this
if(self.name == "FR" || "FL")
shoulder.setPosition(90)
knee.setPosition(60)
ankle.setPosition(120)
if(self.name == "BR" || "BL")
shoulder.setPosition(90)
knee.setPosition(60)
ankle.setPosition(30)
# cleanUp(self)
# stop all servos in Leg
def cleanUp(self):
shoulder.stop
knee.stop
ankle.stop
# calibrate(self)
# for initial calibration of leg joints using command line
def calibrate(self):
s_value = int(raw_input("s_value: "))
k_value = int(raw_input("k_value: "))
a_value = int(raw_input("a_value: "))
self.shoulder.setPosition(s_value)
self.knee.setPosition(k_value)
self.ankle.setPosition(a_value)
def __init__(self, printer):
super(HPX2MaxPlugin, self).__init__(printer)
logging.debug('Activating '+__PLUGIN_NAME__+' plugin...')
# Add the servo
self.head_servo = Servo(int(self.printer.config.get(type(self).__name__, 'servo_channel', 1)),500,750,90,10)
# Load the config for angles
self.t0_angle = float(self.printer.config.get(type(self).__name__, 'extruder_0_angle', 20))
self.t1_angle = float(self.printer.config.get(type(self).__name__, 'extruder_1_angle', 175))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_HPX2Max(self.printer))
self.printer.processor.override_command('T1', T1_HPX2Max(self.printer))
def main():
GPIO.setmode(GPIO.BOARD)
get_out = False
raspberry_pin = 11
my_servo = Servo(raspberry_pin)
while not get_out:
angle = input("Write an angle: ")
my_servo.write_angle(int(angle))
duty_cycke = input("Write duty cyccle: ")
my_servo.write_duty_cycle(float(duty_cycke))
def run (self):
# s_joint = Servo(channel=0, min=250, max=327, freq=50)
# s_joint = Servo(channel=0, min=250, max=530, freq=100) # mg995
# time.sleep(4)
s_tilt = Servo(channel=1, min=250, max=327, freq=50)
time.sleep(1)
# s_pan = Servo(channel=2, min=250, max=380, freq=50)
s_pan = Servo(channel=2, min=300, max=380, freq=50)
# put your init and global variables here
# main loop
while 1:
headPosition = self.getInputs().headPosition
lampPosition = self.getInputs().lampPosition
s_tilt.move_to(1 - headPosition.y)
s_pan.move_to(1- headPosition.x)
# s_joint.move_to(lampPosition.z)
time.sleep(0.5)
pwm = PWM(0x40)
pwm.setPWMFreq(50)
pwm.softwareReset()
def parse(self, obj):
# build a Servo object out of the data read from the JSON
# then append it into the global "servos" list
if "body_part" not in obj:
raise Exception("Could not parse JSON object")
if "disabled" in obj and obj["disabled"] == True:
d = True
else:
d = False
self.all_servos.append(
Servo(obj["id"],
obj["min_pulse"],
obj["max_pulse"],
obj["min_angle"],
obj["max_angle"],
obj["default_angle"],
obj["body_part"],
disabled=d))
def __init__(self, channels):
"""
Each leg has 3 servos/channels
"""
if not len(channels) == 3:
raise LegException('len(channels) != 3')
Servo.bulkServoWrite = True
# angle offsets to line up with fk
self.servos = []
for i in range(0, 3):
self.servos.append(Servo(channels[i]))
self.servos[i].setServoLimits(self.s_offsets[i], *self.s_limits[i])
self.sit_angles = self.convertRawAngles(*self.sit_raw)
# initAngles = [0, 0, -90+30] # nico legs have a small offset
# initAngles = [0, 45, -90+30-45] # nico legs have a small offset
initAngles = self.convertRawAngles(*self.stand_raw)
self.stand_angles = initAngles
self.foot0 = self.fk(*initAngles) # rest/idle position of the foot/leg
def __init__(self, printer):
super(DualServoPlugin, self).__init__(printer)
logging.debug('Activating '+__PLUGIN_NAME__+' plugin...')
# Add the servo
channel = self.printer.config.get(type(self).__name__, 'servo_channel')
pulse_min = self.printer.config.getfloat(type(self).__name__, 'pulse_min')
pulse_max = self.printer.config.getfloat(type(self).__name__, 'pulse_max')
angle_min = self.printer.config.getfloat(type(self).__name__, 'angle_min')
angle_max = self.printer.config.getfloat(type(self).__name__, 'angle_max')
angle_init = self.printer.config.getfloat(type(self).__name__, 'extruder_0_angle')
self.head_servo = Servo(channel, pulse_min, pulse_max, angle_min, angle_max, angle_init)
# Load the config for angles
self.t0_angle = float(self.printer.config.getfloat(type(self).__name__, 'extruder_0_angle'))
self.t1_angle = float(self.printer.config.get(type(self).__name__, 'extruder_1_angle'))
# Override the changing tool command to trigger the servo
self.printer.processor.override_command('T0', T0_DualServo(self.printer))
self.printer.processor.override_command('T1', T1_DualServo(self.printer))
import time
from Adafruit_PWM_Servo_Driver import PWM
from Servo import Servo
if __name__ == '__main__':
# mg995
servo2 = Servo(channel=0, min=150, max=530, freq=100) # mg995
# sg90
# Servo pan
# servo2 = Servo(channel=2, min=250, max=380, freq=50)
# sg90 2
# servo tilt
# servo3 = Servo(channel=1, min=240, max=327, freq=50)
time.sleep(4)
servo2.move_to(0)
# # servo3.move_to(0)
# #
time.sleep(4)
servo2.move_to(1)
#
# time.sleep(0.1)
# servo2.move_to(0)
# # servo3.move_to(1)
#
# #
time.sleep(4)
#!/usr/bin/python import RPi.GPIO as GPIO import time from Servo import Servo ele = Servo(pin=25) rot = Servo(pin=24) ele.step_arriba(); GPIO.cleanup()
class App():
"""
Démarre une application qui permet de gérer la position sur deux axes d'un laser via des servomoteurs.
"""
def __init__(self):
"""
Méthode "main" de l'application.
"""
print "Initialisation des péripheriques..."
self.initPeriph()
print "Initialisation des servos..."
self.initServos()
print "Initialisation terminée !"
print "Programme prêt !"
print ""
#################################################
self.mode = "Manual"
self.modeObj.setMode(self.mode)
# Création des formes
self.shape = Shape(self.horizontalServo, self.verticalServo, self.laser, self.uart)
try:
while 1:
self.reading = self.uart.read()
if self.reading != "Up" and self.reading != "Left" and self.reading != "Right" and self.reading != "Down" and self.reading != "Center" and self.reading != "Laser" and self.reading != "":
self.mode = self.reading
self.modeObj.setMode(self.mode)
if self.mode == "Auto":
self.autoMode()
elif self.mode == "Semi-auto":
self.semiAutoMode()
elif self.mode == "Manual":
self.manualMode()
elif self.mode == "Wii":
if self.nunchukIsConnected:
self.wiiMode()
else:
print "La manette \"Nunchuk\" n'est pas connectée"
self.mode = "Manual"
else:
sleep(2)
self.mode = "Auto"
self.modeObj.setMode(self.mode)
except KeyboardInterrupt:
print "Arret du programme..."
self.modeObj.setMode("Stop")
self.laser.OFF()
self.initServos()
print "Programme arreté"
def initPeriph(self):
"""
Initialise tout les périphériques servant à l'application (Laser, Servos, Manette nunchuk...) et instancie les objets globals.
"""
try:
# GPIOs
Methods.writeFile("/sys/class/gpio/export", "66", "a")
Methods.writeFile("/sys/class/gpio/export", "69", "a")
Methods.writeFile("/sys/class/gpio/export", "45", "a")
# PWMs
Methods.writeFile("/sys/devices/bone_capemgr.8/slots", "am33xx_pwm", "a")
# UART
Methods.writeFile("/sys/devices/bone_capemgr.8/slots", "BB-UART1", "a")
sleep(2)
except IOError:
print "La configuration des périphériques a déjà été faites"
# Création du laser
self.laser = Laser(45)
# Création de la gestion d'indication des modes
self.modeObj = Mode(66, 69)
# Création des servos
self.verticalServo = Servo("P9_14", "10", False)
self.horizontalServo = Servo("P9_22", "11", True)
# Création de l'UART
self.uart = UART()
# Création du Nunchuk
try:
self.nunchuk = Nunchuk()
self.nunchukIsConnected = True
except IOError:
print "Erreur de connexion avec la manette \"Nunchuk\""
self.nunchukIsConnected = False
def initServos(self):
"""
Initialise les servomoteurs à la position 0,0 et éteint le laser.
"""
self.horizontalServo.setPosition(0)
self.verticalServo.setPosition(0)
Methods.sendData(self.uart, 0, 0, self.laser.getState())
def autoMode(self):
"""
Gére le mode "Auto" de l'application. Le mode "Auto" réalise une succession de toutes les formes pré-enregistré en boucle.
"""
while 1:
self.laser.ON()
print "Dessine un carré"
self.shape.startShape("Square", 2)
self.initServos()
self.reading = self.uart.read()
if self.reading == "Auto":
self.reading = self.uart.read()
if self.uart.inWaiting() != 0:
break
sleep(2)
print "Dessine un losange"
self.shape.startShape("Diamond", 3)
self.initServos()
if self.uart.inWaiting() != 0:
break
sleep(2)
print "Dessine un cercle"
self.shape.startShape("Circle", 3)
self.initServos()
if self.uart.inWaiting() != 0:
break
sleep(2)
print "Dessine un infini"
self.shape.startShape("Infinite", 3)
self.initServos()
sleep(2)
break
def semiAutoMode(self):
"""
Gére le mode "Semi-auto" de l'application. Le mode "Semi-auto" réalise les formes demandées via l'IHM.
"""
self.laser.OFF()
horizontalPositionTable = []
verticalPositionTable = []
laserStateTable = []
while 1:
pointDatas = self.uart.read()
if pointDatas == "Finish":
break
if pointDatas != "Semi-auto":
if pointDatas == "Square":
print "Dessine un carré"
self.shape.startShape(pointDatas, 1)
self.initServos()
elif pointDatas == "Diamond":
print "Dessine un losange"
self.shape.startShape(pointDatas, 1)
self.initServos()
elif pointDatas == "Circle":
print "Dessine un cercle"
self.shape.startShape(pointDatas, 1)
self.initServos()
elif pointDatas == "Infinite":
print "Dessine un infini"
self.shape.startShape(pointDatas, 1)
self.initServos()
else:
pointDatasTable = pointDatas.split(",")
horizontalPositionTable.append(int(pointDatasTable[0])*2)
verticalPositionTable.append(int(pointDatasTable[1])*2)
laserStateTable.append(pointDatasTable[2])
if len(horizontalPositionTable) != 0:
print "Dessine une forme personnalisé"
self.shape.start(horizontalPositionTable, verticalPositionTable, laserStateTable)
self.laser.OFF()
self.mode = "Manual"
def manualMode(self):
"""
Gére le mode "Manuel" de l'application. Le mode "Manuel" réalise les mouvements simples via l'IHM (Déplacement vert le Haut, la droite, allumer le laser...).
"""
for i in range(100):
self.reading = self.uart.read()
hPos = self.horizontalServo.getPosition()
vPos = self.verticalServo.getPosition()
if self.reading != "":
if self.reading == "Up":
vPos+=2
elif self.reading == "Left":
hPos-=2
elif self.reading == "Right":
hPos+=2
elif self.reading == "Down":
vPos-=2
elif self.reading == "Center":
hPos = 0
vPos = 0
elif self.reading == "Laser":
if self.laser.getState() == "0":
self.laser.ON()
else:
self.laser.OFF()
else:
break
self.horizontalServo.setPosition(hPos)
self.verticalServo.setPosition(vPos)
Methods.sendData(self.uart, self.horizontalServo.getPosition(), self.verticalServo.getPosition(), self.laser.getState())
sleep(0.01)
def wiiMode(self):
"""
Gére le mode "Wii" de l'application. Le mode "Wii" réalise les actions via la manette nunchuk.
"""
buttons = self.nunchuk.getButtons()
button_c = buttons[0]
button_z = buttons[1]
if button_z:
self.laser.ON()
else:
self.laser.OFF()
if button_c:
axis = self.nunchuk.getAccelerometerAxis()
hAngle = int(axis[0]*1.8-216.0)# Min=70 ; Max=170
vAngle = int(axis[1]*-1.8+225.0)# Min=175 ; Max=75
else:
position = self.nunchuk.getJoystickPosition()
hAngle = int(position[0]*0.93-123.58)# Min=36 ; Max=229
vAngle = int(position[1]*0.95-120.48)# Min=32 ; Max=221
self.horizontalServo.setPosition(hAngle)
self.verticalServo.setPosition(vAngle)
Methods.sendData(self.uart, self.horizontalServo.getPosition(), self.verticalServo.getPosition(), self.laser.getState())
sleep(0.01)
def setup_servo(self, config):
physical_pin = config.getint('Servo', 'physical_pin')
servo_speed_180 = config.getfloat('Servo', 'speed_180')
duty_min = config.getfloat('Servo', 'duty_min')
duty_max = config.getfloat('Servo', 'duty_max')
self.servo = Servo(physical_pin, servo_speed_180, duty_min, duty_max)
#WriringPi pins THROTTLE_PIN = 1 STEERING_PIN = 0 FRONT_LIGHT_PIN = 2 BACK_LIGHT_PIN = 3 # IO Modes on Raspberry OUTPUT = 1 INPUT = 0 print "[..] ", "Init Wiring Pi Lib" wiringpi2.wiringPiSetup() print "[..] ", "Init Servos" throttle = Servo(THROTTLE_PIN) steering = Servo(STEERING_PIN) wiringpi2.pwmSetMode(PWM_MODE_MS) print "[..] ", "Init LEDs" frontLed = LED(FRONT_LIGHT_PIN) backLed = LED(BACK_LIGHT_PIN) ### Open a Socket s = socket.socket() print "[..] New Socket on localhost:5556" host = '' # Get local machine name port = 5556 # Reserve a port for your service.
else:
obj = '/dev/ttyAMA0'
=======
if True:
obj = '/dev/ttyAMA0'
fake = False
else:
obj = SerialFake
fake = True
>>>>>>> e9ffd7ef371450a29e24962357af5942710c8547
serial = Serial()
serial.connect(obj)
<<<<<<< HEAD
servo = Servo(serial, fakemode = Config.FAKE_MODE)
=======
servo = Servo(serial, fakemode = fake)
>>>>>>> e9ffd7ef371450a29e24962357af5942710c8547
servo.init()
#ServoSeq = Servo_Sequencer(servo)
#ServoSeq.daemon = True;
#ServoSeq.start()
#sspeed = Servo_SpeedStep(servo)
sspeed = Servo_SpeedDelay(servo)
sspeed.start()
<<<<<<< HEAD
[ sspeed.setSpeed(i, i * 7) for i in range(0, 14) ]
#!/usr/bin/python
from Servo import Servo
from Position import Position
servo0 = Servo(0)
servo1 = Servo(1)
p = Position()
while True:
(x_pos, y_pos, z_pos) = p.get_position()
print("X:{} Y:{} Z:{}".format(x_pos, y_pos, z_pos))
x_pos_target = int((1.0 + x_pos) * 50)
y_pos_target = int((1.0 + y_pos) * 50)
servo0.move_to_position(x_pos_target)
servo1.move_to_position(y_pos_target)
import time
from Adafruit_PWM_Servo_Driver import PWM
from Servo import Servo
if __name__ == '__main__':
# mg995
servo = Servo(channel=0, min=115, max=460, freq=50) # mg995
# sg90
servo1 = Servo(channel=1, min=190, max=550, freq=50)
# sg90 2
servo2 = Servo(channel=2, min=190, max=550, freq=50)
time.sleep(0.5)
for i in range(11):
# print i
dc = i / 10.0
print dc
servo.move_to(dc)
servo1.move_to(dc)
servo2.move_to(dc)
time.sleep(0.3)
# time.sleep(0.5)
# servo.move_to(0)
# servo1.move_to(0)
# servo2.move_to(0)
def __init__(self):
""" Init """
logging.info("Redeem initializing " + version)
printer = Printer()
self.printer = printer
# check for config files
if not os.path.exists("/etc/redeem/default.cfg"):
logging.error("/etc/redeem/default.cfg does not exist, this file is required for operation")
sys.exit() # maybe use something more graceful?
# Parse the config files.
printer.config = CascadingConfigParser(
['/etc/redeem/default.cfg', '/etc/redeem/printer.cfg',
'/etc/redeem/local.cfg'])
# Find out which capes are connected
self.printer.config.parse_capes()
self.revision = self.printer.config.replicape_revision
if self.revision:
logging.info("Found Replicape rev. " + self.revision)
Path.set_axes(5)
else:
logging.warning("Oh no! No Replicape present!")
self.revision = "0A4A"
# We set it to 5 axis by default
Path.set_axes(5)
if self.printer.config.reach_revision:
Path.set_axes(8)
logging.info("Found Reach rev. "+self.printer.config.reach_revision)
# Get the revision and loglevel from the Config file
level = self.printer.config.getint('System', 'loglevel')
if level > 0:
logging.getLogger().setLevel(level)
if self.revision in ["00A4", "0A4A", "00A3"]:
PWM.set_frequency(100)
elif self.revision in ["00B1"]:
PWM.set_frequency(1000)
# Init the Paths
Path.axis_config = printer.config.getint('Geometry', 'axis_config')
# Init the end stops
EndStop.callback = self.end_stop_hit
EndStop.inputdev = self.printer.config.get("Endstops", "inputdev")
for es in ["X1", "X2", "Y1", "Y2", "Z1", "Z2"]:
pin = self.printer.config.get("Endstops", "pin_"+es)
keycode = self.printer.config.getint("Endstops", "keycode_"+es)
invert = self.printer.config.getboolean("Endstops", "invert_"+es)
self.printer.end_stops[es] = EndStop(pin, keycode, es, invert)
# Backwards compatibility with A3
if self.revision == "00A3":
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00A3("GPIO0_27", "GPIO1_29", "GPIO2_4" , 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00A3("GPIO1_12", "GPIO0_22", "GPIO2_5" , 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00A3("GPIO0_23", "GPIO0_26", "GPIO0_15", 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00A3("GPIO1_28", "GPIO1_15", "GPIO2_1" , 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00A3("GPIO1_13", "GPIO1_14", "GPIO2_3" , 4, "H", 4, 4)
elif self.revision == "00B1":
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00B1("GPIO0_27", "GPIO1_29", "GPIO2_4" , 11, 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00B1("GPIO1_12", "GPIO0_22", "GPIO2_5" , 12, 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00B1("GPIO0_23", "GPIO0_26", "GPIO0_15", 13, 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00B1("GPIO1_28", "GPIO1_15", "GPIO2_1" , 14, 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00B1("GPIO1_13", "GPIO1_14", "GPIO2_3" , 15, 4, "H", 4, 4)
else:
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00A4("GPIO0_27", "GPIO1_29", "GPIO2_4" , 0, 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00A4("GPIO1_12", "GPIO0_22", "GPIO2_5" , 1, 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00A4("GPIO0_23", "GPIO0_26", "GPIO0_15", 2, 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00A4("GPIO1_28", "GPIO1_15", "GPIO2_1" , 3, 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00A4("GPIO1_13", "GPIO1_14", "GPIO2_3" , 4, 4, "H", 4, 4)
if printer.config.reach_revision:
printer.steppers["A"] = Stepper_00A4("GPIO2_2" , "GPIO1_18", "GPIO0_14", 5, 5, "A", 5, 5)
printer.steppers["B"] = Stepper_00A4("GPIO1_14", "GPIO0_5" , "GPIO0_14", 6, 6, "B", 6, 6)
printer.steppers["C"] = Stepper_00A4("GPIO0_3" , "GPIO3_19", "GPIO0_14", 7, 7, "C", 7, 7)
# Enable the steppers and set the current, steps pr mm and
# microstepping
for name, stepper in self.printer.steppers.iteritems():
stepper.in_use = printer.config.getboolean('Steppers', 'in_use_' + name)
stepper.direction = printer.config.getint('Steppers', 'direction_' + name)
stepper.has_endstop = printer.config.getboolean('Endstops', 'has_' + name)
stepper.set_current_value(printer.config.getfloat('Steppers', 'current_' + name))
stepper.set_steps_pr_mm(printer.config.getfloat('Steppers', 'steps_pr_mm_' + name))
stepper.set_microstepping(printer.config.getint('Steppers', 'microstepping_' + name))
stepper.set_decay(printer.config.getboolean("Steppers", "slow_decay_" + name))
# Add soft end stops
Path.soft_min[Path.axis_to_index(name)] = printer.config.getfloat('Endstops', 'soft_end_stop_min_' + name)
Path.soft_max[Path.axis_to_index(name)] = printer.config.getfloat('Endstops', 'soft_end_stop_max_' + name)
# Commit changes for the Steppers
#Stepper.commit()
# Delta printer setup
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
opts = ["Hez", "L", "r", "Ae", "Be", "Ce", "A_radial", "B_radial", "C_radial", "A_tangential", "B_tangential", "C_tangential" ]
for opt in opts:
Delta.__dict__[opt] = printer.config.getfloat('Delta', opt)
Delta.recalculate()
# Set up cold ends
path = self.printer.config.get('Cold-ends', 'path', 0)
if os.path.exists(path):
self.printer.cold_ends.append(ColdEnd(path, "Cold End 0"))
logging.info("Found Cold end on " + path)
else:
logging.info("No cold end present in path: " + path)
# Make Mosfets, thermistors and extruders
heaters = ["E", "H", "HBP"]
if self.printer.config.reach_revision:
heaters.extend(["A", "B", "C"])
for e in heaters:
# Mosfets
channel = self.printer.config.getint("Heaters", "mosfet_"+e)
self.printer.mosfets[e] = Mosfet(channel)
# Thermistors
adc = self.printer.config.get("Heaters", "path_adc_"+e)
chart = self.printer.config.get("Heaters", "temp_chart_"+e)
self.printer.thermistors[e] = Thermistor(adc, "MOSFET "+e, chart)
# Extruders
onoff = self.printer.config.getboolean('Heaters', 'onoff_'+e)
prefix = self.printer.config.get('Heaters', 'prefix_'+e)
if e != "HBP":
self.printer.heaters[e] = Extruder(
self.printer.steppers[e],
self.printer.thermistors[e],
self.printer.mosfets[e], e, onoff)
else:
self.printer.heaters[e] = HBP(
self.printer.thermistors[e],
self.printer.mosfets[e], onoff)
self.printer.heaters[e].prefix = prefix
self.printer.heaters[e].P = self.printer.config.getfloat('Heaters', 'pid_p_'+e)
self.printer.heaters[e].I = self.printer.config.getfloat('Heaters', 'pid_i_'+e)
self.printer.heaters[e].D = self.printer.config.getfloat('Heaters', 'pid_d_'+e)
# Init the three fans. Argument is PWM channel number
self.printer.fans = []
if self.revision == "00A3":
self.printer.fans.append(Fan(0))
self.printer.fans.append(Fan(1))
self.printer.fans.append(Fan(2))
elif self.revision == "0A4A":
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
elif self.revision == "00B1":
self.printer.fans.append(Fan(7))
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
for f in self.printer.fans:
f.set_value(0)
# Init the servos
printer.servos = []
servo_nr = 0
while(printer.config.has_option("Servos", "servo_"+str(servo_nr)+"_enable")):
if printer.config.getboolean("Servos", "servo_"+str(servo_nr)+"_enable"):
channel = printer.config.getint("Servos", "servo_"+str(servo_nr)+"_channel")
angle_off = printer.config.getint("Servos", "servo_"+str(servo_nr)+"_angle_off")
s = Servo(channel, 500, 750, angle_off)
s.angle_on = printer.config.getint("Servos", "servo_"+str(servo_nr)+"_angle_on")
s.angle_off = angle_off
printer.servos.append(s)
logging.info("Added servo "+str(servo_nr))
servo_nr += 1
# Connect thermitors to fans
for t, therm in self.printer.heaters.iteritems():
for f, fan in enumerate(self.printer.fans):
if self.printer.config.getboolean('Cold-ends', "connect-therm-{}-fan-{}".format(t, f)):
c = Cooler(therm, fan, "Cooler-{}-{}".format(t, f), False)
c.ok_range = 4
c.set_target_temperature(60)
c.enable()
self.printer.coolers.append(c)
logging.info("Cooler connects therm {} with fan {}".format(t, f))
# Connect fans to M106
printer.controlled_fans = []
for i, fan in enumerate(self.printer.fans):
if self.printer.config.getboolean('Cold-ends', "add-fan-{}-to-M106".format(i)):
printer.controlled_fans.append(self.printer.fans[i])
logging.info("Added fan {} to M106/M107".format(i))
# Connect the cold end 0 to fan 2
# This is very "Thing" specific, should be configurable somehow.
if len(self.printer.cold_ends):
self.printer.coolers.append(
Cooler(self.printer.cold_ends[0], self.printer.fans[2],
"Cooler0", False))
self.printer.coolers[0].ok_range = 4
self.printer.coolers[0].set_target_temperature(60)
self.printer.coolers[0].enable()
# Make a queue of commands
self.printer.commands = JoinableQueue(10)
# Make a queue of commands that should not be buffered
self.printer.sync_commands = JoinableQueue()
self.printer.unbuffered_commands = JoinableQueue(10)
# Bed compensation matrix
Path.matrix_bed_comp = printer.load_bed_compensation_matrix()
Path.matrix_bed_comp_inv = np.linalg.inv(Path.matrix_bed_comp)
logging.debug("Loaded bed compensation matrix: \n"+str(Path.matrix_bed_comp))
for axis in printer.steppers.keys():
i = Path.axis_to_index(axis)
Path.max_speeds[i] = printer.config.getfloat('Planner', 'max_speed_'+axis.lower())
Path.home_speed[i] = printer.config.getfloat('Homing', 'home_speed_'+axis.lower())
Path.home_backoff_speed[i] = printer.config.getfloat('Homing', 'home_backoff_speed_'+axis.lower())
Path.home_backoff_offset[i] = printer.config.getfloat('Homing', 'home_backoff_offset_'+axis.lower())
Path.steps_pr_meter[i] = printer.steppers[axis].get_steps_pr_meter()
Path.backlash_compensation[i] = printer.config.getfloat('Steppers', 'backlash_'+axis.lower())
dirname = os.path.dirname(os.path.realpath(__file__))
# Create the firmware compiler
pru_firmware = PruFirmware(
dirname + "/firmware/firmware_runtime.p",
dirname + "/firmware/firmware_runtime.bin",
dirname + "/firmware/firmware_endstops.p",
dirname + "/firmware/firmware_endstops.bin",
self.revision, self.printer.config, "/usr/bin/pasm")
printer.maxJerkXY = printer.config.getfloat('Planner', 'maxJerk_xy')
printer.maxJerkZ = printer.config.getfloat('Planner', 'maxJerk_z')
printer.maxJerkEH = printer.config.getfloat('Planner', 'maxJerk_eh')
printer.move_cache_size = printer.config.getfloat('Planner', 'move_cache_size')
printer.print_move_buffer_wait = printer.config.getfloat('Planner', 'print_move_buffer_wait')
printer.min_buffered_move_time = printer.config.getfloat('Planner', 'min_buffered_move_time')
printer.max_buffered_move_time = printer.config.getfloat('Planner', 'max_buffered_move_time')
self.printer.processor = GCodeProcessor(self.printer)
self.printer.plugins = PluginsController(self.printer)
# Path planner
travel_default = False
center_default = False
home_default = False
self.printer.path_planner = PathPlanner(self.printer, pru_firmware)
for axis in printer.steppers.keys():
i = Path.axis_to_index(axis)
printer.acceleration[Path.axis_to_index(axis)] = printer.config.getfloat(
'Planner', 'acceleration_' + axis.lower())
# Sometimes soft_end_stop aren't defined to be at the exact hardware boundary.
# Adding 100mm for searching buffer.
if printer.config.has_option('Geometry', 'travel_' + axis.lower()):
printer.path_planner.travel_length[axis] = printer.config.getfloat('Geometry', 'travel_' + axis.lower())
else:
printer.path_planner.travel_length[axis] = (Path.soft_max[i] - Path.soft_min[i]) + .1
if axis in ['X','Y','Z']:
travel_default = True
if printer.config.has_option('Geometry', 'offset_' + axis.lower()):
printer.path_planner.center_offset[axis] = printer.config.getfloat('Geometry', 'offset_' + axis.lower())
else:
printer.path_planner.center_offset[axis] =(Path.soft_min[i] if Path.home_speed[i] > 0 else Path.soft_max[i])
if axis in ['X','Y','Z']:
center_default = True
if printer.config.has_option('Homing', 'home_' + axis.lower()):
printer.path_planner.home_pos[axis] = printer.config.getfloat('Homing', 'home_' + axis.lower())
else:
printer.path_planner.home_pos[axis] = printer.path_planner.center_offset[axis]
if axis in ['X','Y','Z']:
home_default = True
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
if travel_default:
logging.warning("Axis travel (travel_*) set by soft limits, manual setup is recommended for a delta")
if center_default:
logging.warning("Axis offsets (offset_*) set by soft limits, manual setup is recommended for a delta")
if home_default:
logging.warning("Home position (home_*) set by soft limits or offset_*")
logging.info("Home position will be recalculated...")
# convert home_pos to effector space
Az = printer.path_planner.home_pos['X']
Bz = printer.path_planner.home_pos['Y']
Cz = printer.path_planner.home_pos['Z']
z_offset = Delta.vertical_offset(Az,Bz,Cz) # vertical offset
xyz = Delta.forward_kinematics2(Az, Bz, Cz) # effector position
# The default home_pos, provided above, is based on effector space
# coordinates for carriage positions. We need to transform these to
# get where the effector actually is.
xyz[2] += z_offset
for i, a in enumerate(['X','Y','Z']):
printer.path_planner.home_pos[a] = xyz[i]
logging.info("Home position = %s"%str(printer.path_planner.home_pos))
# Enable PWM and steppers
printer.enable = Enable("P9_41")
printer.enable.set_enabled()
# Set up communication channels
printer.comms["USB"] = USB(self.printer)
printer.comms["Eth"] = Ethernet(self.printer)
if Pipe.check_tty0tty() or Pipe.check_socat():
printer.comms["octoprint"] = Pipe(printer, "octoprint")
printer.comms["toggle"] = Pipe(printer, "toggle")
printer.comms["testing"] = Pipe(printer, "testing")
printer.comms["testing_noret"] = Pipe(printer, "testing_noret")
# Does not send "ok"
printer.comms["testing_noret"].send_response = False
else:
logging.warning("Neither tty0tty or socat is installed! No virtual tty pipes enabled")
