def play_missile_defense(self):
left_pro, _ = pro_controller_factory()
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)
m = MissileDefense(375, 80, pwm, left_pro, TURRET_2, TURRET_3, TURRET_4)
m.play_on()
import os.path
import time
from smbus import SMBus
import Adafruit_PCA9685
import subprocess
import math
from math import pi
import psutil
import quadprog
import time
#from .quadrotor_qp.quadprog_solve import qp_q_dot_des
#import os
from numpy import array
# Global Variables
pwm1 = Adafruit_PCA9685.PCA9685(address=0x40, busnum=1)
pwm2 = Adafruit_PCA9685.PCA9685(address=0x41, busnum=1)
pwm1.set_pwm_freq(50)
pwm2.set_pwm_freq(50)
bus = SMBus(1)
p = psutil.Process(os.getpid())
p.nice(-19)
p.cpu_affinity([2, 3])
if __name__ == '__main__':
SETTINGS_FILE = "RTIMULib"
print("Using settings file " + SETTINGS_FILE + ".ini")
from __future__ import division import time import RPi.GPIO as GPIO import sys import Adafruit_PCA9685 import ultra ''' change this form 1 to 0 to reverse servos ''' pwm0_direction = 1 pwm1_direction = 0 pwm2_direction = 1 pwm3_direction = 1 pwm4_direction = 0 pwm = Adafruit_PCA9685.PCA9685() pwm.set_pwm_freq(50) pwm0_init = 300 pwm0_max = 450 pwm0_min = 150 pwm0_pos = pwm0_init pwm1_init = 300 pwm1_max = 480 pwm1_min = 160 pwm1_pos = pwm1_init pwm2_init = 300 pwm2_max = 500 pwm2_min = 100
import time
import sys
# Import the PCA9685 module.
import Adafruit_PCA9685 as PWM
#set PWM Controller channels
PWM_CH_SERVO = 15
# Uncomment to enable debug output.
#import logging
#logging.basicConfig(level=logging.DEBUG)
# Initialise the PCA9685 at the appropriate address and
# bus (0x40 and 2, respectively)
pwm = PWM.PCA9685(address=0x40, busnum=1)
# Configure min and max servo pulse lengths
servo_min = 150 # Min pulse length out of 4096
servo_mid = 353
servo_max = 580 # Max pulse length out of 4096 (600 torqued one servo too much)
# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)
try:
# Move servos to the value the user supplied
for arg in sys.argv[1:]:
val = int(arg)
print('Moving servo to %d\n' % val)
def __init__(self, freq=60):
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(freq)
create a virtual environment (venv) and use the pip install -r requirements.txt
to install the dependencies
"""
import time
import logging
import Adafruit_PCA9685
# from constants import Channel
from smars_library.constants import Channel
logging.basicConfig(level=logging.CRITICAL)
logging.propagate = False
# Initialise the PCA9685 using the default address (0x40).
try:
PWM = Adafruit_PCA9685.PCA9685()
except OSError as error:
LOG_STRING = "failed to initialise the servo driver (Adafruit PCA9685): "
logging.error(LOG_STRING)
# tell later parts of the code not to actually use the driver
do_no_use_PCA_driver = True
PWM = ""
except:
print(
"There was an error loading the adafruit driver; loading without PCA9685."
)
do_no_use_PCA_driver = True # tell later parts of the code not to actually use the driver
else:
LOG_STRING = "PCA9685 Driver loaded)."
#!/usr/bin/env python
import cv2
import numpy as np
import rospy
#Import bridge to convert open cv frames into ros frames
from cv_bridge import CvBridge
import Adafruit_PCA9685
from sensor_msgs.msg import Image
pwm = Adafruit_PCA9685.PCA9685(address=0x40, busnum=4)
cap = cv2.VideoCapture(0)
# Set camera resolution
cap.set(3, 480)
cap.set(4, 320)
ret, img = cap.read()
rows, cols, _ = img.shape
x_medium = int(cols / 2)
center = int(cols / 2)
position = 200 # degrees
pwm.set_pwm_freq(45)
face_cascade = cv2.CascadeClassifier(
'/home/ubuntu/opencv-3.4.4/data/haarcascades_cuda/haarcascade_frontalface_default.xml'
)
while 1:
#Initializing publisher
pub = rospy.Publisher("frames", Image, queue_size=10)
rospy.init_node('stream_publisher', anonymous=True)
rate = rospy.Rate(10)
def signal_handler(signal, frame):
print '\nCaught interrupt, cleaning up...'
for process in processes:
process.terminate()
for channel in range(10):
print 'setting channel %s to min' % channel
pwm.set_pwm(channel, 0, servo_max)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685(busnum=2)
# Configure min and max servo pulse lengths
servo_min = 200 # Min pulse length out of 4096
servo_max = 440 # Max pulse length out of 4096
channels = range(10)
processes = []
pwm.set_pwm_freq(50)
def servo_process(channel):
signal.signal(signal.SIGINT, signal.SIG_IGN)
while 1:
sleep_time = random.uniform(0.5, 1)
# This will move channel 0 from min to max position repeatedly.
# Author: Tony DiCola
# License: Public Domain
from __future__ import division
import time
# Import the PCA9685 module.
import Adafruit_PCA9685
# Uncomment to enable debug output.
#import logging
#logging.basicConfig(level=logging.DEBUG)
# Initialise the PCA9685 using the default address (0x40).
pwm1 = Adafruit_PCA9685.PCA9685(address=0x40)
pwm2 = Adafruit_PCA9685.PCA9685(address=0x41)
# Alternatively specify a different address and/or bus:
#pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
# Configure min and max servo pulse lengths
servo_min = 150 # Min pulse length out of 4096
servo_max = 600 # Max pulse length out of 4096
# Helper function to make setting a servo pulse width simpler.
def set_servo_pulse(channel, pulse):
pulse_length = 1000000 # 1,000,000 us per second
pulse_length //= 60 # 60 Hz
print('{0}us per period'.format(pulse_length))
pulse_length //= 4096 # 12 bits of resolution
print('{0}us per bit'.format(pulse_length))
import json
import numpy as np
import time
import Adafruit_PCA9685
import math
import smbus
import struct
from BNO055 import *
from subprocess import Popen, PIPE
pwn = Adafruit_PCA9685.PCA9685()
tibia_min = 275 #325 according to json file
tibia_max = 400 #425
chassis_min = 255 #275
chassis_max = 350 #475
#the file path for the gait generator file. Uncomment the one you want to use
#gaitGenerator = './splinegen.py'
#gaitGenerator = './Spyndra_newCode/standinggait.py'
gaitGenerator = './standingGait.py'
pwn.set_pwm_freq(60) #Sets frequency to 60 Hz
#Pulls Min and Max values for femurs and tibias from the json file
def pullMotorVal():
json_data = open(
'/home/pi/Desktop/SpyndraSpy/project/Spyndra_Control/Spyndra_newCode/servo_settings.json'
).read()
parsed_json = json.loads(json_data)
#!/usr/bin/env python3
import Adafruit_PCA9685
pwms = [Adafruit_PCA9685.PCA9685(0x40), Adafruit_PCA9685.PCA9685(0x41)]
for pwm in pwms:
for i in range(16):
pwm.set_pwm(i, 0, 0)
import time
import Adafruit_PCA9685
T = 0.2
Zero = 405
Point = {"PWM":Zero,"Thrust":0}
# ======= Adafruit Hat ===========
hat = Adafruit_PCA9685.PCA9685()
hat.set_pwm_freq(50)
channel = 0
hat.set_pwm(channel, 0, Zero)
# ====== Thrust Measurement =======
def update_thrust():
thrust = 1
return thrust
file = open("Motor", "w")
while Point["PWM"] <= 500 and Point["PWM"] >= 300 :
Point["PWM"] = Point["PWM"] +1
hat.set_pwm(channel , 0 , Point["PWM"] )
# Wait for Motor Response
time.sleep(T)
Point["Thrust"] = update_thrust()
data = str(Point["PWM"]) + ' ' + str(Point["Thrust"])
# initialize Dualshock interface (in pygame mode or throught node mode)
# initialize Navigation interface
from time import sleep
import Adafruit_PCA9685
from src.dualshock.DualshockThroughtPygame import DualshockThroughtPygame
from src.dualshock.DualshockThroughtNode import DualshockThroughtNode
from src.Navigation import Navigation
from src.ControlDispatcher import ControlDispatcher
servoInterface = Adafruit_PCA9685.PCA9685()
servoInterface.set_pwm_freq(50)
# servoInterface = None
#dualshock = DualshockThroughtNode()
#dualshock = DualshockThroughtPygame()
navigation = Navigation(servoInterface)
def main():
#controlDispatcher = ControlDispatcher(dualshock, navigation)
navigation.frontLeft()
while True:
sleep(0)
#dualshock.start()
'''
def mappyt(x, inMin, inMax, outMin, outMax):
def __init__(self, Channel):
self.Channel = Channel
#initialize PCA9685
self.PWM = ap.PCA9685(address=0x40)
self.PWM.set_pwm_freq(60) # 60Hz
def config_pwm(hz):
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(hz)
return pwm
def main():
'''
Hacky version of our contigency plan
'''
#------Variable Declarations------
# Tachometer Specific Data
rightTachValue = 0
leftTachValue = 0
prevRightTachValue = 0
prevLeftTachValue = 0
rightVelocity = 0
rightStripCount = 0
leftVelocity = 0
leftStripCount = 0
loopCount = 0
leftHigh = 0
leftThresholdHigh = 8500
leftThresholdLow = 6500
rightHigh = 0
rightThresholdHigh = 10000
rightThresholdLow = 8000
steeringPotValue = 0
distTraveled = 0.0
distStartTime = 0.0
distEndTime = 0.0
# Timer
startTime = 0
elapsedTime = 0
fractional = 0
# Steering / Turning Setup
turnGoal = STRAIGHT
steeringAvg = 0.0
steeringPID = PID(1.0, 0.2, 0.0, 3.0)
steeringFilter = Filter(0.9)
steeringPID.setGoal(turnGoal)
# Velocity Setup
velDuration = STOP
avgVelocity = 0.0
velGoal = 0.5
velocityPID = PID(50.0, 100.0, 0.0, 3.0)
velocityFilter = Filter(0.9)
velocityPID.setGoal(velGoal)
# Distance sensors
leftDistSensor = DistanceSensor(echo=DIST_LEFT_ECHO_PIN,
trigger=DIST_LEFT_TRIGGER_PIN,
max_distance=DIST_MAX_DISTANCE,
queue_len=DIST_QUEUE_LENGTH)
rightDistSensor = DistanceSensor(echo=DIST_RIGHT_ECHO_PIN,
trigger=DIST_RIGHT_TRIGGER_PIN,
max_distance=DIST_MAX_DISTANCE,
queue_len=DIST_QUEUE_LENGTH)
distFilter = Filter(0.8)
leftDist = 0.0
rightDist = 0.0
# Line Following Setup
wallFollowPID = PID(10000.0, 0.0, 0.0, 0.8)
wallFollowPID.setGoal(MAX_WALL_DIST)
#---------------------------------
# Setup bus
pwm = Adafruit_PCA9685.PCA9685()
adc = Adafruit_ADS1x15.ADS1115()
try:
pwm.set_pwm_freq(FREQ)
print("Start")
distStartTime = time.time()
while True:
if (loopCount % 1 == 0):
prevRightTachValue = rightTachValue
prevLeftTachValue = leftTachValue
steeringPotValue = adc.read_adc(POT_CHNL,
gain=GAIN,
data_rate=DATA_RATE)
rightTachValue = adc.read_adc(RIGHT_WHEEL_CHNL,
gain=GAIN,
data_rate=DATA_RATE)
leftTachValue = adc.read_adc(LEFT_WHEEL_CHNL,
gain=GAIN,
data_rate=DATA_RATE)
distTraveled += (avgVelocity / (time.time() - distStartTime))
#distFilter.recvMeasurement(rightDistSensor.distance)
#rightDist = distFilter.filter()
#distFilter.recvMeasurement(leftDistSensor.distance)
#leftDist = distFilter.filter()
rightDist = rightDistSensor.distance
leftDist = leftDistSensor.distance
#print('Left Distance: ', leftDistSensor.distance * 100)
#print('Right Distance: ', rightDistSensor.distance * 100)
# Wall follow PID
wallFollowPID.setCurrentMeasurement(rightDist)
turnGoal = (wallFollowPID.control() + STRAIGHT)
steeringFilter.recvMeasurement(steeringPotValue)
steeringPotValue = steeringFilter.filter()
steeringPID.setCurrentMeasurement(steeringPotValue / 1000.0)
steeringPID.setGoal(turnGoal / 1000.0)
if (rightHigh == 0 and rightTachValue > rightThresholdHigh):
rightStripCount += 0.5
rightHigh = 1
if (rightHigh == 1 and rightTachValue < rightThresholdHigh):
rightStripCount += 0.5
rightHigh = 0
if (leftHigh == 0 and leftTachValue > leftThresholdHigh):
leftStripCount += 0.5
leftHigh = 1
if (leftHigh == 1 and leftTachValue < leftThresholdHigh):
leftStripCount += 0.5
leftHigh = 0
loopCount += 1
# Steering
steeringDuration = -steeringPID.control() + 0.5
'''
if steeringDuration > 0:
#steeringDuration = 670
# Dead Band
steeringDuration = 675 - steeringDuration
elif steeringDuration < 0:
#steeringDuration = 890
steeringDuration = 875 - steeringDuration
# Max Zone
if steeringDuration > 930:
steeringDuration = 930
if steeringDuration < 630:
steeringDuration = 630
'''
controlChnl(pwm, MOTOR_CHNL, velDuration)
#controlChnl(pwm, MOTOR_CHNL, 0.75)
#print("SD: {0}".format(steeringDuration))
controlChnl(pwm, TURN_CHNL, steeringDuration)
#controlChnl(pwm, TURN_CHNL, 0.99)
#controlChnl(pwm, TURN_CHNL, 930)
#controlChnl(pwm, TURN_CHNL, 0.0)
#controlChnl(pwm, TURN_CHNL, 780)
#controlChnl(pwm, TURN_CHNL, velDuration)
#controlChnl(pwm, TURN_CHNL, s)
#print("VD: {0}".format(int(velDuration)))
#print("SD: {0}".format(int(steeringDuration)))
if (loopCount >= MAX_LOOP_COUNT):
elapsedTime = (time.time() * 1000) - startTime
startTime = (time.time() * 1000)
rightVelocity = ((rightStripCount / 30.0) * 0.36 *
math.pi) / (elapsedTime / 1000.0)
leftVelocity = ((leftStripCount / 30.0) * 0.36 *
math.pi) / (elapsedTime / 1000.0)
# Velocity
avgVelocity = (leftVelocity + rightVelocity) / 2.0
if velDuration > STOP:
avgVelocity *= -1
velocityFilter.recvMeasurement(avgVelocity)
avgVelocity = velocityFilter.filter()
velocityPID.setCurrentMeasurement(avgVelocity)
velocityPID.setGoal(velGoal)
velDuration = STOP - velocityPID.control()
#if velDuration < 550:
# velDuration = 550
#if velDuration > 1000:
# velDuration = 1000
print('Total Distance: ', distTraveled)
print('Left Distance: ', leftDistSensor.distance * 100)
print('Right Distance: ', rightDistSensor.distance * 100)
print("LDist: {0}".format(leftDist))
print("RDist: {0}".format(rightDist))
print("LDist: {0}".format(leftDistSensor.distance * 100))
print("RDist: {0}".format(rightDistSensor.distance * 100))
print("LV: {0}".format(leftVelocity))
print("RV: {0}".format(rightVelocity))
print("Avg Vel: {0}".format(avgVelocity))
print("S: {0}".format(steeringPotValue))
print("ET: {0}".format(elapsedTime))
print("VD: {0}".format(velDuration))
print("VG: {0}".format(velGoal))
print("SD: {0}".format(steeringDuration))
print("SG: {0}".format(turnGoal))
print('Steering PID:')
print(steeringPID)
print('Velocity PID:')
print(velocityPID)
print('Wall Follow PID:')
print(wallFollowPID)
print('')
loopCount = 0
rightStripCount = 0
leftStripCount = 0
steeringAvg = 0.0
finally:
#ramp(pwm, MOTOR_CHNL, velDuration, STOP, 1, 5)
#ramp(pwm, TURN_CHNL, steeringDuration, STOP, 1, 5)
#controlChnl(pwm, MOTOR_CHNL, STOP)
#controlChnl(pwm, TURN_CHNL, STOP)
pass
from settings import wiringport
import os,sys, select, termios, tty
from suiron import SuironIO
from suiron import Clock
import json
import time
from cv_bridge import CvBridge, CvBridgeError
import Adafruit_PCA9685
from threading import Lock
#-----------------------------------------------------------------
REV = 2.3 # select your board
# If your board has a PWM module built in we will need the correct chip
if REV == 2.3:
# import Adafruit_PCA9685
# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685(0x40)
pwm.set_pwm_freq(97.1)
#---------------------------------------------------------------
#Set you max speeds forward(maxspeed) and backwards(minspeed)
maxspeed=0.17
minspeed=0.13
#-------------------------------------------------------------
#max_line=rospy.get_param('~max_line', 100)
#Our attempt to speed up the process
nos = os.system
#buttonWasPressed = False
with open('/home/ubuntu/settings.json') as d:
SETTINGS = json.load(d)
br=CvBridge()
count = 0
import Adafruit_PCA9685
#---------------------------------------------------------------
#Set your parameters max speeds forward(maxspeed) and backwards(minspeed) car freq and sensor address
maxspeed = 0.17
minspeed = 0.13
carFreq = 97.1 # CHECK YOUR BOARD AGAINST OSCILLOSCOPE AND MAKE SURE IT IS AS CLOSE TO 100Hz
address = 0x40 # default is 0x40
#-------------------------------------------------------------
#-----------------------------------------------------------------
REV = 2.3 # select your board
# If your board has a PWM module built in we will need the correct chip
if REV == 2.3:
# import Adafruit_PCA9685
# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685(address)
pwm.set_pwm_freq(carFreq) #CHECK THE PWM OF YOUR BOARD AND FINE TUNE THIS #
#-------------------------------------------------------------
##
#declare io
with open('/home/ubuntu/catkin_ws/settings.json') as d:
SETTINGS = json.load(d)
print(SETTINGS['width'], SETTINGS['height'])
suironio = SuironIO(id=0, width=SETTINGS['width'], height=SETTINGS['height'], depth=SETTINGS['depth'])
suironio.init_saving()
clck=Clock(suironio, 1000)
clck.start()
#clck.join()
os.remove('tmp')
if "FTDI" in result:
roomba_port = "/dev/ttyUSB0"
bno_port = "/dev/ttyUSB1"
else:
roomba_port = "/dev/ttyUSB1"
bno_port = "/dev/ttyUSB0"
# Connect to Roomba
robot = Roomba(roomba_port)
# Connect to Hedgehog
hh = Hedgehog("/dev/ttyACM0")
# Connect servos
servos = Adafruit_PCA9685.PCA9685(0x6f)
marker = 0 # channel 0
slide = 1 # channel 1
## Marker servo
marker_down = 1492 #1485 #1900
marker_up = 2250
## Slide servo
slide_left = 1130
slide_right = 1900
# Default position
set_servo_pulse(marker, marker_up)
set_servo_pulse(slide, slide_right)
# Connect to IMU
bno = BNO055.BNO055(serial_port=bno_port)
if not bno.begin():
def __init__(self):
self.driver = Adafruit_PCA9685.PCA9685()
self.driver.set_pwm_freq(60)
self.allStop()
""" All the shared variables are declared here and this file needs to be imported into any file which needs these variables just needs to import sharedVar """ # Import the PCA9685 module. import Adafruit_PCA9685 speed = 0 # speed of the motor (0-4096 representing the percentage of #frequecy that the pulse width is) ang = ((310.0 + 490) / 2) # angle of the serveo (310-490 pulse width size) runMode = 0 # (0) brake, (1) manual, (2) image recognition esc = 0 # set to 1 in order to esc all shared multitasking loops Color = -1 # stop light color indicator pwm = Adafruit_PCA9685.PCA9685() # Initiate pwm chip communication pwm.set_pwm_freq(50) # Set pwm frequency to 50 Hz so that it can control both #the servo and the motor
def setup(self):
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(self.frequency)
def __init__(self):
self.angleMin = 18
self.angleMax = 162
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(50) # Set the cycle frequency of PWM
import Adafruit_PCA9685
import time
pwm = Adafruit_PCA9685.PCA9685(address=0x60, busnum=1)
pwm.set_pwm_freq(60)
for i in range(0, 4097, 256):
print str(i)
pwm.set_pwm(7, i, 4096 - i)
pwm.set_pwm(6, 0, 4096)
pwm.set_pwm(5, 4096, 0)
time.sleep(1)
import speech import cv2 from collections import deque import numpy as np import argparse import imutils from rpi_ws281x import * import argparse import zmq import base64 import os import subprocess #time.sleep(4) #サーボモーター用 pwm = Adafruit_PCA9685.PCA9685() #Ultrasonic Control dis_dir = [] distance_stay = 0.4 distance_range = 2 led_status = 0 left_R = 15 left_G = 16 left_B = 18 right_R = 19 right_G = 21 right_B = 22 spd_ad = 1 #Speed Adjustment
def __init__(self, channel, frequency=60):
import Adafruit_PCA9685
# Initialise the PCA9685 using the default address (0x40).
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(frequency)
self.channel = channel
def __init__(self):
confs = utils.getConfiguration('PCA_PWM_frequency')
frequency = confs['value']
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(frequency)
print("ExecutePWM initialized")
from __future__ import division
import time
import Adafruit_PCA9685
pwm = Adafruit_PCA9685.PCA9685(
) #pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
def set_servo_pulse(channel, pulse):
pulse_length = 1000000
pulse_length //= 60
print('{0}us per period'.format(pulse_length))
pulse_length //= 4096
print('{0}us per bit'.format(pulse_length))
pulse *= 1000
pulse //= pulse_length
pwm.set_pwm(channel, 0, pulse)
pwm.set_pwm_freq(60)
class CON_B:
def initial_condition(self):
#초기 상태
pwm.set_pwm(12, 0, 210) #로봇팔 적재된 박스 위치로
pwm.set_pwm(15, 0, 180) #right(뒤로)
pwm.set_pwm(8, 0, 450) #left(위로)
pwm.set_pwm(9, 0, 350) #right(앞 뒤)
def clean_all():
global pwm
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50)
pwm.set_all_pwm(0, 0)
def play_pong(self):
c_1, c_2 = pro_controller_factory()
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)
p = Pong(375, 100, pwm, c_1, c_2, TURRET_2, TURRET_4, TURRET_3)
p.play_on()
