class PCA9685: def __init__(self, i2c_address = 0x40): self.i2c_address = i2c_address self._pca = PCA(address = i2c_address) if self._pca is None: raise Exception("Unable to instantiate PCA9685 device") self.set_pwm_frequency() self.duty = [0] * 16 def set_pwm_frequency(self, freq = 1526): self._pca.set_pwm_freq(freq) def set_on(self, channel): self.set_duty(channel, 100) def set_off(self, channel): self.set_duty(channel, 0) def set_duty(self, channel, duty): self._pca.set_pwm(channel, 0, int(duty * 4095 / 100)) self.duty[channel] = duty def disable(self, channel): self.set_off(channel) def disable_all(self): self.set_all_pwm(0, 0)
def setup(): frequency = 50 pwm.set_pwm_frequency(frequency) bitLength = 1000 / frequency / 4096 #Creating an array with the 4 servo's parameters servos = [] servo[0] = Servo(0, 180, 0.5, 2.5) servo[1] = Servo(1, 180, 0.5, 2) servo[2] = Servo(2, 180, 0.5, 2) servo[3] = Servo(3, 180, 0.7, 2.3)
def __init__(self, logger, settings): """ Initializes the LED pin out. Room to change shelf mappings. Constructs the 16 bit pwm modules. Creates the Schedule to track the stage of the day. Parameters ---------- logger : Logger Logs and saves the data seperated by day settings : Settings Reference to the shelves configuration. Contains LED and RGB configs """ self.logger = logger self.settings = settings self.logger.info('Initializing Electronics object') self.led_pins = { 'A1': 11, 'A2': 10, 'A3': 9, 'B1': 0, 'B2': 1, 'B3': 2, 'C1': 3, 'C2': 4, 'C3': 5 } self.rgb_pins = {'A': [8, 9, 10], 'B': [0, 1, 2], 'C': [4, 5, 6]} self.logger.debug('LED pin out ' + str(self.led_pins)) self.logger.debug('RGB pin out ' + str(self.rgb_pins)) try: self.pwm_led = PCA9685(address=0x40) self.pwm_rgb = PCA9685(address=0x41) # Higher the frequency, the smoother the light looks self.pwm_led.set_pwm_freq(120) self.pwm_rgb.set_pwm_freq(120) self.logger.info('Initialized the LED and RGB pwm modules') except: self.logger.critical('Unable to access I/O pwm module') self.logger.critical('Electronic initializing failed') else: self.logger.info('Electronic Initializing finished') self.schedule = Schedule(logger)
def __init__(self, ): GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(LaserCamBox.AMP_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.OE_PIN, GPIO.OUT, initial=GPIO.HIGH) GPIO.setup(LaserCamBox.LASER_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.CAM_LED_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED1_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED2_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED3_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.TOP_BTN_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.add_event_detect(LaserCamBox.TOP_BTN_PIN, GPIO.FALLING, bouncetime=500, callback=self.btn_callback) self.top_btn_func = None self.PWM = PWM(LaserCamBox.PWM_I2C) self.PWM.set_pwm_freq(LaserCamBox.PWM_FREQ) self.camera = None self._mjpegger = None self.camera_x = LaserCamBox.CAMERA_HOME[0] self.camera_y = LaserCamBox.CAMERA_HOME[1] self.camera_step = LaserCamBox.DEF_STEP self.laser_x = LaserCamBox.LASER_HOME[0] self.laser_y = LaserCamBox.LASER_HOME[1] self.laser_step = LaserCamBox.DEF_STEP
def set_up_pins(self): # Set up file lock. self.lock = wgloballock.WFileLock("my_lock.txt", dir="/home/pi/temp") GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(config.touchInputPin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) GPIO.setup(config.touchOutputPin, GPIO.OUT) self.touchInputPin = 0 self.touchOutputPin = 0 self.forward_ch = 0 self.backwards_ch = 1 # PWM Setup Variables self.lock.acquire() self.motor = PCA9685( 0x41 ) # Always use the default i2c address for the PWM hats. This call turns off any pwm signal currently being sent. self.pwm_freq = 5000 self.motor.set_pwm_freq(self.pwm_freq) self.lock.release() self.pwm_bitres = 4096 self.pwm_scaler = self.pwm_bitres / (1 / self.pwm_freq)
def __init__(self): """ Attribute initialization """ self.speed = { "range_pwm": (375, 409, 413), # Be careful, wrong values could destroy the car. "current": 0, "target": 0, "pin": 5, } self.direction = { "range_pwm": (315, 410, 530), "current": 0, "target": 0, "pin": 15, } self.speed_lock = Lock() self.dir_lock = Lock() # Connection initialization with servos try: from Adafruit_PCA9685 import PCA9685 self.pwm = PCA9685() self.pwm.set_pwm_freq(60) except Exception as e: print("Error :", e)
def send_signal(port=23, pulse=46, value=7., duration=1., verbose=True): try: from Adafruit_PCA9685 import PCA9685 pwm = PCA9685() pwm.set_pwm_freq(60) except Exception as e: print('The car will not be able to move') print('Are you executing this code on your laptop?') print('The adafruit error: ', e) pwm = None print(" port : " + str(port) + ", pulse : " + str(pulse) + ", signal : " + str(signal) + " , duration : " + str(duration)) if pwm is not None: pwm.set_pwm(port, 0, int(value)) if verbose: print('GAS : ', value) else: if verbose: print('PWM module not loaded') time.sleep(duration) print("STOP") return
def __init__(self): self.mode = 'resting' self.started = False # If True, car will move, if False car won't move. self.model = None self.current_model = None # Name of the model self.graph = None self.curr_dir = 0 self.curr_gas = 0 self.max_speed_rate = 0.5 self.model_loaded = False self.streaming_state = False self.n_img = 0 self.save_number = 0 self.verbose = True self.mode_function = self.default_call # PWM setup try: from Adafruit_PCA9685 import PCA9685 self.pwm = PCA9685() self.pwm.set_pwm_freq(60) except Exception as e: print('Adafruit error : ', e) self.pwm = None self.load_config() from threading import Thread self.camera_thread = Thread(target=self.camera_loop, args=()) self.camera_thread.start()
def __init__(self, i2c_address = 0x40): self.i2c_address = i2c_address self._pca = PCA(address = i2c_address) if self._pca is None: raise Exception("Unable to instantiate PCA9685 device") self.set_pwm_frequency() self.duty = [0] * 16
def __init__(self, queue): super(LightingWorker, self).__init__() self.queue = queue self.pwm = PCA9685() self.pwm.set_pwm_freq(600) self.current = {} self.wanted = {} self.rates = {}
def rotate(self, angle, debug): #Checks if the angle asked is in the servo's range if not 0 <= angle <= self.angleMax: print("[Error] Angle not in servo range (0 ~ " + str(self.angleMax) + " | " + str(angle) + ")") return None #Compute the pulse length in ms from the angle value pulse = self.scaler(angle) #Determine the equivalent in bits bits = pulse // bitLength #Tells the servo to rotate to that angle pwm.set_pwm(self.channel, 0, int(bits)) if debug: print("---Channel---") print(self.channel) print("---Angle & Pulse---") print(angle, pulse) print("---BitLength & Bits---") print(bitLength, bits)
def __init__(self, address, channels=[]): assert address > 0, "Invalid Pwm Address" assert isinstance(channels, list), "Channels must be a list" for channel in channels: assert channel >= 0 and channel < Pwm.CHANNELS self.address = address self.module = PCA9685(address) self.module.set_pwm_freq(Pwm.SERVO_DEFAULT_FREQUENCY) self.channels = channels
def __init__(self, pins, freq=200, address=0x40): """ Initialize the driver. :param pins: The pin numbers, that should be controlled. :param freq: The pwm frequency. :param address: The address of the PCA9685. """ super().__init__(pins, self.RESOLUTION, freq) self._device = PCA9685(address) self._device.set_pwm_freq(self._freq)
def __init__(self, zero_power=init_hw_constants.ZERO_POWER, neg_max_power=init_hw_constants.NEG_MAX_POWER, pos_max_power=init_hw_constants.POS_MAX_POWER, frequency=init_hw_constants.FREQUENCY): self.ZERO_POWER = zero_power self.NEG_MAX_POWER = neg_max_power self.POS_MAX_POWER = pos_max_power self.pwm = PCA9685() self.pwm.set_pwm_freq(frequency) # By default, set everything to off self.pwm.set_all_pwm(0, self.ZERO_POWER)
def __init__(self, pca9865_address=0x40, en_pin=None, tilt_servo_ch=None, rotation_servo_ch=None): if en_pin is not None: self.en_led = LED(en_pin) self.en_led.off() else: self.en_led = None try: self.expander = PCA9685(address=pca9865_address) self.expander.set_pwm_freq(60) if tilt_servo_ch is not None: self.tilt_servo = ServoController( expander=self.expander, channel=tilt_servo_ch, min_angle=0, max_angle=90, min_pulse=140, max_pulse=340 ) self.tilt_servo.set_angle(0) else: self.tilt_servo = None if rotation_servo_ch is not None: self.rotation_servo = ServoController( expander=self.expander, channel=rotation_servo_ch, min_angle=0, max_angle=180, min_pulse=180, max_pulse=660 ) self.rotation_servo.set_angle(0) else: self.rotation_servo = None except: self.expander = None self.tilt_servo = None self.rotation_servo = None print("Collector positioner could not initiated")
def __init__(self): self.graph = None # voir predict_from_img A QUOI CA SERT ? self.mode = 'resting' # resting, training, auto or dirauto # if True, car will move, if False car won't move. self.started = False self.model_loaded = False self.model = None self.current_model = None self.curr_dir = 0 self.curr_gas = 0 self.max_speed_rate = 0.5 self.speed_mode = 'constant' # Verbose is a general programming term for produce lots of logging output. self.verbose = True self.mode_function = self.default_call # Camera Attribut #self.streaming_state = False self.n_img = 0 from threading import Thread # self.camera_thread = Thread(target=self.camera_loop, args=()) DECOMMENTER # self.camera_thread.start() DECOMMENTER self.load_config() # PWM setup try: from Adafruit_PCA9685 import PCA9685 self.pwm = PCA9685() self.pwm.set_pwm_freq(60) except Exception as e: print('The car ill not be able to move') print('Are you executing this code on your laptop?') print('The adafruit error: ', e) self.pwm = None
class PWM(object): __ARM = 290 __pwm = PCA9685() def __init__(self): self.__pwm.set_pwm_freq(48) def arm(self): self.__pwm.set_pwm(LF, 0, self.__ARM) self.__pwm.set_pwm(RF, 0, self.__ARM) self.__pwm.set_pwm(LD, 0, self.__ARM) self.__pwm.set_pwm(RD, 0, self.__ARM) def set_speed(self, code, speed): speed += self.__ARM if speed > 350: speed = 350 elif speed < 150: speed = 150 self.__pwm.set_pwm(code, 0, speed) sleep(1.0 / 48)
def __init__(self): self.mode = 'resting' # resting, training, auto or dirauto self.speed_mode = 'constant' # constant, confidence or auto self.started = False # If True, car will move, if False car won't move. self.model = None self.current_model = None # Name of the model self.graph = None self.curr_dir = 0 self.curr_gas = 0 self.max_speed_rate = 0.5 self.model_loaded = False self.streaming_state = False self.dist = 100 self.n_img = 0 self.save_number = 0 self.verbose = True self.mode_function = self.default_call # PWM setup try: from Adafruit_PCA9685 import PCA9685 self.pwm = PCA9685() self.pwm.set_pwm_freq(60) except Exception as e: print('The car will not be able to move') print('Are you executing this code on your laptop?') print('The adafruit error: ', e) self.pwm = None self.load_config() from threading import Thread self.camera_thread = Thread(target=self.camera_loop, args=()) self.camera_thread.start()
def __init__(self, radius=0, centre=0, vertical_channel=0, horizontal_channel=1): """ The constructor takes a horizontal and vertical channel as input, which correspond to which channel each servomotor is connected to on the Raspberry PI PWM Hat. The 'radius' and 'centre' arguments are for the simulation model, which is currently not included in this codebase. """ # PWM Setup Variables self.eye = PCA9685( 0x40) # Always use the default i2c address for the PWM hats self.vert_ch = vertical_channel self.horiz_ch = horizontal_channel self.pwm_freq = 50.0 self.eye.set_pwm_freq(int(self.pwm_freq)) self.pwm_bitres = 4096.0 self.pwm_scaler = self.pwm_bitres / (1 / self.pwm_freq) # The PWM periods for a typical servomotor is between 1ms and 2ms, with 1.5ms being the middle of the useable range. self.pwm_period_min = 10e-4 * self.pwm_scaler # This is the minimum angle (1ms) These will need to be calibrated. self.pwm_period_zero = 15e-4 * self.pwm_scaler # This angle is the middle of the servo range (1.5ms) self.pwm_period_max = 20e-4 * self.pwm_scaler # This is the maximum angle (2ms) self.pwm_period_range = self.pwm_period_max - self.pwm_period_min self.servo_angle_max = 70 # This is the range of the unrestricted servo movement self.servo_angle_min = -70 # This is the range of the unrestricted servo movement and will need to be calibrated. self.servo_angle_range = self.servo_angle_max - self.servo_angle_min self.eye_movement_max_radius = 60 self.eye_movement_corner_angle = math.sqrt( (self.eye_movement_max_radius ^ 2) / 2) # Servo Position Tracking Variables (degrees) self.eye_vert_angle = 0 self.eye_horiz_angle = 0 # Eye Simulation self.radius = radius self.centre = centre
def __init__(self): self.pwm = PCA9685() # Set the frequency high for motors to run smoothly # We need to change this to preserve the servo later... self.pwm.set_pwm_freq(300) # Based on the pwm frequency, these variables represent # max and min speeds/directions self.motor_min = 1700 self.motor_max = 2500 self.servo_min = 1800 self.servo_max = 2700 # Set the initial neutral speed/direction self.motor_pulse = 2150 self.servo_pulse = 2250 # Configure how quickly values change self.motor_speed = 2 # Keep track of the current motor direction self.direction = 'forward'
class LaserCamBox(): """Provides hardware interface to laser/camera box.""" TOP_BTN_PIN = 23 # GPIO pin for button on top AMP_PIN = 18 # GPIO pin for enabling audio amp OE_PIN = 4 # GPIO pin for PWM enable(LOW)/disable(HIGH) LASER_PIN = 16 # GPIO pin for laser on(HIGH)/off(LOW) CAM_LED_PIN = 20 # white LED for camera LED1_PIN = 26 # front status LED 1 - TOP : GREEN LED2_PIN = 19 # front status LED 2 - MID : BLUE LED3_PIN = 13 # front status LED 3 - BOT : GREEN LASER_X_CHAN = 2 # PWM channel for laser X LASER_Y_CHAN = 3 # PWM channel for laser Y CAMERA_X_CHAN = 0 # PWM channel for camera X CAMERA_Y_CHAN = 1 # PWM channel for camera Y #SERVO_MIN = 205 # minimum PWM value for servo #SERVO_MAX = 410 # maximum PWM value for servo SERVO_MIN = 130 # minimum PWM value for servo SERVO_MAX = 600 # maximum PWM value for servo CAMERA_HOME = (290,100) LASER_HOME = (320,490) DEF_STEP = 10 # default servo step PWM_I2C = 0x40 # i2c address for PWM controller PWM_FREQ = 50 # frequency in Hz for PWM controller def __init__(self, ): GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(LaserCamBox.AMP_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.OE_PIN, GPIO.OUT, initial=GPIO.HIGH) GPIO.setup(LaserCamBox.LASER_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.CAM_LED_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED1_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED2_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED3_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.TOP_BTN_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.add_event_detect(LaserCamBox.TOP_BTN_PIN, GPIO.FALLING, bouncetime=500, callback=self.btn_callback) self.top_btn_func = None self.PWM = PWM(LaserCamBox.PWM_I2C) self.PWM.set_pwm_freq(LaserCamBox.PWM_FREQ) self.camera = None self._mjpegger = None self.camera_x = LaserCamBox.CAMERA_HOME[0] self.camera_y = LaserCamBox.CAMERA_HOME[1] self.camera_step = LaserCamBox.DEF_STEP self.laser_x = LaserCamBox.LASER_HOME[0] self.laser_y = LaserCamBox.LASER_HOME[1] self.laser_step = LaserCamBox.DEF_STEP def register_btn_func(self, func): self.top_btn_func = func def btn_callback(self, chan): if chan == LaserCamBox.TOP_BTN_PIN: if not self.top_btn_func == None: self.top_btn_func() def _bound(self, val, val_min, val_max): val = val if val > val_min else val_min val = val if val < val_max else val_max return val def check_servo_values(self, ): """Bound the PWM values to MIN and MAX.""" self.laser_x = self._bound(self.laser_x, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) self.laser_y = self._bound(self.laser_y, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) self.camera_x = self._bound(self.camera_x, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) self.camera_y = self._bound(self.camera_y, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) def update_pwm(self, ): """Send current PWM values to PWM device.""" self.check_servo_values() self.PWM.set_pwm(LaserCamBox.LASER_X_CHAN, 0, self.laser_x) self.PWM.set_pwm(LaserCamBox.LASER_Y_CHAN, 0, self.laser_y) self.PWM.set_pwm(LaserCamBox.CAMERA_X_CHAN, 0, self.camera_x) self.PWM.set_pwm(LaserCamBox.CAMERA_Y_CHAN, 0, self.camera_y) def enable_pwm(self, update=False): """Enable power to the servos.""" GPIO.output(LaserCamBox.OE_PIN, GPIO.LOW) if update: self.update_pwm() def disable_pwm(self, ): """Disable power to the servos.""" GPIO.output(LaserCamBox.OE_PIN, GPIO.HIGH) def get_pwm_state(self, ): """Return 0 if servos are enabled or 1 if disabled.""" return GPIO.input(LaserCamBox.OE_PIN) def is_pwm_enabled(self, ): """Return True if servos are enabled, otherwise False.""" state = GPIO.input(LaserCamBox.OE_PIN) if state == 0: return True elif state == 1: return False else: return None def camera_up(self, step=None): """Move the camera up the specified number of steps.""" if step == None: step = self.camera_step self.camera_y -= step self.update_pwm() def camera_down(self, step=None): """Move the camera down the specified number of steps.""" if step == None: step = self.camera_step self.camera_y += step self.update_pwm() def camera_left(self, step=None): """Move the camera left the specified number of steps.""" if step == None: step = self.camera_step self.camera_x -= step self.update_pwm() def camera_right(self, step=None): """Move the camera right the specified number of steps.""" if step == None: step = self.camera_step self.camera_x += step self.update_pwm() def camera_move_relative(self, amount): """Move the camera the amount specified in the tuple.""" self.camera_x += amount[0] self.camera_y += amount[1] self.update_pwm() def camera_home(self, ): """Move the camera to the home positon.""" self.camera_set_position(LaserCamBox.CAMERA_HOME) def camera_set_position(self, position): """Move the camera to the absolute position.""" if not isinstance(position, tuple): return if not len(position) == 2: return self.camera_x = position[0] self.camera_y = position[1] self.update_pwm() def mjpegstream_start(self, port=8081, resize=(640,360)): """Start thread to serve MJPEG stream on specified port.""" if not self._mjpegger == None: return camera = picamera.PiCamera(sensor_mode=5) camera.hflip = True camera.vflip = True kwargs = {'camera':camera, 'port':port, 'resize':resize} self._mjpegger = mjpegger.MJPEGThread(kwargs=kwargs) self._mjpegger.start() while not self._mjpegger.streamRunning: pass def mjpegstream_stop(self, ): """Stop the MJPEG stream, if running.""" if not self._mjpegger == None: if self._mjpegger.is_alive(): self._mjpegger.stop() self._mjpegger = None def mjpgstream_is_alive(self, ): """Return True if stream is running, False otherwise.""" if self._mjpegger == None: return False else: return self._mjpegger.is_alive() def camera_get_position(self, ): """Return the current camera position.""" return (self.camera_x, self.camera_y) def laser_up(self, step=None): """Move the laser up the specified number of steps.""" if step == None: step = self.laser_step self.laser_y += step self.update_pwm() def laser_down(self, step=None): """Move the laser down the specified number of steps.""" if step == None: step = self.laser_step self.laser_y -= step self.update_pwm() def laser_left(self, step=None): """Move the laser left the specified number of steps.""" if step == None: step = self.laser_step self.laser_x -= step self.update_pwm() def laser_right(self, step=None): """Move the laser right the specified number of steps.""" if step == None: step = self.laser_step self.laser_x += step self.update_pwm() def laser_home(self, ): """Move the laser to the home positon.""" self.laser_set_position(LaserCamBox.LASER_HOME) def laser_set_position(self, position): """Move the laser to the absolute position.""" if not isinstance(position, tuple): return if not len(position) == 2: return self.laser_x = position[0] self.laser_y = position[1] self.update_pwm() def laser_get_position(self, ): """Return the current laser position.""" return (self.laser_x, self.laser_y) def laser_on(self, ): """Turn the laser on.""" GPIO.output(LaserCamBox.LASER_PIN, GPIO.HIGH) def laser_off(self, ): """Turn the laser off.""" GPIO.output(LaserCamBox.LASER_PIN, GPIO.LOW) def get_laser_state(self, ): """Return current laser state.""" return GPIO.input(LaserCamBox.LASER_PIN) def camera_led_on(self, ): """Turn camera LED on.""" GPIO.output(LaserCamBox.CAM_LED_PIN, GPIO.HIGH) def camera_led_off(self, ): """Turn camera LED off.""" GPIO.output(LaserCamBox.CAM_LED_PIN, GPIO.LOW) def get_camera_led_state(self, ): """Return current LED state.""" return GPIO.input(LaserCamBox.CAM_LED_PIN) def status_led_on(self, led): """Turn on the specified front panel status LED.""" if led == 1: GPIO.output(LaserCamBox.LED1_PIN, GPIO.HIGH) elif led == 2: GPIO.output(LaserCamBox.LED2_PIN, GPIO.HIGH) elif led == 3: GPIO.output(LaserCamBox.LED3_PIN, GPIO.HIGH) else: pass def status_led_off(self, led): """Turn off the specified front panel status LED.""" if led == 1: GPIO.output(LaserCamBox.LED1_PIN, GPIO.LOW) elif led == 2: GPIO.output(LaserCamBox.LED2_PIN, GPIO.LOW) elif led == 3: GPIO.output(LaserCamBox.LED3_PIN, GPIO.LOW) else: pass def status_led_all_off(self, ): """Turn off all of the front panel status LEDs.""" self.statusLEDOff(1) self.statusLEDOff(2) self.statusLEDOff(3) def enable_audio(self, ): """Enable the audio amplifier.""" GPIO.output(LaserCamBox.AMP_PIN, GPIO.HIGH) def disable_audio(self, ): """Disable the audio amplifier.""" GPIO.output(LaserCamBox.AMP_PIN, GPIO.LOW) def speak(self, msg="hello world"): """Output the supplied message on the speaker.""" self.enable_audio() opt = '-p 45 -s 165' # espeak options os.system('espeak '+opt+' "%s"' % msg) self.disable_audio()
def steering_listener(): rospy.Subscriber("steering_pwm", Int8, servo_callback) def emergency_callback(data): dc_motor.set_pwm(STANDSTILL_PWM) def emergency_listener(): rospy.Subscriber("emergency_stop", String, emergency_callback) if __name__ == '__main__': rospy.init_node('vehicle_controller') pca = PCA9685(busnum=1) # check which bus that is used pca.set_pwm_freq(pwm_freq) print(pca) servo = ServoMotor(pca) dc_motor = DC_Motor(pca) emergency_listener() servo.set_pwm(servo_init_pwm) dc_motor.set_pwm(dc_init_pwm) speed_listener() steering_listener() rospy.spin()
class LaserCamBox(): """Provides hardware interface to laser/camera box.""" TOP_BTN_PIN = 23 # GPIO pin for button on top AMP_PIN = 18 # GPIO pin for enabling audio amp OE_PIN = 4 # GPIO pin for PWM enable(LOW)/disable(HIGH) LASER_PIN = 16 # GPIO pin for laser on(HIGH)/off(LOW) CAM_LED_PIN = 20 # white LED for camera LED1_PIN = 26 # front status LED 1 - TOP : GREEN LED2_PIN = 19 # front status LED 2 - MID : BLUE LED3_PIN = 13 # front status LED 3 - BOT : GREEN LASER_X_CHAN = 2 # PWM channel for laser X LASER_Y_CHAN = 3 # PWM channel for laser Y CAMERA_X_CHAN = 0 # PWM channel for camera X CAMERA_Y_CHAN = 1 # PWM channel for camera Y #SERVO_MIN = 205 # minimum PWM value for servo #SERVO_MAX = 410 # maximum PWM value for servo SERVO_MIN = 130 # minimum PWM value for servo SERVO_MAX = 600 # maximum PWM value for servo CAMERA_HOME = (290, 100) LASER_HOME = (320, 490) DEF_STEP = 10 # default servo step PWM_I2C = 0x40 # i2c address for PWM controller PWM_FREQ = 50 # frequency in Hz for PWM controller def __init__(self, ): GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(LaserCamBox.AMP_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.OE_PIN, GPIO.OUT, initial=GPIO.HIGH) GPIO.setup(LaserCamBox.LASER_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.CAM_LED_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED1_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED2_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.LED3_PIN, GPIO.OUT, initial=GPIO.LOW) GPIO.setup(LaserCamBox.TOP_BTN_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.add_event_detect(LaserCamBox.TOP_BTN_PIN, GPIO.FALLING, bouncetime=500, callback=self.btn_callback) self.top_btn_func = None self.PWM = PWM(LaserCamBox.PWM_I2C) self.PWM.set_pwm_freq(LaserCamBox.PWM_FREQ) self.camera = None self._mjpegger = None self.camera_x = LaserCamBox.CAMERA_HOME[0] self.camera_y = LaserCamBox.CAMERA_HOME[1] self.camera_step = LaserCamBox.DEF_STEP self.laser_x = LaserCamBox.LASER_HOME[0] self.laser_y = LaserCamBox.LASER_HOME[1] self.laser_step = LaserCamBox.DEF_STEP def register_btn_func(self, func): self.top_btn_func = func def btn_callback(self, chan): if chan == LaserCamBox.TOP_BTN_PIN: if not self.top_btn_func == None: self.top_btn_func() def _bound(self, val, val_min, val_max): val = val if val > val_min else val_min val = val if val < val_max else val_max return val def check_servo_values(self, ): """Bound the PWM values to MIN and MAX.""" self.laser_x = self._bound(self.laser_x, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) self.laser_y = self._bound(self.laser_y, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) self.camera_x = self._bound(self.camera_x, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) self.camera_y = self._bound(self.camera_y, LaserCamBox.SERVO_MIN, LaserCamBox.SERVO_MAX) def update_pwm(self, ): """Send current PWM values to PWM device.""" self.check_servo_values() self.PWM.set_pwm(LaserCamBox.LASER_X_CHAN, 0, self.laser_x) self.PWM.set_pwm(LaserCamBox.LASER_Y_CHAN, 0, self.laser_y) self.PWM.set_pwm(LaserCamBox.CAMERA_X_CHAN, 0, self.camera_x) self.PWM.set_pwm(LaserCamBox.CAMERA_Y_CHAN, 0, self.camera_y) def enable_pwm(self, update=False): """Enable power to the servos.""" GPIO.output(LaserCamBox.OE_PIN, GPIO.LOW) if update: self.update_pwm() def disable_pwm(self, ): """Disable power to the servos.""" GPIO.output(LaserCamBox.OE_PIN, GPIO.HIGH) def get_pwm_state(self, ): """Return 0 if servos are enabled or 1 if disabled.""" return GPIO.input(LaserCamBox.OE_PIN) def is_pwm_enabled(self, ): """Return True if servos are enabled, otherwise False.""" state = GPIO.input(LaserCamBox.OE_PIN) if state == 0: return True elif state == 1: return False else: return None def camera_up(self, step=None): """Move the camera up the specified number of steps.""" if step == None: step = self.camera_step self.camera_y -= step self.update_pwm() def camera_down(self, step=None): """Move the camera down the specified number of steps.""" if step == None: step = self.camera_step self.camera_y += step self.update_pwm() def camera_left(self, step=None): """Move the camera left the specified number of steps.""" if step == None: step = self.camera_step self.camera_x -= step self.update_pwm() def camera_right(self, step=None): """Move the camera right the specified number of steps.""" if step == None: step = self.camera_step self.camera_x += step self.update_pwm() def camera_move_relative(self, amount): """Move the camera the amount specified in the tuple.""" self.camera_x += amount[0] self.camera_y += amount[1] self.update_pwm() def camera_home(self, ): """Move the camera to the home positon.""" self.camera_set_position(LaserCamBox.CAMERA_HOME) def camera_set_position(self, position): """Move the camera to the absolute position.""" if not isinstance(position, tuple): return if not len(position) == 2: return self.camera_x = position[0] self.camera_y = position[1] self.update_pwm() def mjpegstream_start(self, port=8081, resize=(640, 360)): """Start thread to serve MJPEG stream on specified port.""" if not self._mjpegger == None: return camera = picamera.PiCamera(sensor_mode=5) camera.hflip = True camera.vflip = True kwargs = {'camera': camera, 'port': port, 'resize': resize} self._mjpegger = mjpegger.MJPEGThread(kwargs=kwargs) self._mjpegger.start() while not self._mjpegger.streamRunning: pass def mjpegstream_stop(self, ): """Stop the MJPEG stream, if running.""" if not self._mjpegger == None: if self._mjpegger.is_alive(): self._mjpegger.stop() self._mjpegger = None def mjpgstream_is_alive(self, ): """Return True if stream is running, False otherwise.""" if self._mjpegger == None: return False else: return self._mjpegger.is_alive() def camera_get_position(self, ): """Return the current camera position.""" return (self.camera_x, self.camera_y) def laser_up(self, step=None): """Move the laser up the specified number of steps.""" if step == None: step = self.laser_step self.laser_y += step self.update_pwm() def laser_down(self, step=None): """Move the laser down the specified number of steps.""" if step == None: step = self.laser_step self.laser_y -= step self.update_pwm() def laser_left(self, step=None): """Move the laser left the specified number of steps.""" if step == None: step = self.laser_step self.laser_x -= step self.update_pwm() def laser_right(self, step=None): """Move the laser right the specified number of steps.""" if step == None: step = self.laser_step self.laser_x += step self.update_pwm() def laser_home(self, ): """Move the laser to the home positon.""" self.laser_set_position(LaserCamBox.LASER_HOME) def laser_set_position(self, position): """Move the laser to the absolute position.""" if not isinstance(position, tuple): return if not len(position) == 2: return self.laser_x = position[0] self.laser_y = position[1] self.update_pwm() def laser_get_position(self, ): """Return the current laser position.""" return (self.laser_x, self.laser_y) def laser_on(self, ): """Turn the laser on.""" GPIO.output(LaserCamBox.LASER_PIN, GPIO.HIGH) def laser_off(self, ): """Turn the laser off.""" GPIO.output(LaserCamBox.LASER_PIN, GPIO.LOW) def get_laser_state(self, ): """Return current laser state.""" return GPIO.input(LaserCamBox.LASER_PIN) def camera_led_on(self, ): """Turn camera LED on.""" GPIO.output(LaserCamBox.CAM_LED_PIN, GPIO.HIGH) def camera_led_off(self, ): """Turn camera LED off.""" GPIO.output(LaserCamBox.CAM_LED_PIN, GPIO.LOW) def get_camera_led_state(self, ): """Return current LED state.""" return GPIO.input(LaserCamBox.CAM_LED_PIN) def status_led_on(self, led): """Turn on the specified front panel status LED.""" if led == 1: GPIO.output(LaserCamBox.LED1_PIN, GPIO.HIGH) elif led == 2: GPIO.output(LaserCamBox.LED2_PIN, GPIO.HIGH) elif led == 3: GPIO.output(LaserCamBox.LED3_PIN, GPIO.HIGH) else: pass def status_led_off(self, led): """Turn off the specified front panel status LED.""" if led == 1: GPIO.output(LaserCamBox.LED1_PIN, GPIO.LOW) elif led == 2: GPIO.output(LaserCamBox.LED2_PIN, GPIO.LOW) elif led == 3: GPIO.output(LaserCamBox.LED3_PIN, GPIO.LOW) else: pass def status_led_all_off(self, ): """Turn off all of the front panel status LEDs.""" self.statusLEDOff(1) self.statusLEDOff(2) self.statusLEDOff(3) def enable_audio(self, ): """Enable the audio amplifier.""" GPIO.output(LaserCamBox.AMP_PIN, GPIO.HIGH) def disable_audio(self, ): """Disable the audio amplifier.""" GPIO.output(LaserCamBox.AMP_PIN, GPIO.LOW) def speak(self, msg="hello world"): """Output the supplied message on the speaker.""" self.enable_audio() opt = '-p 45 -s 165' # espeak options os.system('espeak ' + opt + ' "%s"' % msg) self.disable_audio()
# This simple test outputs a 50% duty cycle PWM single on the 0th channel. Connect an LED and # resistor in series to the pin to visualize duty cycle changes and its impact on brightness. from board import SCL, SDA import busio import time # Import the PCA9685 module. from Adafruit_PCA9685 import PCA9685 # Create the I2C bus interface. i2c_bus = busio.I2C(SCL, SDA) c = int(input("channel : ")) # Create a simple PCA9685 class instance. pca = PCA9685() # Set the PWM frequency to 60hz. freq = 50 pca.set_pwm_freq(freq) # Set the PWM duty cycle for channel zero to 50%. duty_cycle is 16 bits to match other PWM objects # but the PCA9685 will only actually give 12 bits of re50solution. left_range = int(input("min range:")) right_range = int(input("max range:")) for i in range(right_range, left_range, -5): #while True: #a = int(input("duty : ")) # print(i) pca.set_pwm(c, 0, i) time.sleep(1)
def __init__(self, radius=0, centre=0, vertical_channel=0, horizontal_channel=1): """ The constructor takes a horizontal and vertical channel as input, which correspond to which channel each servomotor is connected to on the Raspberry PI PWM Hat. The 'radius' and 'centre' arguments are for the simulation model, which is currently not included in this codebase. """ # Set up file lock. self.lock = wgloballock.WFileLock("my_lock.txt", dir="/home/pi/temp") # PWM Setup Variables self.lock.acquire() self.eye = PCA9685( 0x40 ) # Always use the default i2c address for the PWM hats. This call turns off any pwm signal currently being sent. self.vert_ch = vertical_channel self.horiz_ch = horizontal_channel self.pwm_freq = 50.0 # Fixed by servos used. self.eye.set_pwm_freq(int(self.pwm_freq)) self.lock.release() self.pwm_bitres = 4096.0 self.pwm_scaler = self.pwm_bitres / (1 / self.pwm_freq) # The PWM periods for a typical servomotor is between 1ms and 2ms, with 1.5ms being the middle of the useable range. self.pwm_period_min = 10e-4 * self.pwm_scaler # This is the minimum angle (1ms) These will need to be calibrated. self.pwm_period_zero = 15e-4 * self.pwm_scaler # This angle is the middle of the servo range (1.5ms) self.pwm_period_max = 20e-4 * self.pwm_scaler # This is the maximum angle (2ms) self.pwm_period_range = self.pwm_period_max - self.pwm_period_min self.servo_angle_max = 90 # This is the range of the unrestricted servo movement- use max as servos have been calibrated to # the max physical range. self.servo_angle_min = -90 # This is the range of the unrestricted servo movement- use max as servos have been calibrated to # the max physical range. self.servo_angle_range = self.servo_angle_max - self.servo_angle_min self.eye_movement_max_radius = 90 self.eye_movement_corner_angle = math.sqrt( (self.eye_movement_max_radius**2) / 2) # Servo Position Tracking Variables (degrees) self.eye_vert_angle = 0 self.eye_horiz_angle = 0 # Note that unless move_to(0, 0) is called the initial position will # be wrong. This is called in the controller as calling PCA9685(0x40) # turns off any pwm signal being sent so this must be done after the # eyes have each been constructed. # Eye Simulation self.radius = radius self.centre = centre self.use_mapping = True # Mapping up front calibration computations. # calibrated values for final eyes. 23/05/18 self.extreme_up_vert = self.eye_movement_max_radius #Calibrate self.extreme_up_horiz = 15 #Calibrate self.extreme_down_vert = -self.eye_movement_max_radius #Calibrate self.extreme_down_horiz = 45 #Calibrate self.extreme_left_vert = -15 #Calibrate self.extreme_left_horiz = self.eye_movement_max_radius #Calibrate self.extreme_right_vert = 0 #Calibrate self.extreme_right_horiz = -self.eye_movement_max_radius #Calibrate self.zero_position_vert = 0 #Calibrate self.zero_position_horiz = 25 #Calibrate
# copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. from ledtheatre import init, Sequence try: # Initialise from Adafruit_PCA9685 import PCA9685 init(PCA9685(), pull_up=True) except ImportError: # This is not important - ledtheatre will issue a warning later # We will fall back to simulating the PCA9685 pass # A list of all the LED#s used in this example MAX_LEDS = 8 ALL = range(0, MAX_LEDS) # Turn off all LEDs Sequence().led(ALL, 0).execute() # A sample Sequence fade = Sequence() \ .transition(0.5).led(0, 1) \
from Adafruit_PCA9685 import PCA9685 import time servo = PCA9685(0x40) servo.set_pwm_freq(60) servo_min = 125 servo_max = 625 servo_initial = 375 servo_current = servo_initial while True: for i in range(125, 626): servo.set_pwm(0, 0, i) time.sleep(0.010) for i in range(624, 124, -1): servo.set_pwm(0, 0, i) time.sleep(0.010)
def __init__(self, mqtt_client): self._pwm = PCA9685(0x41) self._pwm.set_pwm_freq(60) self.cancel_event = Event() self._lock = RLock() self.mqtt_client = mqtt_client
class AbstractMoteur: """ La classe AbstractMoteur représente un tout moteur que l'on cherche à contrôler par PWM et branché sur la carte PCA9685. Attributes ---------- canal : int Canal du PWM sur la PCA9685 rapport_cyclique_min : int Nombre de division minimale sur une résolution de 4096 rapport_cyclique_max : int Nombre de division maximale sur une résolution de 4096 charge : int Charge en pourcentage compris entre 0 et 100 pwm : PCA9685 Carte PCA9685 verrou : Lock Verrou permettant l'exclusivité d'un thread sur la PCA9685 """ verrou = Lock() pwm = PCA9685() def __init__(self, rapport_cyclique_min, rapport_cyclique_max, canal): self._rapport_cyclique_min = rapport_cyclique_min self._rapport_cyclique_max = rapport_cyclique_max self._canal = canal self._charge = None @property def charge(self): """ Getter pour la dernière charge utilisée """ return self._charge @charge.setter def charge(self, pourcentage): """ Définit une nouvelle charge pour le moteur. Parameters ---------- pourcentage : int Nouvelle charge compris entre 0 et 100 Returns ------ void Rien. """ if pourcentage > 100: pourcentage = 100 elif pourcentage < 0: pourcentage = 0 else: pourcentage = int(pourcentage) if self._charge is not pourcentage: self._charge = pourcentage etendue = self._rapport_cyclique_max - self._rapport_cyclique_min rapport_cyclique = int(self._rapport_cyclique_min + etendue * pourcentage / 100) AbstractMoteur.Launcher(rapport_cyclique, self._canal).start() class Launcher(Thread): """ La clase Launcher sert exclusivement à lancer des actions sur la carte PWM9685. Attributes ---------- _attente : int Attente en seconde avant de lancer _rapport : int Nouveau rapport compris entre 0 et 4096 _canal : int Canal sur lequel définir le rapport """ def __init__(self, rapport, canal, attente=0): super().__init__() self._attente = attente self._rapport = rapport self._canal = canal def run(self): """ Méthode héritée de Thread """ if self._attente > 0: sleep(self._attente) AbstractMoteur.verrou.acquire() if conf("debug.controle"): print("@", self._canal, "\tset_pwm(", self._canal, ", ", 0, ", ", self._rapport, ")") AbstractMoteur.pwm.set_pwm(self._canal, 0, self._rapport) AbstractMoteur.verrou.release()
def __init__(self): # Set the GPIO numbering mode io.setmode(GPIO_MODE) # Connect to servo and motor pca9685 boards servo_pca9685 = PCA9685(SERVO_I2C_ADDRESS) motor_pca9685 = PCA9685(MOTOR_I2C_ADDRESS) # Set pca9685 board frequencies servo_pca9685.set_pwm_freq(SERVO_PWM_FREQ) motor_pca9685.set_pwm_freq(MOTOR_PWM_FREQ) motors = [ # Motors MotorComponent(pca9685=motor_pca9685, pca9685_channel=0, dir_pin=33), # Front Right MotorComponent(pca9685=motor_pca9685, pca9685_channel=1, dir_pin=29, reverse=True), # Front Left MotorComponent(pca9685=motor_pca9685, pca9685_channel=2, dir_pin=35), # Back Right MotorComponent(pca9685=motor_pca9685, pca9685_channel=3, dir_pin=31, reverse=True), # Back Left ] self.output_components = [ # Servos ServoComponent(pca9685=servo_pca9685, pca9685_channel=SERVO_0_CHANNEL, modifier=SERVO_0_MODIFIER, max_pwm=SERVO_0_MAX_PWM, min_pwm=SERVO_0_MIN_PWM, presets=SERVO_0_PRESETS, control_speed=SERVO_0_CONTROL_SPEED, servo_speed=SERVO_0_SPEED, reverse=False), ServoComponent(pca9685=servo_pca9685, pca9685_channel=SERVO_1_CHANNEL, modifier=SERVO_1_MODIFIER, max_pwm=SERVO_1_MAX_PWM, min_pwm=SERVO_1_MIN_PWM, presets=SERVO_1_PRESETS, control_speed=SERVO_1_CONTROL_SPEED, servo_speed=SERVO_1_SPEED, reverse=False), ServoComponent(pca9685=servo_pca9685, pca9685_channel=SERVO_2_CHANNEL, modifier=SERVO_2_MODIFIER, max_pwm=SERVO_2_MAX_PWM, min_pwm=SERVO_2_MIN_PWM, presets=SERVO_2_PRESETS, control_speed=SERVO_2_CONTROL_SPEED, servo_speed=SERVO_2_SPEED, reverse=False), # Motors MotorController(fwd_axis=MOTOR_FORWARD_AXIS, back_axis=MOTOR_BACKWARD_AXIS, steer_axis=MOTOR_STEER_AXIS, steer_speed=MOTOR_STEER_SPEED, motors=motors, min_pwm=MOTOR_MIN_PWM, reverse=True), # LED LEDComponent(pca9685=servo_pca9685, pca9685_channel=LED_CHANNEL, button=LED_BUTTON, value=LED_VALUE), ] # check output components for output in self.output_components: assert isinstance(output, OutputComponent) # Commented out sensors for debug, we removed the actual sensors / pcb from the bot self.input_components = [ # SensorComponent(SENSOR_0_NAME, SENSOR_0_CHANNEL, SENSOR_0_COEFFICIENTS), # SensorComponent(SENSOR_1_NAME, SENSOR_1_CHANNEL, SENSOR_1_COEFFICIENTS), # SensorComponent(SENSOR_2_NAME, SENSOR_2_CHANNEL, SENSOR_2_COEFFICIENTS), # SensorComponent(SENSOR_3_NAME, SENSOR_3_CHANNEL, SENSOR_3_COEFFICIENTS), # SensorComponent(SENSOR_4_NAME, SENSOR_4_CHANNEL, SENSOR_4_COEFFICIENTS), # SensorComponent(SENSOR_5_NAME, SENSOR_5_CHANNEL, SENSOR_5_COEFFICIENTS), # SensorComponent(SENSOR_6_NAME, SENSOR_6_CHANNEL, SENSOR_6_COEFFICIENTS), # SensorComponent(SENSOR_7_NAME, SENSOR_7_CHANNEL, SENSOR_7_COEFFICIENTS), ] # check input components for input_ in self.input_components: assert isinstance(input_, InputComponent)
maxs = 400 neutral = int((maxs + mins) / 2) # maxs = 600 # mins = 100 def angleToPWM(angle, min, max): servo_min = mins # Min pulse length out of 4096 servo_max = maxs # Max pulse length out of 4096 m = (servo_max - servo_min) / (max - min) b = servo_max - m * max pulse = m * angle + b # y=mx+b return int(pulse) pwm = PCA9685() # pwm = Device(0x40) print('Turn everything off') pwm.set_all_pwm(0, 0x1010) pwm.set_pwm_freq(60) # not sure why ... should be 50 Hz # pwm.set_pwm_frequency(60) # print('change all to {}'.format(neutral)) # for i in range(0,16): # # pwm.set_pwm(i, neutral) # pwm.set_pwm(i, 0, neutral) # time.sleep(1) #