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
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): # 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, 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 __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 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 __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, 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 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()
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()
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 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 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 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 __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
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, 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 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")) )
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, 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 __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 main(argv='0'): GPIO.setmode(GPIO.BOARD) # Servo Parameters raspberry_pin = 11 servo_model = 'MyOwn' frequency = 50 max_angle = 180 min_pulse_width_in_millis = 1 args = (frequency, max_angle, min_pulse_width_in_millis) my_own_servo = Servo(raspberry_pin, servo_model, *args) my_own_servo.write_angle(180) # Move to position 180 º time.sleep(1) my_own_servo.write_angle(0) # Move to position 0 º time.sleep(2) my_desired_angle = int(argv[0]) my_own_servo.write_angle(my_desired_angle)
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, driver_board, driver_index): self.s = Servo(self.driver_board, self.hip_index, 1, type='bend')
outboundMessageQueue = MessageQueue() lastReceivedMessageNumber = -1 currentReceivedMessageNumber = -1 stateStartTime = -1 # initialize motors LOGGER.Debug("Initializing motor objects...") leftMotor = Motor("LeftMotor", 1, MOTOR_MODES.K_PERCENT_VBUS) rightMotor = Motor("RightMotor", 2, MOTOR_MODES.K_PERCENT_VBUS) #initialize sensors LOGGER.Debug("Initializing sensorobjects...") #initialize servos LOGGER.Debug("Initializing servo objects...") cameraServo = Servo() # initialize motor handler and add motors LOGGER.Debug("Linking motors to motor handler...") motorHandler.addMotor(leftMotor) motorHandler.addMotor(rightMotor) #Initialize sensor handler and add sensors and motors LOGGER.Debug("Linking sensors and servos to sensor handler...") sensorHandler.addServo(cameraServo) #initialize encoder reset flags driveEncoderResetFlag = False scoopEncoderResetFlag = False depthEncoderResetFlag = False winchEncoderResetFlag = False
def __init__(self): firmware_version = "1.1.8~Raw Deal" logging.info("Redeem initializing " + firmware_version) printer = Printer() self.printer = printer Path.printer = printer printer.firmware_version = firmware_version # 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? if not os.path.exists("/etc/redeem/local.cfg"): logging.info("/etc/redeem/local.cfg does not exist, Creating one") os.mknod("/etc/redeem/local.cfg") # Parse the config files. printer.config = CascadingConfigParser([ '/etc/redeem/default.cfg', '/etc/redeem/printer.cfg', '/etc/redeem/local.cfg' ]) # Get the revision and loglevel from the Config file level = self.printer.config.getint('System', 'loglevel') if level > 0: logging.getLogger().setLevel(level) # Set up additional logging, if present: if self.printer.config.getboolean('System', 'log_to_file'): logfile = self.printer.config.get('System', 'logfile') formatter = '%(asctime)s %(name)-12s %(levelname)-8s %(message)s' printer.redeem_logging_handler = logging.handlers.RotatingFileHandler( logfile, maxBytes=2 * 1024 * 1024) printer.redeem_logging_handler.setFormatter( logging.Formatter(formatter)) printer.redeem_logging_handler.setLevel(level) logging.getLogger().addHandler(printer.redeem_logging_handler) logging.info("-- Logfile configured --") # 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) else: logging.warning("Oh no! No Replicape present!") self.revision = "00B3" # We set it to 5 axis by default Path.NUM_AXES = 5 if self.printer.config.reach_revision: logging.info("Found Reach rev. " + self.printer.config.reach_revision) if self.printer.config.reach_revision == "00A0": Path.NUM_AXES = 8 elif self.printer.config.reach_revision == "00B0": Path.NUM_AXES = 7 if self.revision in ["00A4", "0A4A", "00A3"]: PWM.set_frequency(100) elif self.revision in ["00B1", "00B2", "00B3"]: PWM.set_frequency(1000) # Test the alarm framework Alarm.printer = self.printer Alarm.executor = AlarmExecutor() alarm = Alarm(Alarm.ALARM_TEST, "Alarm framework operational") # Init the Watchdog timer printer.watchdog = Watchdog() # Enable PWM and steppers printer.enable = Enable("P9_41") printer.enable.set_disabled() # Init the Paths Path.axis_config = printer.config.getint('Geometry', 'axis_config') # Init the end stops EndStop.inputdev = self.printer.config.get("Endstops", "inputdev") # Set up key listener Key_pin.listener = Key_pin_listener(EndStop.inputdev) for es in ["Z2", "Y2", "X2", "Z1", "Y1", "X1"]: # Order matches end stop inversion mask in Firmware 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(printer, pin, keycode, es, invert) self.printer.end_stops[es].stops = self.printer.config.get( 'Endstops', 'end_stop_' + es + '_stops') # Init the 5 Stepper motors (step, dir, fault, DAC channel, name) if self.revision == "00A3": printer.steppers["X"] = Stepper_00A3("GPIO0_27", "GPIO1_29", "GPIO2_4", 0, "X") printer.steppers["Y"] = Stepper_00A3("GPIO1_12", "GPIO0_22", "GPIO2_5", 1, "Y") printer.steppers["Z"] = Stepper_00A3("GPIO0_23", "GPIO0_26", "GPIO0_15", 2, "Z") printer.steppers["E"] = Stepper_00A3("GPIO1_28", "GPIO1_15", "GPIO2_1", 3, "E") printer.steppers["H"] = Stepper_00A3("GPIO1_13", "GPIO1_14", "GPIO2_3", 4, "H") elif self.revision == "00B1": printer.steppers["X"] = Stepper_00B1("GPIO0_27", "GPIO1_29", "GPIO2_4", 11, 0, "X") printer.steppers["Y"] = Stepper_00B1("GPIO1_12", "GPIO0_22", "GPIO2_5", 12, 1, "Y") printer.steppers["Z"] = Stepper_00B1("GPIO0_23", "GPIO0_26", "GPIO0_15", 13, 2, "Z") printer.steppers["E"] = Stepper_00B1("GPIO1_28", "GPIO1_15", "GPIO2_1", 14, 3, "E") printer.steppers["H"] = Stepper_00B1("GPIO1_13", "GPIO1_14", "GPIO2_3", 15, 4, "H") elif self.revision == "00B2": printer.steppers["X"] = Stepper_00B2("GPIO0_27", "GPIO1_29", "GPIO2_4", 11, 0, "X") printer.steppers["Y"] = Stepper_00B2("GPIO1_12", "GPIO0_22", "GPIO2_5", 12, 1, "Y") printer.steppers["Z"] = Stepper_00B2("GPIO0_23", "GPIO0_26", "GPIO0_15", 13, 2, "Z") printer.steppers["E"] = Stepper_00B2("GPIO1_28", "GPIO1_15", "GPIO2_1", 14, 3, "E") printer.steppers["H"] = Stepper_00B2("GPIO1_13", "GPIO1_14", "GPIO2_3", 15, 4, "H") elif self.revision == "00B3": printer.steppers["X"] = Stepper_00B3("GPIO0_27", "GPIO1_29", 90, 11, 0, "X") printer.steppers["Y"] = Stepper_00B3("GPIO1_12", "GPIO0_22", 91, 12, 1, "Y") printer.steppers["Z"] = Stepper_00B3("GPIO0_23", "GPIO0_26", 92, 13, 2, "Z") printer.steppers["E"] = Stepper_00B3("GPIO1_28", "GPIO1_15", 93, 14, 3, "E") printer.steppers["H"] = Stepper_00B3("GPIO1_13", "GPIO1_14", 94, 15, 4, "H") elif self.revision in ["00A4", "0A4A"]: printer.steppers["X"] = Stepper_00A4("GPIO0_27", "GPIO1_29", "GPIO2_4", 0, 0, "X") printer.steppers["Y"] = Stepper_00A4("GPIO1_12", "GPIO0_22", "GPIO2_5", 1, 1, "Y") printer.steppers["Z"] = Stepper_00A4("GPIO0_23", "GPIO0_26", "GPIO0_15", 2, 2, "Z") printer.steppers["E"] = Stepper_00A4("GPIO1_28", "GPIO1_15", "GPIO2_1", 3, 3, "E") printer.steppers["H"] = Stepper_00A4("GPIO1_13", "GPIO1_14", "GPIO2_3", 4, 4, "H") # Init Reach steppers, if present. if printer.config.reach_revision == "00A0": printer.steppers["A"] = Stepper_reach_00A4("GPIO2_2", "GPIO1_18", "GPIO0_14", 5, 5, "A") printer.steppers["B"] = Stepper_reach_00A4("GPIO1_16", "GPIO0_5", "GPIO0_14", 6, 6, "B") printer.steppers["C"] = Stepper_reach_00A4("GPIO0_3", "GPIO3_19", "GPIO0_14", 7, 7, "C") elif printer.config.reach_revision == "00B0": printer.steppers["A"] = Stepper_reach_00B0("GPIO1_16", "GPIO0_5", "GPIO0_3", 5, 5, "A") printer.steppers["B"] = Stepper_reach_00B0("GPIO2_2", "GPIO0_14", "GPIO0_3", 6, 6, "B") # 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.getint("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) slave = printer.config.get('Steppers', 'slave_' + name) if slave: Path.add_slave(name, slave) logging.debug("Axis " + name + " has slave " + slave) # Commit changes for the Steppers #Stepper.commit() Stepper.printer = printer # 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() # Discover and add all DS18B20 cold ends. import glob paths = glob.glob("/sys/bus/w1/devices/28-*/w1_slave") logging.debug("Found cold ends: " + str(paths)) for i, path in enumerate(paths): self.printer.cold_ends.append(ColdEnd(path, "ds18b20-" + str(i))) logging.info("Found Cold end " + str(i) + " on " + 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) resistance = self.printer.config.getfloat("Heaters", "resistance_" + e) self.printer.thermistors[e] = Thermistor(adc, "MOSFET " + e, chart, resistance) self.printer.thermistors[e].printer = printer # 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) # Min/max settings self.printer.heaters[e].min_temp = self.printer.config.getfloat( 'Heaters', 'min_temp_' + e) self.printer.heaters[e].max_temp = self.printer.config.getfloat( 'Heaters', 'max_temp_' + e) self.printer.heaters[ e].max_temp_rise = self.printer.config.getfloat( 'Heaters', 'max_rise_temp_' + e) self.printer.heaters[ e].max_temp_fall = self.printer.config.getfloat( 'Heaters', 'max_fall_temp_' + 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 in ["00B1", "00B2", "00B3"]: self.printer.fans.append(Fan(7)) self.printer.fans.append(Fan(8)) self.printer.fans.append(Fan(9)) self.printer.fans.append(Fan(10)) if printer.config.reach_revision == "00A0": self.printer.fans.append(Fan(14)) self.printer.fans.append(Fan(15)) self.printer.fans.append(Fan(7)) # Disable all fans 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.get( "Servos", "servo_" + str(servo_nr) + "_channel") pulse_min = printer.config.getfloat( "Servos", "servo_" + str(servo_nr) + "_pulse_min") pulse_max = printer.config.getfloat( "Servos", "servo_" + str(servo_nr) + "_pulse_max") angle_min = printer.config.getfloat( "Servos", "servo_" + str(servo_nr) + "_angle_min") angle_max = printer.config.getfloat( "Servos", "servo_" + str(servo_nr) + "_angle_max") angle_init = printer.config.getfloat( "Servos", "servo_" + str(servo_nr) + "_angle_init") s = Servo(channel, pulse_min, pulse_max, angle_min, angle_max, angle_init) 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 not self.printer.config.has_option( 'Cold-ends', "connect-therm-{}-fan-{}".format(t, f)): continue if printer.config.getboolean( 'Cold-ends', "connect-therm-{}-fan-{}".format(t, f)): c = Cooler(therm, fan, "Cooler-{}-{}".format(t, f), True) # Use ON/OFF on these. c.ok_range = 4 opt_temp = "therm-{}-fan-{}-target_temp".format(t, f) if printer.config.has_option('Cold-ends', opt_temp): target_temp = printer.config.getfloat( 'Cold-ends', opt_temp) else: target_temp = 60 c.set_target_temperature(target_temp) c.enable() 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 not self.printer.config.has_option( 'Cold-ends', "add-fan-{}-to-M106".format(i)): continue 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 colds to fans for ce, cold_end in enumerate(self.printer.cold_ends): for f, fan in enumerate(self.printer.fans): option = "connect-ds18b20-{}-fan-{}".format(ce, f) if self.printer.config.has_option('Cold-ends', option): if self.printer.config.getboolean('Cold-ends', option): c = Cooler(cold_end, fan, "Cooler-ds18b20-{}-{}".format(ce, f), False) c.ok_range = 4 opt_temp = "cooler_{}_target_temp".format(ce) if printer.config.has_option('Cold-ends', opt_temp): target_temp = printer.config.getfloat( 'Cold-ends', opt_temp) else: target_temp = 60 c.set_target_temperature(target_temp) c.enable() printer.coolers.append(c) logging.info( "Cooler connects temp sensor ds18b20 {} with fan {}" .format(ce, f)) # Init roatray encs. printer.filament_sensors = [] # Init rotary encoders printer.rotary_encoders = [] for ex in ["E", "H", "A", "B", "C"]: if not printer.config.has_option('Rotary-encoders', "enable-{}".format(ex)): continue if printer.config.getboolean("Rotary-encoders", "enable-{}".format(ex)): logging.debug("Rotary encoder {} enabled".format(ex)) event = printer.config.get("Rotary-encoders", "event-{}".format(ex)) cpr = printer.config.getint("Rotary-encoders", "cpr-{}".format(ex)) diameter = printer.config.getfloat("Rotary-encoders", "diameter-{}".format(ex)) r = RotaryEncoder(event, cpr, diameter) printer.rotary_encoders.append(r) # Append as Filament Sensor ext_nr = Path.axis_to_index(ex) - 3 sensor = FilamentSensor(ex, r, ext_nr, printer) alarm_level = printer.config.getfloat( "Filament-sensors", "alarm-level-{}".format(ex)) logging.debug("Alarm level" + str(alarm_level)) sensor.alarm_level = alarm_level printer.filament_sensors.append(sensor) # 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.min_speeds[i] = printer.config.getfloat( 'Planner', 'min_speed_' + axis.lower()) Path.jerks[i] = printer.config.getfloat('Planner', 'max_jerk_' + 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.printer, "/usr/bin/pasm") 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 # Setting acceleration before PathPlanner init for axis in printer.steppers.keys(): Path.acceleration[Path.axis_to_index( axis)] = printer.config.getfloat( 'Planner', 'acceleration_' + axis.lower()) self.printer.path_planner = PathPlanner(self.printer, pru_firmware) for axis in printer.steppers.keys(): i = Path.axis_to_index(axis) # 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 Stepper timeout timeout = printer.config.getint('Steppers', 'timeout_seconds') printer.swd = StepperWatchdog(printer, timeout) if printer.config.getboolean('Steppers', 'use_timeout'): printer.swd.start() # 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" )
scoopEncoderResetFlag = False depthEncoderResetFlag = False winchEncoderResetFlag = False # initialize sensors LOGGER.Debug("Initializing sensor objects...") leftDriveCurrentSense = Sensor("LeftDriveCurrentSense") rightDriveCurrentSense = Sensor("RightDriveCurrentSense") collectorDepthCurrentSense = Sensor("CollectorDepthCurrentSense") collectorScoopsCurrentSense = Sensor("CollectorScoopsCurrentSense") winchMotorCurrentSense = Sensor("WinchMotorCurrentSense") scoopReedSwitch = Sensor("ScoopReedSwitch") bucketMaterialDepthSense = Sensor("BucketMaterialDepthSense") #initialize servos ratchetServo = Servo() camServo1 = Servo() camServo2 = Servo() camServo3 = Servo() camServo4 = Servo() # initialize sensor handler and add sensors LOGGER.Debug("Linking sensor objects to sensor handler...") sensorHandler.addSensor(leftDriveCurrentSense) sensorHandler.addSensor(rightDriveCurrentSense) sensorHandler.addSensor(collectorDepthCurrentSense) sensorHandler.addSensor(collectorScoopsCurrentSense) sensorHandler.addSensor(winchMotorCurrentSense) sensorHandler.addSensor(scoopReedSwitch) sensorHandler.addSensor(bucketMaterialDepthSense)