def start():
print("TRANSLATION SCRIPT")
kit = ServoKit(channels=16)
VELOCITY_MOD = .75
THRUSTER_FRONT_LEFT = kit._items[0] = servo.ContinuousServo(
kit._pca.channels[0])
THRUSTER_FRONT_LEFT_THRUST_VECTOR = Vector(SQRT2 / 2, SQRT2 / 2, 0)
THRUSTER_FRONT_RIGHT = kit._items[1] = servo.ContinuousServo(
kit._pca.channels[1])
THRUSTER_FRONT_RIGHT_THRUST_VECTOR = Vector(-SQRT2 / 2, SQRT2 / 2, 0)
THRUSTER_BACK_LEFT = kit._items[2] = servo.ContinuousServo(
kit._pca.channels[2])
THRUSTER_BACK_LEFT_THRUST_VECTOR = Vector(SQRT2 / 2, -SQRT2 / 2, 0)
THRUSTER_BACK_RIGHT = kit._items[3] = servo.ContinuousServo(
kit._pca.channels[3])
THRUSTER_BACK_RIGHT_THRUST_VECTOR = Vector(-SQRT2 / 2, -SQRT2 / 2, 0)
THRUSTER_FRONT_LEFT.set_pulse_width_range(1200, 2000)
THRUSTER_FRONT_RIGHT.set_pulse_width_range(1200, 2000)
THRUSTER_BACK_LEFT.set_pulse_width_range(1200, 2000)
THRUSTER_BACK_RIGHT.set_pulse_width_range(1200, 2000)
while True:
poll = Controller.getRightStick()
target = Vector(poll[0], poll[1], 0)
print(target.toString())
THRUSTER_FRONT_LEFT.throttle = THRUSTER_FRONT_LEFT_THRUST_VECTOR.dotProduct(
target) * VELOCITY_MOD
THRUSTER_FRONT_RIGHT.throttle = THRUSTER_FRONT_RIGHT_THRUST_VECTOR.dotProduct(
target) * VELOCITY_MOD
THRUSTER_BACK_LEFT.throttle = THRUSTER_BACK_LEFT_THRUST_VECTOR.dotProduct(
target) * VELOCITY_MOD
THRUSTER_BACK_RIGHT.throttle = THRUSTER_BACK_RIGHT_THRUST_VECTOR.dotProduct(
target) * VELOCITY_MOD
def start():
print("AZIMUTH ROTATION SCRIPT")
kit = ServoKit(channels=16)
VELOCITY_MOD = .5
TURN = False
THRUSTER_FRONT_LEFT = kit._items[0] = servo.ContinuousServo(
kit._pca.channels[0])
THRUSTER_FRONT_LEFT_THRUST_VECTOR = Vector(SQRT2 / 2, SQRT2 / 2, 0)
THRUSTER_FRONT_RIGHT = kit._items[1] = servo.ContinuousServo(
kit._pca.channels[1])
THRUSTER_FRONT_RIGHT_THRUST_VECTOR = Vector(-SQRT2 / 2, SQRT2 / 2, 0)
THRUSTER_BACK_LEFT = kit._items[2] = servo.ContinuousServo(
kit._pca.channels[2])
THRUSTER_BACK_LEFT_THRUST_VECTOR = Vector(SQRT2 / 2, -SQRT2 / 2, 0)
THRUSTER_BACK_RIGHT = kit._items[3] = servo.ContinuousServo(
kit._pca.channels[3])
THRUSTER_BACK_RIGHT_THRUST_VECTOR = Vector(-SQRT2 / 2, -SQRT2 / 2, 0)
THRUSTER_FRONT_LEFT.set_pulse_width_range(1200, 2000)
THRUSTER_FRONT_RIGHT.set_pulse_width_range(1200, 2000)
THRUSTER_BACK_LEFT.set_pulse_width_range(1200, 2000)
THRUSTER_BACK_RIGHT.set_pulse_width_range(1200, 2000)
while True:
rightStick = Controller.getRightStick()
target = Vector()
if (Controller.getButtonPresses == 1):
target = Vector(0, 0, 1)
elif (rightStick[2] == 1):
target = Vector(0, 0, 0)
if (TURN):
print(target.toString())
THRUSTER_FRONT_LEFT.throttle = -(
THRUSTER_FRONT_LEFT_THRUST_VECTOR.crossProduct(
target).getMagnitude() * VELOCITY_MOD)
THRUSTER_FRONT_RIGHT.throttle = (
THRUSTER_FRONT_RIGHT_THRUST_VECTOR.crossProduct(
target).getMagnitude() * VELOCITY_MOD)
THRUSTER_BACK_LEFT.throttle = (
THRUSTER_BACK_LEFT_THRUST_VECTOR.crossProduct(
target).getMagnitude() * VELOCITY_MOD)
THRUSTER_BACK_RIGHT.throttle = -(
THRUSTER_BACK_RIGHT_THRUST_VECTOR.crossProduct(
target).getMagnitude() * VELOCITY_MOD)
if (Controller.getButtonPresses().rs):
if (TURN):
TURN = False
else:
TURN = True
def __init__(self):
# init i2c
i2c = busio.I2C(SCL, SDA)
# init PCA
self.pca = PCA9685(i2c)
self.pca.frequency = 50
self.servo_steer = servo.Servo(self.pca.channels[0])
self.esc = servo.ContinuousServo(self.pca.channels[1])
# init model
model = neural_network.Net()
self.model = model.eval()
self.model.load_state_dict(torch.load('model/autopilot.pt'))
self.device = torch.device('cuda')
self.model.to(self.device)
# init vars
self.temp = 0
mean = 255.0 * np.array([0.485, 0.456, 0.406])
stdev = 255.0 * np.array([0.229, 0.224, 0.225])
self.normalize = torchvision.transforms.Normalize(mean, stdev)
self.angle_out = 0
# init Camera
self.cam = camera.Camera()
# initial content
with open('control_data.csv', 'w') as f:
f.write('date,steering,speed\n')
def __init__(self, driver, channel, *args, **kwargs):
super(Motor, self).__init__(*args, **kwargs) # initializes traitlets
self._driver = driver
self._motor = servo.ContinuousServo(self._driver.channels[channel])
# The pulse range is 750 - 2250 by default.
# If your servo doesn't stop once the script is finished you may need to tune the
# reference_clock_speed above or the min_pulse and max_pulse timings below.
# servo7 = servo.ContinuousServo(pca.channels[7], min_pulse=750, max_pulse=2250)
atexit.register(self._release)
def __init__(self, driver, max_switches, pca, config):
super().__init__(driver, "Dusty Bridge")
self.config = config
# Setup RF Transmitter
self.rf = RFDevice(self.config.rf.pin)
self.rf.enable_tx()
self.rf.tx_repeat = self.config.rf.tx_repeat
self.rf.tx_proto = self.config.rf.tx_proto
self.dust_collector_on = False
self.gate_to_close = None
self.active_switch = None
self.gate_close_counter = -1
self.servos = {}
for pin in range(max_switches):
self.servos[pin] = servo.ContinuousServo(pca.channels[pin], min_pulse=self.config.servo.min_pulse, max_pulse=self.config.servo.max_pulse)
self.add_accessory(DustySwitch(self, pin, driver, "Dusty Switch #"+str(pin)))
shutdown_switch = ShutdownSwitch(driver,"Halt")
self.add_accessory(shutdown_switch)
def continuous_servo():
# Continuous Servo Test Program for CircuitPython
# create a PWMOut object on Pin A2.
pwm = pulseio.PWMOut(board.A2, frequency=50)
# Create a servo object, my_servo.
my_servo = servo.ContinuousServo(pwm)
while True:
print("forward")
my_servo.throttle = 1.0
time.sleep(2.0)
print("stop")
my_servo.throttle = 0.0
time.sleep(2.0)
print("reverse")
my_servo.throttle = -1.0
time.sleep(2.0)
print("stop")
my_servo.throttle = 0.0
time.sleep(4.0)
def start(debug=False):
print("3 Axis Drive / Rotation Script Started")
kit = ServoKit(channels=16)
turn = False
zup = False
zdown = False
# creating all of the thrusters
# THRUSTER_FRONT_LEFT = kit._items[0] = servo.ContinuousServo(kit._pca.channels[0])
# THRUSTER_FRONT_RIGHT = kit._items[1] = servo.ContinuousServo(kit._pca.channels[1])
# THRUSTER_BACK_LEFT = kit._items[2] = servo.ContinuousServo(kit._pca.channels[2])
# THRUSTER_BACK_RIGHT = kit._items[3] = servo.ContinuousServo(kit._pca.channels[3])
THRUSTER_Z_0 = kit._items[4] = servo.ContinuousServo(kit._pca.channels[4])
THRUSTER_Z_1 = kit._items[5] = servo.ContinuousServo(kit._pca.channels[5])
THRUSTER_Z_2 = kit._items[6] = servo.ContinuousServo(kit._pca.channels[6])
THRUSTER_Z_3 = kit._items[7] = servo.ContinuousServo(kit._pca.channels[7])
# THRUSTER_FRONT_LEFT.set_pulse_width_range(1200,2000)
# THRUSTER_FRONT_RIGHT.set_pulse_width_range(1200,2000)
# THRUSTER_BACK_LEFT.set_pulse_width_range(1200,2000)
# THRUSTER_BACK_RIGHT.set_pulse_width_range(1200,2000)
THRUSTER_Z_0.set_pulse_width_range(1200,2000)
THRUSTER_Z_1.set_pulse_width_range(1200,2000)
THRUSTER_Z_2.set_pulse_width_range(1200,2000)
THRUSTER_Z_3.set_pulse_width_range(1200,2000)
while True:
# poll the controller
Controller.updateController()
presses = Controller.getButtonPresses()
RS = Controller.getRightStick()
targetTorque = Vector()
targetTranslation = Vector()
targetThrottles = [0 for i in range(8)]
if(presses.rs):
turn = not turn
# Translation
if(not turn):
# Set target x,y
targetTranslation = Vector(0, 0, RS[1])
# Ignore small values (without this we would get unwanted torques)
if(abs(targetTranslation.getX()) < .1):
targetTranslation.setX(0)
if(abs(targetTranslation.getY()) < .1):
targetTranslation.setY(0)
if(debug):
pass
# print(f"Translation Target = {targetTranslation}")
# TODO: consider changing this to slave thrusters together (kinda hard with this implementation)
# targetThrottles[0] = THRUSTER_FRONT_LEFT_THRUST_VECTOR.dotProduct(targetTranslation)
# targetThrottles[1] = THRUSTER_FRONT_RIGHT_THRUST_VECTOR.dotProduct(targetTranslation)
# targetThrottles[2] = THRUSTER_BACK_LEFT_THRUST_VECTOR.dotProduct(targetTranslation)
# targetThrottles[3] = THRUSTER_BACK_RIGHT_THRUST_VECTOR.dotProduct(targetTranslation)
targetThrottles[4] = THRUSTER_Z_0_THRUST_VECTOR.dotProduct(targetTranslation)
targetThrottles[5] = THRUSTER_Z_1_THRUST_VECTOR.dotProduct(targetTranslation)
targetThrottles[6] = THRUSTER_Z_2_THRUST_VECTOR.dotProduct(targetTranslation)
targetThrottles[7] = THRUSTER_Z_3_THRUST_VECTOR.dotProduct(targetTranslation)
# Turning
else:
# set X
targetTorque.setX(RS[0])
# set Y
targetTorque.setY(RS[1])
# set Z TODO: change this shitty implementation (create 3 axis joystick out of two joysticks???)
if(presses.dup):
zdown = False
zup = not zup
if(presses.ddown):
zup = False
zdown = not zdown
if(zup):
targetTorque.setZ(SQRT05)
if(zdown):
targetTorque.setZ(-SQRT05)
# Ignore small values (without this we would get unwanted torques)
if(abs(targetTorque.getX()) < .1):
targetTorque.setX(0)
if(abs(targetTorque.getY()) < .1):
targetTorque.setY(0)
if(debug):
print(f"Torque Target = {targetTorque}")
# Explanation incoming.....
# Each thruster has a specific thruster torque (torque created on COM if only that thruster was activated) defined as r cross F where F is their thrust vector
# In order to get the throttle, you must dot this torque vector with the target Torque vector to see how much their torque vector acts upon the target torque vector
# This should return maximum of 1 and minimum of -1 (float inaccuracies make it slightly different so we must check it is within [-1,1]
# targetThrottles[0] = (THRUSTER_FRONT_LEFT_TORQUE_VECTOR.dotProduct(targetTorque))
# targetThrottles[1] = (THRUSTER_FRONT_RIGHT_TORQUE_VECTOR.dotProduct(targetTorque))
# targetThrottles[2] = (THRUSTER_BACK_LEFT_TORQUE_VECTOR.dotProduct(targetTorque))
# targetThrottles[3] = (THRUSTER_BACK_RIGHT_TORQUE_VECTOR.dotProduct(targetTorque))
targetThrottles[4] = (THRUSTER_Z_0_TORQUE_VECTOR.dotProduct(targetTorque))
targetThrottles[5] = (THRUSTER_Z_1_TORQUE_VECTOR.dotProduct(targetTorque))
targetThrottles[6] = (THRUSTER_Z_2_TORQUE_VECTOR.dotProduct(targetTorque))
targetThrottles[7] = (THRUSTER_Z_3_TORQUE_VECTOR.dotProduct(targetTorque))
# Check for incorrect throttle values
for throttleValue in targetThrottles:
if(throttleValue > 1):
throttleValue = 1
elif(throttleValue < -1):
throttleValue = -1
throttleValue *= VELOCITY_MOD
# always write thrusters (defaults are 0)
for Thruster in range(4):
print(targetThrottles[Thruster+4])
if(kit._items[Thruster+4] is not None):
kit._items[Thruster+4].throttle = targetThrottles[Thruster+4]
# if(debug):
# # shouldnt really have to ever uncomment this one (these values shouldnt change once set)
# # print(f"""
# # Torque Vectors:
# # Front Left: {THRUSTER_FRONT_LEFT_TORQUE_VECTOR.toString()}
# # Front Right: {THRUSTER_FRONT_RIGHT_TORQUE_VECTOR.toString()}
# # Back Left: {THRUSTER_BACK_LEFT_TORQUE_VECTOR.toString()}
# # Back Right: {THRUSTER_BACK_RIGHT_TORQUE_VECTOR.toString()}
# # Z0: {THRUSTER_Z_0_TORQUE_VECTOR.toString()}
# # Z1: {THRUSTER_Z_1_TORQUE_VECTOR.toString()}
# # Z2: {THRUSTER_Z_2_TORQUE_VECTOR.toString()}
# # Z3: {THRUSTER_Z_3_TORQUE_VECTOR.toString()}
# # """)
# print(f"""
# Throttles:
# Front Left: {targetThrottles[0]}
# Front Right: {targetThrottles[1]}
# Back Left: {targetThrottles[2]}
# Back Right: {targetThrottles[3]}""")
# Z0: {targetThrottles[4]}
# Z1: {targetThrottles[5]}
# Z2: {targetThrottles[6]}
# Z3: {targetThrottles[7]}
# """)
from adafruit_motor import servo from analogio import AnalogIn import pulseio import board import digitalio from time import sleep pwm = pulseio.PWMOut(board.D7, frequency=50) contserv = servo.ContinuousServo(pwm) pot = AnalogIn(board.A0) photo = digitalio.DigitalInOut(board.D2) photo.direction = digitalio.Direction.INPUT photo.pull = digitalio.Pull.UP photo2 = digitalio.DigitalInOut(board.D3) photo2.direction = digitalio.Direction.INPUT photo2.pull = digitalio.Pull.UP photo_state = False last_state = False photo2_state = False last2_state = False led = digitalio.DigitalInOut(board.D8) led2 = digitalio.DigitalInOut(board.D9) led3 = digitalio.DigitalInOut(board.D10) led4 = digitalio.DigitalInOut(board.D11)
from board import SCL, SDA
import busio
# Import the PCA9684 module.
from adafruit_pca9684 import PCA9685
from adafruit_motor import servo
i1c = busio.I2C(SCL, SDA)
# create a simple PCA9684 class interface
pca = PCA9684(i2c)
# You can optionally provide a finer tuned reference clock speed to improve the accuracy of the
# timing pulses. This calibration will be specific to each board and its environment. See the
# calibration.py example in the PCA9684 driver.
# pca = PCA9684(i2c, reference_clock_speed=25630710)
pca.frequency = 49
# The pulse range is 749 - 2250 by default.
servo-1 = servo.ContinuousServo(pca.channels[0])
# To get the full range of the servo you will likely need to adjust the min_pulse and max_pulse to
# match the stall points of the servo.
# If your servo doesn't stop once the script is finished you may need to tune the
# reference_clock_speed above or the min_pulse and max_pulse timings below.
# servo6 = servo.ContinuousServo(pca.channels[7], min_pulse=750, max_pulse=2250)
print("Forwards")
servo-1.throttle = 1
time.sleep(0)
print("Backwards")
servo-1.throttle = -1
time.sleep(0)
print("Stop")
def left_rotate():
leftServoPin = pwmio.PWMOut(board.D27, frequency=50)
leftServo = servo.ContinuousServo(leftServoPin)
leftServo.throttle = 0.08
time.sleep(0.1)
leftServo.throttle = 0.0
def right_rotate():
rightServoPin = pwmio.PWMOut(board.D17, frequency=50)
rightServo = servo.ContinuousServo(rightServoPin)
rightServo.throttle = 0.08
time.sleep(0.1)
rightServo.throttle = 0.0
# Create a simple PCA9685 class instance.
pca = PCA9685(i2c)
# You can optionally provide a finer tuned reference clock speed to improve the accuracy of the
# timing pulses. This calibration will be specific to each board and its environment. See the
# calibration.py example in the PCA9685 driver.
# pca = PCA9685(i2c, reference_clock_speed=25630710)
pca.frequency = 50
# The pulse range is 750 - 2250 by default.
#servo7 = servo.ContinuousServo(pca.channels[0])
# If your servo doesn't stop once the script is finished you may need to tune the
# reference_clock_speed above or the min_pulse and max_pulse timings below.
servo7 = servo.ContinuousServo(pca.channels[0], min_pulcse=750, max_pulse=2250)
#print("Forwards")
#servo7.throttle = 1
#time.sleep(1)
print("Backwards")
servo7.throttle = -1
time.sleep(1)
print("Stop")
servo7.throttle = 0
pca.deinit()
from adafruit_motor import servo
i2c = busio.I2C(SCL, SDA)
# Create a simple PCA9685 class instance.
pca = PCA9685(i2c)
# You can optionally provide a finer tuned reference clock speed to improve the accuracy of the
# timing pulses. This calibration will be specific to each board and its environment. See the
# calibration.py example in the PCA9685 driver.
# pca = PCA9685(i2c, reference_clock_speed=25630710)
pca.frequency = 50
# The pulse range is [1250 (full forward), 1750 (full reverse)].
pulses[
servo.ContinuousServo(pca.channels[7], min_pulse=1250, max_pulse=1750),
servo.ContinuousServo(pca.channels[8], min_pulse=1250, max_pulse=1750)
]
# TODO MATH OF PULSE INTO FLOAT RANGE [-1, 1]
while 1:
//forward
pulses[0].throttle = 1 # writeMicroseconds(1600);
pulses[1].throttle = 1 # writeMicroseconds(1500);
time.sleep(2)
//backward
pulses[0].throttle = 1 # writeMicroseconds(1400);
pulses[1].throttle = 1 # writeMicroseconds(1500);
time.sleep(2)
i2c = busio.I2C(board.SCL, board.SDA)
rtc = adafruit_ds3231.DS3231(i2c)
audio = audioio.AudioOut(board.A0)
strip = neopixel.NeoPixel(NEOPIXEL_PIN, NUM_PIXELS, brightness=1, auto_write=False)
strip.fill(0) # NeoPixels off ASAP on startup
strip.show()
switch = Debouncer(SWITCH_PIN, Pull.UP, 0.01)
# create a PWMOut object on Pin A2.
pwm = pulseio.PWMOut(SERVO_PIN, duty_cycle=2 ** 15, frequency=50)
# Create a servo object, my_servo.
servo = servo.ContinuousServo(pwm)
servo.throttle = 0.0
# Set the time for testing
# Once finished testing, the time can be set using the REPL using similar code
if TESTING:
# year, mon, date, hour, min, sec, wday, yday, isdst
t = time.struct_time((2018, 12, 31, 23, 58, 55, 1, -1, -1))
# you must set year, mon, date, hour, min, sec and weekday
# yearday is not supported, isdst can be set but we don't do anything with it at this time
print("Setting time to:", t)
rtc.datetime = t
print()
################################################################################
# Global Variables
import board
import pulseio
from adafruit_motor import servo
# I got the adafruit_motor library here: https://github.com/adafruit/Adafruit_CircuitPython_Motor/releases/
# CPX has PWM on the following pins: A1, A2, A3, A6, A8, A9
# There is NO PWM on: A0, A4, A5, A7
# create two PWMOut objects on pins A2 and A3
pwm1 = pulseio.PWMOut(board.A2, duty_cycle=0, frequency=50)
pwm2 = pulseio.PWMOut(board.A3, duty_cycle=0, frequency=50)
# Create two servo objects, one 180 and one continuous
servo180 = servo.Servo(pwm1)
servoCont = servo.ContinuousServo(pwm2)
while True:
for angle in range(0, 180): # 0 - 180 degrees, 5 degrees at a time.
servo180.angle = angle
time.sleep(0.01)
for angle in range(180, 0, -1): # 180 - 0 degrees, 5 degrees at a time.
servo180.angle = angle
time.sleep(0.01)
for throttle in range(-100, 100, 2):
servoCont.throttle = throttle / 100
time.sleep(0.05)
for throttle in range(100, -100, -2):
servoCont.throttle = throttle / 100
time.sleep(0.05)
#Shaft Servo Set Up pwmShaftServo = pulseio.PWMOut(board.D9, duty_cycle=2**15, frequency=50) shaftServo = servo.Servo(pwmShaftServo) shaftPot = AnalogIn(board.A1) #Pitch Servo Set Up pwmPitchServo = pulseio.PWMOut(board.A3, duty_cycle=2**15, frequency=50) pitchServo = servo.Servo(pwmPitchServo) pitchServoPot = AnalogIn(board.A0) #Motor Set Up pwmMotor = pulseio.PWMOut(board.A2, frequency=50) my_Motor = servo.ContinuousServo(pwmMotor) motorPot = AnalogIn(board.A5) #PhotoInteruppter SetUp photoPin = digitalio.DigitalInOut(board.D8) photoPin.direction = digitalio.Direction.INPUT photoPin.pull = digitalio.Pull.UP interrupts = -1 lread = True fread = True #LCD Set Up from lcd.lcd import LCD from lcd.i2c_pcf8574_interface import I2CPCF8574Interface from lcd.lcd import CursorMode
#connect to LiDAR
lidar = RPLidar("/dev/ttyUSB0") # may change ttyUSBX where X is 0,1,2,3
time.sleep(1)
#connect to PCA9685 ie servo driver
i2c = busio.I2C(board.SCL, board.SDA)
pca = adafruit_pca9685.PCA9685(i2c)
#sets the motor and servo channel on servo driver
speed_channel = pca.channels[4]
steer_channel = pca.channels[5]
pca.frequency = 60
steer = servo.Servo(steer_channel, min_pulse=1000, max_pulse=2000)
speed = servo.ContinuousServo(speed_channel,
min_pulse=1000,
max_pulse=2000)
steer.angle = aligned
speed.throttle = 0
except Exception as e:
print("something went wrong initializing...")
print(e)
curses.endwin()
br = False
#used to stop all motion
def still():
steer.angle = aligned
import adafruit_pca9685
import busio
from board import SCL, SDA
import time
from adafruit_motor import servo
i2c = busio.I2C(SCL, SDA)
pwm = adafruit_pca9685.PCA9685(i2c)
pwm.frequency = 60
esc = servo.ContinuousServo(pwm.channels[1], min_pulse=1000, max_pulse=2000)
# esc.set_pulse_width_range(1000,2000)
esc.throttle = 1
input("connect power and press ENTER")
esc.throttle = 0
input("neutral")
time.sleep(1)
esc.throttle = 1
input("max")
time.sleep(1)
esc.throttle = -1
input("min")
time.sleep(1)
esc.throttle = 0.075
time.sleep(1)
esc.throttle = -1
pwm.deinit()
lostSignalTimer = 5.0 sonarSensitivityFront = 50.0 # obstacle distance in cm before avoiding defaultMoveSpeed = 0.7 defaultTurnSpeed = 0. # -------- set up ultrasonic sensor ----------- trig1 = board.D echo1 = board.D60 sonar_front = hcsr04_lib.HCSR04(trig1, echo1) # -------- set up servos ----------- # dc motor motor_pin = board.D31 pwm = pulseio.PWMOut(motor_pin, frequency=50) servo_dc = servo.ContinuousServo(pwm) # steering servo servo_pin = board.D42 pwm1 = pulseio.PWMOut(servo_pin, duty_cycle=0, frequency=50) servo_turn = servo.Servo(pwm1, min_pulse=500, max_pulse=2500) # # create a PWMOut object on the control pin. # pwm1 = pulseio.PWMOut(board.D31, duty_cycle=0, frequency=50) # pwm2 = pulseio.PWMOut(board.D36, duty_cycle=0, frequency=50) # # pulse widths exercise the full range of the 169 servo, other servos are different # servo1 = servo.ContinuousServo(pwm1, min_pulse=500, max_pulse=2500) # servo2 = servo.ContinuousServo(pwm2, min_pulse=500, max_pulse=2500) # -------- set up GPS -----------
from adafruit_servokit import ServoKit
from adafruit_motor import servo
# initialize pca9685
kit = ServoKit(channels=16)
# declare motor esc as continuous servo because it expects servo like PWM input
kit._items[4] = servo.ContinuousServo(kit._pca.channels[0])
ESC = kit._items[4]
# experimentally found pulse width range shifted -> 100
ESC.set_pulse_width_range(1200, 2000)
# allow myself to control it
while True:
ESC.throttle = float(input("OUT> "))
# spi = busio.SPI(board.SCK, board.MOSI, board.MISO) # csag = DigitalInOut(board.D5) # csag.direction = Direction.OUTPUT # csag.value = True # csm = DigitalInOut(board.D6) # csm.direction = Direction.OUTPUT # csm.value = True # sensor = adafruit_lsm9ds1.LSM9DS1_SPI(spi, csag, csm) # -------- set up servos ----------- # create a PWMOut object on the control pin. pwm1 = pulseio.PWMOut(board.D42, duty_cycle=0, frequency=50) pwm2 = pulseio.PWMOut(board.D41, duty_cycle=0, frequency=50) # pulse widths exercise the full range of the 169 servo, other servos are different servo1 = servo.ContinuousServo(pwm1, min_pulse=500, max_pulse=2500) servo2 = servo.ContinuousServo(pwm2, min_pulse=500, max_pulse=2500) # -------- set up GPS ----------- # Define RX and TX pins for the board's serial port connected to the GPS. # These are the defaults you should use for the GPS FeatherWing. # For other boards set RX = GPS module TX, and TX = GPS module RX pins. RX = board.RX1 TX = board.TX1 # Create a serial connection for the GPS connection using default veloc and # a slightly higher timeout (GPS modules typically update once a second). uart = busio.UART(TX, RX, baudrate=9600, timeout=30) # for a computer, use the pyserial library for uart access #import serial
import time
from board import SCL, SDA
import busio
from adafruit_metrom0 import metrom0
from adafruit_motor import servo
i2c = busio.I2C(SCL, SDA)
metrom0 = metrom0(i2c)
metrom0.frequency = 50
servo7 = servo.ContinuousServo(metrom0.channels[7])
print("forwards")
servo7.throttle = 1
time.sleep(.1)
print("backwards")
servo7.throttle = -1
print("stop")
servo7.throttle = 0
metrom0.deinit()
x = sonar.distance
except RuntimeError:
x = 999
return x
# setup beeper
beeper = digitalio.DigitalInOut(board.A5)
beeper.direction = digitalio.Direction.OUTPUT
beeper.value = False
# create a PWMOut object on Pin D12 and D11
pwmL = pulseio.PWMOut(board.D12, frequency=50)
pwmR = pulseio.PWMOut(board.D11, frequency=50)
# Create a servo object, left_servo.
left_servo = servo.ContinuousServo(pwmL)
right_servo = servo.ContinuousServo(pwmR)
# create object for sonar device
sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.D10, echo_pin=board.D7, timeout=1.0)
# setup for NeoPixels (RGB) ########################################################
NUMPIXELS = 7
ORDER = neopixel.GRB
neopixels = neopixel.NeoPixel(board.D5, NUMPIXELS, brightness=0.2, auto_write=False, pixel_order=ORDER)
# startup delay
for i in range(5):
neopixels[i+2] = (255, 255, 0)
neopixels.show()
beep(0.25)
import board
import busio
import pulseio
from adafruit_pca9685 import PCA9685
from adafruit_motor import servo
import time
i2c = busio.I2C(board.SCL, board.SDA)
pca = PCA9685(i2c)
pca.frequency = 50
joint_1 = servo.ContinuousServo(pca.channels[0], min_pulse= 1350, max_pulse= 1765)
joint_2 = servo.ContinuousServo(pca.channels[1], min_pulse= 1350, max_pulse= 1770)
def get_feedback(pin):
global pulses
unitsFC = 360
dutyScale = 1000
tc = 0
dcMin = 29
dcMax = 971
q2min = unitsFC/4
q3max = q2min * 3
turns = 0
pulses = pulseio.PulseIn(pin)
thetaP = 0
while True:
while len(pulses) < 2:
if pulses.paused == True:
# Import the PCA9685 module. Available in the bundle and here:
# https://github.com/adafruit/Adafruit_CircuitPython_PCA9685
from adafruit_pca9685 import PCA9685
from adafruit_motor import servo
STEERING_MAX_POS = 68
STEERING_ZERO = 48
STEERING_MAX_NEG = 28
THROTTLE_SCALE = 0.5
i2c = busio.I2C(SCL, SDA)
pca = PCA9685(i2c)
pca.frequency = 60
motor = servo.ContinuousServo(pca.channels[0])
steering = servo = servo.Servo(pca.channels[2], actuation_range=90)
def stop():
motor.throttle = 0
def drive(amount):
if (amount > 1):
amount = 1
if (amount < -1):
amount = -1
motor.throttle = amount * THROTTLE_SCALE
import time
import pulseio
import board
import touchio
from adafruit_motor import servo
#imports all libraries
touch_A0 = touchio.TouchIn(board.A0) # A0touch on Metro M0 Express
touch_A4 = touchio.TouchIn(board.A4) # A4 touch on Metro M0 Express
# create a PWMOut object on Pin A2
pwm = pulseio.PWMOut(board.A2, frequency=50)
# Create a servo object
moth_slippers = servo.ContinuousServo(pwm)
while True: #loops
if touch_A0.value: #if wire is touched
moth_slippers.throttle = 1.0 #rotate slowly forwards
print("A0")
if touch_A4.value:
moth_slippers.throttle = -1.0
print("A4") #rotate slowly forwards
if not touch_A0.value and not touch_A4.value:
moth_slippers.throttle = 0.0
# makes sure servo doesn't move when wires aren't touched
time.sleep(0.1)
from adafruit_motor import motor
from controller import PS4Controller
#import camera
#init controller
ps4 = PS4Controller()
#init i2c
i2c = busio.I2C(SCL, SDA)
#init PCA
pca = PCA9685(i2c)
pca.frequency = 50
servo_steer = servo.Servo(pca.channels[0])
esc = servo.ContinuousServo(pca.channels[1])
def scale_servo(x):
y = round((30 - 70) * x + 1 / 1 + 1 + 70, 2) #used to scale -1,1 to 0,180
return y
def scale_esc(x):
y = round((x + 1) / 8, 2) #used to scale -1,1 to 0,180
return y
def drive(axis_data):
servo_steer.angle = scale_servo(axis_data[0])
sum_inputs = round(-scale_esc(axis_data[4]) + scale_esc(axis_data[3]), 2)
from adafruit_pca9685 import PCA9685
from adafruit_motor import servo
i2c = busio.I2C(SCL, SDA)
# Create a simple PCA9685 class instance.
pca = PCA9685(i2c)
# You can optionally provide a finer tuned reference clock speed to improve the accuracy of the
# timing pulses. This calibration will be specific to each board and its environment. See the
# calibration.py example in the PCA9685 driver.
# pca = PCA9685(i2c, reference_clock_speed=25630710)
pca.frequency = 50
# The pulse range is 750 - 2250 by default.
servo7 = servo.ContinuousServo(pca.channels[7])
# If your servo doesn't stop once the script is finished you may need to tune the
# reference_clock_speed above or the min_pulse and max_pulse timings below.
# servo7 = servo.ContinuousServo(pca.channels[7], min_pulse=750, max_pulse=2250)
print("Forwards")
servo7.throttle = 1
time.sleep(1)
print("Backwards")
servo7.throttle = -1
time.sleep(1)
print("Stop")
servo7.throttle = 0