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 __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 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)
#
