def __init__(self, screen_dashboard):
self.screen_dashboard = screen_dashboard
self.max_accel = 10 # max allowed acceleration in pct per 0.1 sec
self.cur_throt_L = 0
self.cur_throt_R = 0
self.Ain1 = pulseio.PWMOut(board.D10, frequency=1600)
self.Ain2 = pulseio.PWMOut(board.D9, frequency=1600)
self.motorR = motor.DCMotor(self.Ain1, self.Ain2)
self.Bin1 = pulseio.PWMOut(board.D11, frequency=1600)
self.Bin2 = pulseio.PWMOut(board.D12, frequency=1600)
self.motorL = motor.DCMotor(self.Bin1, self.Bin2)
#
# speed calibration constants to equalize motor speed
# note each throttle request is multiplied by this per-motor constant
# the "slower" motor should be set to 1.0,
# the faster motor should be set to whatever it takes to reduces its
# speed to match slower motor
#
self.motorCalibrateL = 1
self.motorCalibrateR = 0.95
self.motorCalibrateL_turn = 0.80
self.motorCalibrateR_turn = 1
# throttle and duration for 90 (or 180) degree turn-in-place
# assumes one motor goes forward, the other back at same throttle
self.seconds_for_360 = SECONDS_FOR_360 = 2.5
self.throttle_for_360 = 0.25
self.cm_per_sec_at_100pct = 60
self.cm_per_sec_at_25pct = 15
self.max_delta_throt = 0.1
def __init__(self, pca, channels):
self.pca = pca #the 16 channel pca9685
#the motors
self.motor = motor.DCMotor(self.pca.channels[channels[0]],
self.pca.channels[channels[1]])
self.motor.decay_mode = (motor.SLOW_DECAY)
def motor2(self):
""":py:class:`~adafruit_motor.motor.DCMotor` controls for motor 2.
.. image :: /_static/motor_featherwing/m2.jpg
:alt: Motor 2 location
This example moves the motor forwards for one fifth of a second at full speed.
.. code-block:: python
import time
from adafruit_featherwing import motor_featherwing
wing = motor_featherwing.MotorFeatherWing()
wing.motor2.throttle = 1.0
time.sleep(0.2)
wing.motor2.throttle = 0
"""
if not self._motor2:
if self._stepper1:
raise RuntimeError(
"Cannot use motor2 at the same time as stepper1.")
self._pca.channels[13].duty_cycle = 0xffff
self._motor2 = motor.DCMotor(self._pca.channels[11],
self._pca.channels[12])
return self._motor2
def pcaInit(self):
self.i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(self.i2c, address=0x60)
self.pca.frequency = 100
self.pca.channels[8].duty_cycle = 0xFFFF
self.motor1 = motor.DCMotor(self.pca.channels[10],
self.pca.channels[9])
self.pca.channels[13].duty_cycle = 0xFFFF
self.motor2 = motor.DCMotor(self.pca.channels[11],
self.pca.channels[12])
self.pca.channels[2].duty_cycle = 0xFFFF
self.motor3 = motor.DCMotor(self.pca.channels[4], self.pca.channels[3])
self.pca.channels[7].duty_cycle = 0xFFFF
self.motor4 = motor.DCMotor(self.pca.channels[5], self.pca.channels[6])
pass
def __init__(self):
print("init.............")
self.i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(self.i2c, address=0x40)
self.pca.frequency = 100
self.pca.channels[4].duty_cycle = 0xFFFF
self.pca.channels[9].duty_cycle = 0xFFFF
self.pca.channels[15].duty_cycle = 0xFFFF
self.pca.channels[10].duty_cycle = 0xFFFF
self.motor1 = motor.DCMotor(self.pca.channels[5], self.pca.channels[6])
self.motor2 = motor.DCMotor(self.pca.channels[8], self.pca.channels[7])
self.motor3 = motor.DCMotor(self.pca.channels[14],
self.pca.channels[13])
self.motor4 = motor.DCMotor(self.pca.channels[11],
self.pca.channels[12])
print("init.............finish")
def _motor(self, motor_name, channels, stepper_name):
from adafruit_motor import motor
motor_name = "_motor" + str(motor_name)
stepper_name = "_stepper" + str(stepper_name)
if not getattr(self, motor_name):
if getattr(self, stepper_name):
raise RuntimeError(
"Cannot use {} at the same time as {}.".format(motor_name[1:],
stepper_name[1:]))
self._pca.channels[channels[0]].duty_cycle = 0xffff
setattr(self, motor_name, motor.DCMotor(self._pca.channels[channels[1]],
self._pca.channels[channels[2]]))
return getattr(self, motor_name)
import time
from busio import I2C
from adafruit_seesaw.seesaw import Seesaw
from adafruit_seesaw.pwmout import PWMOut
from adafruit_motor import motor, servo
from digitalio import DigitalInOut, Direction, Pull
import board
print("Mag Neat-o!")
# Create seesaw object
i2c = I2C(board.SCL, board.SDA)
seesaw = Seesaw(i2c)
# Create one motor on seesaw PWM pins 22 & 23
motor_a = motor.DCMotor(PWMOut(seesaw, 22), PWMOut(seesaw, 23))
# Create another motor on seesaw PWM pins 19 & 18
motor_b = motor.DCMotor(PWMOut(seesaw, 19), PWMOut(seesaw, 18))
# Create servo object
pwm = PWMOut(seesaw, 17) # Servo 1 is on s.s. pin 17
pwm.frequency = 50 # Servos like 50 Hz signals
my_servo = servo.Servo(pwm) # Create my_servo with pwm signa
my_servo.angle = 90
def smooth_move(start, stop, num_steps):
return [(start + (stop - start) * i / num_steps) for i in range(num_steps)]
buttona = DigitalInOut(board.BUTTON_A)
# Create a simple PCA9685 class instance for the Motor FeatherWing's default address.
pca = PCA9685(i2c, address=0x40)
pca.frequency = 60
# Motor 1 is channels 9 and 10 with 8 held high.
# Motor 2 is channels 11 and 12 with 13 held high.
# Motor 3 is channels 3 and 4 with 2 held high.
# Motor 4 is channels 5 and 6 with 7 held high.
# DC Motors generate electrical noise when running that can reset the microcontroller in extreme
# cases. A capacitor can be used to help prevent this. The demo uses motor 4 because it worked ok
# in testing without a capacitor.
# See here for more info: https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/faq#faq-13
pca.channels[7].duty_cycle = 0xffff
motor4 = motor.DCMotor(pca.channels[0], pca.channels[1])
print("Forwards slow")
motor4.throttle = 0.5
print("throttle:", motor4.throttle)
time.sleep(1)
print("Forwards")
motor4.throttle = 1
print("throttle:", motor4.throttle)
time.sleep(1)
print("Backwards")
motor4.throttle = -1
print("throttle:", motor4.throttle)
time.sleep(1)
import pwmio from adafruit_motor import servo, motor import adafruit_vl53l0x # Initialize buttons btn1 = digitalio.DigitalInOut(board.GP20) btn2 = digitalio.DigitalInOut(board.GP21) btn1.direction = digitalio.Direction.INPUT btn2.direction = digitalio.Direction.INPUT btn1.pull = digitalio.Pull.UP btn2.pull = digitalio.Pull.UP # Initialize DC motors m1a = pwmio.PWMOut(board.GP8, frequency=50) m1b = pwmio.PWMOut(board.GP9, frequency=50) motor1 = motor.DCMotor(m1a, m1b) m2a = pwmio.PWMOut(board.GP10, frequency=50) m2b = pwmio.PWMOut(board.GP11, frequency=50) motor2 = motor.DCMotor(m2a, m2b) # Initialize I2C bus and sensor. i2c = busio.I2C(board.GP5, board.GP4) vl53 = adafruit_vl53l0x.VL53L0X(i2c) piezo = board.GP22 # ------------------------------------------------- # FOREVER LOOP: Check buttons & animate RGB LEDs # ------------------------------------------------- start = False while True:
##initialize gripper
left_gripper_sensor = DigitalInOut(board.D12)
left_gripper_sensor.direction = Direction.INPUT
left_gripper_sensor.pull = Pull.DOWN
left_gripper = Gripper(servokit.servo[LEFT_GRIPPER_SERVO], left_gripper_sensor)
##initialize wheel motors
ain1 = pulseio.PWMOut(board.D5)
ain2 = pulseio.PWMOut(board.D6)
wheels_sleep = DigitalInOut(board.D9)
bin2 = pulseio.PWMOut(board.D10)
bin1 = pulseio.PWMOut(board.D11)
wheels_sleep.direction = Direction.OUTPUT
wheels_sleep.value = True
left_wheel_motor = motor.DCMotor(ain1, ain2)
right_wheel_motor = motor.DCMotor(bin1, bin2)
left_wheel_motor.throttle = 0
right_wheel_motor.throttle = 0
left_wheel = Wheel(left_wheel_motor)
right_wheel = Wheel(right_wheel_motor)
##battery management
vbat_voltage = AnalogIn(board.VOLTAGE_MONITOR)
def get_voltage(pin):
return (pin.value * 3.3) / 65536 * 2
#functions
class DC:
controlCourse = 0
controlCourseFlag = True
getDirectionFlag = False
LE_MAX = 1
LE_MAX_NEGATIVE = -1
LE_CORRECTION = 0.7
right_index = 100
left_index = 100
MQTT_SERVER = "localhost"
MQTT_PATH = "rpi/gpio"
RE_MAX = 1
RE_MAX_NEGATIVE = -1
RE_CORRECTION = 0.8
client2 = mqtt.Client()
i2c = busio.I2C(SCL, SDA)
# Create a simple PCA9685 class instance.
pca = PCA9685(i2c, address=0x40)
motor_hl = motor.DCMotor(pca.channels[2], pca.channels[1])
motor_hr = motor.DCMotor(pca.channels[3], pca.channels[4])
#Magnetometer
sensor = SL_MPU9250(0x68, 1)
sensor.resetRegister()
sensor.powerWakeUp()
sensor.setMagRegister('100Hz', '16bit')
def __init__(self):
self.pca.frequency = 100
def setPower(self, rightEngine, leftEngine):
self.right_index = rightEngine / 100
self.left_index = leftEngine / 100
print(self.right_index)
def forward(self, speed):
self.getDirectionFlag = True
self.controlCourseFlag = True
self.motor_hr.throttle = self.right_index * self.RE_MAX * speed / 10
self.motor_hl.throttle = self.left_index * self.LE_MAX * speed / 10
self.sendMagnetomrterTelemetry()
# x = threading.Thread(target=self.getDirection).start()
def on_connect(self, client, userdata, flags, rc):
# Subscribing in on_connect() means that if we lose the connection and
# reconnect then subscriptions will be renewed.
self.client2.subscribe(self.MQTT_PATH)
def on_message2(self, client, userdata, msg):
print("On message 2")
def on_publish(self, client, userdata, mid):
print("This is on publish from dc")
def sendMagnetomrterTelemetry(self):
print("send magnetos 1")
self.client2.on_connect = self.on_connect
self.client2.on_message = self.on_message2
self.client2.on_publish = self.on_publish
self.client2.connect(self.MQTT_SERVER, 1883, 60)
print("send magnetos 2")
now = time.time()
mag = self.sensor.getMag()
print("send magnetos 3")
print("%+8.7f" % mag[0] + " ")
print("%+8.7f" % mag[1] + " ")
print("%+8.7f" % mag[2])
deg = round(math.atan2(mag[1], mag[0]) * 180 / math.pi) + 180
self.client2.publish(
self.MQTT_PATH, '{"msg":"from python","x":' + str(mag[0]) +
',"y":' + str(mag[1]) + ',"status":' + str(deg) + '}')
self.client2.disconnect()
def getDirection(self):
print(str(self.getDirectionFlag))
while self.getDirectionFlag:
now = time.time()
mag = self.sensor.getMag()
print("%+8.7f" % mag[0] + " ")
print("%+8.7f" % mag[1] + " ")
print("%+8.7f" % mag[2])
deg = round(math.atan2(mag[1], mag[0]) * 180 / math.pi) + 180
print(str(deg))
if (self.controlCourseFlag):
self.controlCourse = deg
self.controlCourseFlag = False
if (deg < self.controlCourse - 1):
self.courseCorrection(direction="right")
elif (deg > self.controlCourse + 1):
self.courseCorrection(direction="left")
#else:
#self.courseCorrection(direction = "ahead")
#self.getDirectionFlag = False
sleepTime = 1 - (time.time() - now)
if sleepTime < 0.0:
continue
time.sleep(sleepTime)
def courseCorrection(self, direction):
if (direction == "left"):
hr = 0.3 #self.RE_MAX
hl = 0 #self.LE_CORRECTION
elif (direction == "right"):
hr = 0 # self.RE_CORRECTION
hl = 0.3 #self.LE_MAX
else:
return
if (self.getDirectionFlag == False):
return
self.motor_hr.throttle = hr
self.motor_hl.throttle = hl
return
def stop(self):
self.getDirectionFlag = False
self.motor_hr.throttle = 0
self.motor_hl.throttle = 0
def right(self, speed):
self.getDirectionFlag = False
self.motor_hr.throttle = self.right_index * self.RE_MAX_NEGATIVE * speed / 10
self.motor_hl.throttle = self.left_index * self.LE_MAX * speed / 10
def left(self, speed):
self.getDirectionFlag = False
self.motor_hr.throttle = self.right_index * self.RE_MAX * speed / 10
self.motor_hl.throttle = self.left_index * self.LE_MAX_NEGATIVE * speed / 10
def back(self, speed):
self.getDirectionFlag = False
self.motor_hr.throttle = self.right_index * self.RE_MAX_NEGATIVE * speed / 10
self.motor_hl.throttle = self.left_index * self.LE_MAX_NEGATIVE * speed / 10
# Create a simple PCA9685 class instance for the Motor FeatherWing's default address.
pca = PCA9685(i2c, address=0x60)
pca.frequency = 100
# Motor 1 is channels 9 and 10 with 8 held high.
# Motor 2 is channels 11 and 12 with 13 held high.
# Motor 3 is channels 3 and 4 with 2 held high.
# Motor 4 is channels 5 and 6 with 7 held high.
# DC Motors generate electrical noise when running that can reset the microcontroller in extreme
# cases. A capacitor can be used to help prevent this. The demo uses motor 4 because it worked ok
# in testing without a capacitor.
# See here for more info: https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/faq#faq-13
pca.channels[7].duty_cycle = 0xFFFF
motor4 = motor.DCMotor(pca.channels[5], pca.channels[6])
print("Forwards slow")
motor4.throttle = 0.5
print("throttle:", motor4.throttle)
time.sleep(1)
print("Forwards")
motor4.throttle = 1
print("throttle:", motor4.throttle)
time.sleep(1)
print("Backwards")
motor4.throttle = -1
print("throttle:", motor4.throttle)
time.sleep(1)
from adafruit_servokit import ServoKit import board import busio import adafruit_pca9685 import time from adafruit_motor import servo, motor from adafruit_motorkit import MotorKit i2c = busio.I2C(board.SCL, board.SDA) pca = adafruit_pca9685.PCA9685(i2c, address=0x40) pca.frequency = 100 motor4 = motor.DCMotor(pca.channels[6], pca.channels[7]) motor4.decay_mode = (motor.SLOW_DECAY) motor4.throttle = 1 time.sleep(3) motor4.throttle = 0 time.sleep(3) motor4.throttle = -1 time.sleep(3) motor4.throttle = 0 """i2c = busio.I2C(board.SCL, board.SDA) pca = adafruit_pca9685.PCA9685(i2c) pca.frequency = 50 servo0 = servo.Servo(pca.channels[8]) servo1 = servo.Servo(pca.channels[9]) #servo0.angle=90
chan_PWMA = 0
chan_AIN2 = 1
chan_AIN1 = 4
chan_BIN1 = 6
chan_BIN2 = 5
chan_PWMB = 7
# initialize PCA 9685
i2c = busio.I2C(board.SCL, board.SDA)
pca = adafruit_pca9685.PCA9685(i2c)
pca.frequency = 1000
# motor A settings
pca.channels[chan_PWMA].duty_cycle = 0xffff #hold high
motorA = motor.DCMotor(pca.channels[chan_AIN1], pca.channels[chan_AIN2])
# motor B settings
pca.channels[chan_PWMB].duty_cycle = 0xffff
motorB = motor.DCMotor(pca.channels[chan_BIN2], pca.channels[chan_BIN1])
motorA.throttle = 0.0
motorB.throttle = 0.0
motor_speed_left = 0.0
motor_speed_right = 0.0
def callback(msg):
rospy.loginfo("Received a /cmd_vel message")
rospy.loginfo("Linear Components: [%f, %f, %f]" %
(msg.linear.x, msg.linear.y, msg.linear.z))
# Create a simple PCA9685 class instance for the Motor FeatherWing's default address.
pca = PCA9685(i2c, address=0x40)
pca.frequency = 100
# Motor 1 is channels 9 and 10 with 8 held high.
# Motor 2 is channels 11 and 12 with 13 held high.
# Motor 3 is channels 3 and 4 with 2 held high.
# Motor 4 is channels 5 and 6 with 7 held high.
# DC Motors generate electrical noise when running that can reset the microcontroller in extreme
# cases. A capacitor can be used to help prevent this. The demo uses motor 4 because it worked ok
# in testing without a capacitor.
# See here for more info: https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/faq#faq-13
#pca.channels[7].duty_cycle = 0xffff
motor_rl = motor.DCMotor(pca.channels[10], pca.channels[9])
motor_rr = motor.DCMotor(pca.channels[7], pca.channels[8])
motor_hl = motor.DCMotor(pca.channels[2], pca.channels[1])
motor_hr = motor.DCMotor(pca.channels[3], pca.channels[4])
print("Forwards slow")
motor_rr.throttle = 0.5
motor_hr.throttle = 0.5
motor_rl.throttle = 0.5
motor_hl.throttle = 0.5
print("throttle:", motor_rr.throttle)
time.sleep(1)
print("Forwards fast")
import board
import pwmio
import neopixel
import random
pwm_a = pwmio.PWMOut(board.GP0, frequency=50)
pwm_b = pwmio.PWMOut(board.GP2, frequency=50)
pixelCount = 12
pixels = neopixel.NeoPixel(board.GP1,
pixelCount,
brightness=.8,
auto_write=False)
stirring = motor.DCMotor(pwm_a, pwm_b)
stirring.throttle = 0
print("motor set up")
stirring.throttle = 1
pixels.fill((0, 0, 0))
while True:
pixels.fill((255, 255, 255))
pixels[random.randint(0, (pixelCount - 1))] = (200, 50, 50)
pixels[random.randint(0, (pixelCount - 1))] = (50, 200, 50)
pixels[random.randint(0, (pixelCount - 1))] = (50, 50, 200)
pixels.show()
time.sleep(.1)
#################### 4 Servos
servos = []
for ss_pin in (17, 16, 15, 14):
pwm = PWMOut(ss, ss_pin)
pwm.frequency = 50
_servo = servo.Servo(pwm)
_servo.angle = 90 # starting angle, middle
servos.append(_servo)
#################### 2 DC motors
motors = []
for ss_pin in ((22, 23), (18, 19)):
pwm0 = PWMOut(ss, ss_pin[0])
pwm1 = PWMOut(ss, ss_pin[1])
_motor = motor.DCMotor(pwm0, pwm1)
motors.append(_motor)
servos[0].angle = 180
while True:
if switch.value:
# Switch is on, activate MUSIC POWER!
# motor forward slowly
motors[0].throttle = 0.2
# mote the head forward slowly, over 0.9 seconds
for a in range(180, 90, -1):
servos[0].angle = a
time.sleep(0.01)
print("servo smooth test: 180 - 0, -1º steps")
for angle in range(180, 0, -1): # 180 - 0 degrees, -1º at a time.
servo1.angle = angle
time.sleep(0.01)
time.sleep(1)
print("servo smooth test: 0 - 180, 1º steps")
for angle in range(0, 180, 1): # 0 - 180 degrees, 1º at a time.
servo1.angle = angle
time.sleep(0.01)
time.sleep(1)
# DC motor setup
pwm_a = pwmio.PWMOut(board.GP28, frequency=50)
pwm_b = pwmio.PWMOut(board.GP27, frequency=50)
motor1 = motor.DCMotor(pwm_a, pwm_b)
# DC motor test
def dc_motor_test():
print("DC motor test: 0.5")
motor1.throttle = 0.5
time.sleep(0.5)
print("DC motor test: None")
motor1.throttle = None
time.sleep(0.5)
print("DC motor test: -0.5")
motor1.throttle = -0.5
time.sleep(0.5)
print("DC motor test: None")
motor1.throttle = None
return x
# setup beeper
beeper = digitalio.DigitalInOut(board.A5)
beeper.direction = digitalio.Direction.OUTPUT
beeper.value = False
# create PWMOut objects for motors
pwmA_m1 = pulseio.PWMOut(board.D12, frequency=50)
pwmB_m1 = pulseio.PWMOut(board.D11, frequency=50)
pwmA_m2 = pulseio.PWMOut(board.D13, frequency=50)
pwmB_m2 = pulseio.PWMOut(board.D9, frequency=50)
# then create the motor objects
left_motor = motor.DCMotor(pwmA_m1, pwmB_m1)
right_motor = motor.DCMotor(pwmA_m2, pwmB_m2)
# 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)
from busio import I2C
from adafruit_seesaw.seesaw import Seesaw
from adafruit_seesaw.pwmout import PWMOut
from adafruit_motor import motor
import board
import time
# Create seesaw object
i2c = I2C(board.SCL, board.SDA)
seesaw = Seesaw(i2c)
# Create one motor on seesaw PWM pins 22 & 23
motor_a = motor.DCMotor(PWMOut(seesaw, 22), PWMOut(seesaw, 23))
motor_a.throttle = 0.5 # half speed forward
# Create drive (PWM) object
my_drive = PWMOut(seesaw, 13) # Drive 1 is on s.s. pin 13
my_drive.frequency = 1000 # Our default frequency is 1KHz
while True:
my_drive.duty_cycle = 32768 # half on
time.sleep(0.8)
my_drive.duty_cycle = 16384 # dim
time.sleep(0.1)
# and repeat!
import time import board import pulseio from adafruit_motor import servo, motor # create a PWMOut object on Pin A2. # duty_cycle=2 ** 15, spd = pulseio.PWMOut(board.D13, frequency=0) # 50 = clockwise # < 50 = counterclockwise dir = pulseio.PWMOut(board.D4, frequency=50) # Create a servo object, my_servo. #my_servo = servo.Servo(spd) spd_servo = motor.DCMotor(spd, dir) #dir_servo = servo.ContinuousServo(dir) #while True: # for angle in range(0, 180, 5): # 0 - 180 degrees, 5 degrees at a time. # my_servo.angle = angle # time.sleep(0.05) # for angle in range(180, 0, -5): # 180 - 0 degrees, 5 degrees at a time. # my_servo.angle = angle # time.sleep(0.05) spd_servo.throttle = 0.5 time.sleep(5.0) spd_servo.throttle = 0 time.sleep(5.0) spd_servo.throttle = -0.5
