def __init__(self):
"""Constructs Arm class.
"""
servo_info = {}
servo_info['s1'] = {
'function': 'waist',
'default_value': 90.0,
'min_value': 0.0,
'max_value': 180.0
}
servo_info['s2'] = {
'function': 'shoulder',
'default_value': 150.0,
'min_value': 0.0,
'max_value': 180.0
}
servo_info['s3'] = {
'function': 'elbow',
'default_value': 35.0,
'min_value': 0.0,
'max_value': 180.0
}
servo_info['s4'] = {
'function': 'wrist_roll',
'default_value': 140.0,
'min_value': 0.0,
'max_value': 180.0
}
servo_info['s5'] = {
'function': 'wrist_pitch',
'default_value': 85.0,
'min_value': 0.0,
'max_value': 180.0
}
servo_info['s6'] = {
'function': 'grip',
'default_value': 80.0,
'min_value': 0.0,
'max_value': 180.0
}
self._servo_info = servo_info
segment_info = {}
segment_info['seg1'] = {
'base_servo': 's1',
'segment_length': 1.0,
'axis_of_rotation': 'Z'
}
segment_info['seg2'] = {
'base_servo': 's2',
'segment_length': 120.0,
'axis_of_rotation': 'Y'
}
segment_info['seg3'] = {
'base_servo': 's3',
'segment_length': 1.0,
'axis_of_rotation': 'Y'
}
segment_info['seg4'] = {
'base_servo': 's4',
'segment_length': 1.0,
'axis_of_rotation': 'X'
}
segment_info['seg5'] = {
'base_servo': 's5',
'segment_length': 1.0,
'axis_of_rotation': 'Y'
}
self._segment_info = arm_info
current_angles = {}
current_angles['s1'] = 0.0
current_angles['s2'] = 0.0
current_angles['s3'] = 0.0
current_angles['s4'] = 0.0
current_angles['s5'] = 0.0
current_angles['s6'] = 0.0
self._current_angles = current_angles
self._servo_speed = 1.0
self._solver = Solver(servo_info, segment_info)
self._kit = ServoKit(channels=16)
self.configure_board()
import pygame import sys from adafruit_servokit import ServoKit import os # set path to current dir for csv file sleep(1) os.environ["SDL_VIDEODRIVER"] = "dummy" #controller Disconnect controller = ControllerInput() # setting 16 channels for hat as well as i2c address to 60 kit = ServoKit(channels=16, address=96) #Pinout map ''' 0 left motor 1 right motor 2 left motor 2 3 right motor 2 4 arm motor 1 5 arm motor 2 6 servo 1 7 servo 2test '''
def configPWMBoard(pwmConfig):
return ServoKit(channels=pwmConfig.channels,
address=pwmConfig.address,
frequency=pwmConfig.frequency)
"""
This is the test script and diary of getting the servo hat to work.
For starters we're following this tutorial.
https://learn.adafruit.com/adafruit-16-channel-pwm-servo-hat-for-raspberry-pi/using-the-python-library
2021-10-31, JDL: Retry after earlier setbacks.
"""
from adafruit_servokit import ServoKit
import time
kit = ServoKit(channels=16, address=0x41) # Don't forget we moved the address.
servo_loop_quit = False
while servo_loop_quit != True:
print('Want to move the arms? (y/n)')
user_input = input()
try:
if user_input == 'y':
user_input = input("0 or 1?")
if int(user_input) == 0:
ZeroArmAngle = int(input("Angle of servo 0?"))
kit.servo[0].angle = ZeroArmAngle
def init_kit(channels, pulse_width_range):
kit = ServoKit(channels=channels)
for i in range(channels):
kit.servo[i].set_pulse_width_range(*pulse_width_range)
kit.servo[i].throttle = 1
from time import sleep
from adafruit_servokit import ServoKit
from ultrasonic_sensors_driver import Ultrasonic_driver
kit = ServoKit(channels=8) #we dont need 16
#BCM
gpio_list_trigger = [14, 18]
gpio_list_echo = [4, 17]
#BOARD
# gpio_list_trigger = [19,21]
# gpio_list_echo = [24,26]
positions = ["r_left", "r_right"]
sensors = Ultrasonic_driver(gpio_list_trigger, gpio_list_echo, positions)
for t in range(10):
sleep(.5)
for i in range(180):
kit.servo[0].angle = i
# print(sensors.distance_all())
# sleep(.01)
sleep(.5)
for i in range(180, 0, -1):
kit.servo[0].angle = i
# print(sensors.distance_all())
# sleep(.01)
kit.servo[0].angle = 0
from adafruit_servokit import ServoKit
kit = ServoKit(channels = 8)
import adafruit_motor.servo
# sudo pip3 install adafruit-circuitpython-pca9685
# sudo pip3 install adafruit-circuitpython-servokit
class ServoModel():
'''
Class to contain servo model and presets and to move servo.
'''
def __init__(self,name,channel,presetDict={'home':0},minAngle=0,maxAngle=180):
#self.servo = kit.servo[channel]
self.servo = adafruit_motor.servo.Servo(channel)
self.name = name
self.presetDict = presetDict
self.minAngle = minAngle
self.maxAngle = maxAngle
self.servo.actuation_angle = maxAngle #set max angle in the kit.servo controller
def updateAbsoluteAngle(self,angle):
#Ensure angle remains in the range
if angle < self.minAngle:
angle = self.minAngle
elif angle > self.maxAngle:
angle = self.maxAngle
from adafruit_servokit import ServoKit
import time
import RPi.GPIO as GPIO
ledPin = 'GPIO_PZ0' #31
GPIO.setup(ledPin, GPIO.OUT) #Led
perchPin = 'GPIO_PE6' #32
GPIO.setup(perchPin, GPIO.IN) #Perch
buttonPhasePin = 'LCD_TE' #15
GPIO.setup(buttonPhasePin, GPIO.IN) #buttonPhase
kit = ServoKit(channels=16) #3 SDA, 5 SCL
phase = 1
INITIALSERVOPOSITION = 0
FINALSERVOPOSITION = 90
currentServoPosition = 0
step = 15
def IsBirdOnPerch(perch):
if perch == True:
return True
else:
return False
def OpenServo():
import RPi.GPIO as GPIO import time from adafruit_servokit import ServoKit kit = ServoKit(channels=16, address=65) #i2c address 1x41 count = 0 #Hat pins for Servos #0-Bread Pusher #1-Left Condiment Holder #2-Right Condiment Holder #3-Condiment Spreader #4-Toaster Primer #5-Toater Rotation #6-Top Bread Chute #7-Botton Bread Chute #GPIO pins of the buttons (in BCM) button1 = 5 button2 = 6 button3 = 13 button4 = 19 button5 = 26 button6 = 20 #list of all buttons button_list = [button1, button2, button3, button4, button5, button6] #breakfast choices choiceA = ""
def __init__(self):
# self.i2c = busio.I2C(board.SCL, board.SDA)
# self.pca = adafruit_pca9685.PCA9685(self.i2c)
self.kit = ServoKit(channels=16)
for i in range(4):
self.kit.servo[i].set_pulse_width_range(1000, 2000)
import time from adafruit_servokit import ServoKit board1 = ServoKit(channels=16) #board2 = ServoKit(channels=16, address=0x40) board1.servo[0].angle = 90
#!/usr/bin/env python3
# Rasitha Fernando
# 12/05/2019
import rospy
from std_msgs.msg import String
import time
import signal
from adafruit_servokit import ServoKit
import board
import busio
i2c_bus0 = (busio.I2C(board.SCL_1, board.SDA_1))
kit = ServoKit(channels=16, i2c=i2c_bus0)
kit.continuous_servo[3].throttle = 0.0
time.sleep(1)
kit.continuous_servo[3].throttle = 0.1
time.sleep(1)
kit.continuous_servo[3].throttle = 0.15
time.sleep(1)
kit.continuous_servo[3].throttle = 0.2
time.sleep(1)
kit.continuous_servo[3].throttle = 0.0
#------------------------------------------------------
def callback(data):
flag = 0
dec = int(data.data)
kit.servo[4].angle = dec
def __init__(self, IOConfig):
self.controller = ServoKit(channels=16)
self.IOConfig = IOConfig
self.servoList = dict()
for i in range(len(self.IOConfig.servoDict)):
self.servoList[i] = self.IOConfig.servoDict[i]['channel']
from adafruit_servokit import ServoKit
import board
import busio
import time
# On the Jetson Nano
# Bus 0 (pins 28,27) is board SCL_1, SDA_1 in the jetson board definition file
# Bus 1 (pins 5, 3) is board SCL, SDA in the jetson definition file
# Default is to Bus 1; We are using Bus 0, so we need to construct the busio first ...
print("Initializing Servos")
i2c_bus0 = (busio.I2C(board.SCL_1, board.SDA_1))
print("Initializing ServoKit")
kit = list()
kit.append(ServoKit(channels=16, i2c=i2c_bus0, address=0x40))
# kit.append(ServoKit(channels=16, i2c=i2c_bus0, address=0x41))
# kit[0] is the front servos
# kit[1] is the rear servos
print("Done initializing")
# [0]~[2] : FL // [3]~[5] : FR // [6]~[8] : RL // [9]~[11] : RR
val_list = [90]
# val_list = [90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90]
if __name__ == '__main__':
for x in range(len(val_list)):
kit[int(x / 6)].servo[int(x % 6)].angle = val_list[x]
while True:
# num is index of motor to rotate
def __init__(self, name, channel, default_angle):
self.name = name
self.channel = channel
self.angle = default_angle
self.kit = ServoKit(channels=16)
#sudo apt-get install python.smbus #sudo apt-get install i2c-tools #sudo pip3 install adafruit-circuitpython-servokit import time import random from adafruit_servokit import ServoKit import board import busio import adafruit_pca9685 i2c = busio.I2C(board.SCL, board.SDA) hat = adafruit_pca9685.PCA9685(i2c) kit = ServoKit(channels=16) #, address=0x40, reference_clock_speed=25000000) kit.servo[0].angle = 0 kit.servo[1].angle = 0 time.sleep(5) time.sleep(1) kit.servo[0].angle = 10 kit.servo[1].angle = 15 time.sleep(1) kit.servo[0].angle = 20 kit.servo[1].angle = 25 time.sleep(1) kit.servo[0].angle = 30 kit.servo[1].angle = 45 time.sleep(1) kit.servo[0].angle = 40 kit.servo[1].angle = 65
# 172.20.10.2 on iPhone
from xbox_controller import XboxController
from debounce import debounce
from robot2 import Robot
import actuation
import time
from adafruit_servokit import ServoKit
boards = [
ServoKit(channels=16, address=0x40),
ServoKit(channels=16, address=0x41)
]
def setup(legs):
for leg in legs:
for joint in leg.joints:
boards[joint.board].servo[joint.channel].set_pulse_width_range(
joint.minPulse, joint.maxPulse)
boards[joint.board].servo[joint.channel].angle = joint.startAngle
def actuate(legs):
for leg in legs:
for joint in leg.joints:
boards[joint.board].servo[joint.channel].angle = joint.currAngle
moveTimeout = 1
raiseLowerTimeout = 0.5
def __init__(self):
self.kit = ServoKit(channels = 16)
self.kit.servo[0].set_pulse_width_range(660, 2300)
self.kit.servo[1].set_pulse_width_range(610, 2250)
self.kit.servo[3].set_pulse_width_range(400, 2450)
self.kit.servo[2].set_pulse_width_range(550, 2250)
#!/usr/bin/env python3
# Python ROS
import rospy
from dimos_test.msg import Sonars
from time import sleep
from adafruit_servokit import ServoKit
kit1 = ServoKit(channels=16, address=0x40)
kit2 = ServoKit(channels=16, address=0x41)
# kit1 3 left legs : front leg ch 0 , 1, 2 , middle ch :3 ,4 ,5 back ch 6 ,7 ,8
# kit2 3 right legs : front leg ch 0 , 1, 2 , middle ch :3 ,4 ,5 back ch 6 ,7 ,8
#
#
joint_properties = {
'LFH': (0, 100, 0, -1),
'LFK': (1, 70, 0, -1),
'LFA': (2, 180, 0, 1),
'RFH': (3, 90, 0, 1),
'RFK': (4, 120, 0, 1),
'RFA': (5, 130, 610, -1),
'LMH': (6, 320, 470, -1),
'LMK': (7, 251, 531, -1),
'LMA': (8, 130, 610, 1),
'RMH': (0, 380, 530, 1),
'RMK': (1, 290, 605, 1),
'RMA': (2, 150, 630, -1),
'LBH': (6, 350, 500, -1),
def turn_on_robot_locomotion():
kit = ServoKit(channels=16)
number_of_servos = 12
global robot_is_walking
robot_is_walking = False
global robot_is_stopping
robot_is_stopping = False
# Define hip positions
hip_center = 90
hip_forwards = 60
hip_backwards = 120
# Define knee positions
knee_center = 85
knee_up = 40
knee_down = 100
# Center the legs
robot_leg_functions.center_servos(hip_center, knee_center, kit)
# Pause before starting walk
# time.sleep(2)
# Phases
phase_duration = 0.5
phase_duration_max = 0.8
phase_duration_min = 0.2
phase = 0
# Walk forwards gait
walk_forwards_hip_phase_order = [
hip_center, hip_forwards, hip_center, hip_backwards
]
walk_forwards_knee_phase_order = [
knee_up, knee_center, knee_down, knee_center
]
# Smoothing 0 - ease both, 1 - ease out from current, 2 = ease in to next, 3 = linear
walk_forwards_hip_smooth = [2, 1, 2, 1]
walk_forwards_knee_smooth = [0, 0, 0, 0]
# Stopping gait raised leg
stop_raised_hip_phase_order = [-1, hip_center, hip_center, hip_center]
stop_raised_knee_phase_order = [knee_up, knee_up, knee_down, knee_center]
# Smoothing 0 - ease both, 1 - ease out from current, 2 = ease in to next, 3 = linear
stop_raised_hip_smooth = [2, 1, 0, 0]
stop_raised_knee_smooth = [2, 1, 0, 0]
# Stopping gait down leg
stop_down_hip_phase_order = [-1, -1, -1, hip_center]
stop_down_knee_phase_order = [-1, -1, knee_up, knee_center]
# Smoothing 0 - ease both, 1 - ease out from current, 2 = ease in to next, 3 = linear
stop_down_hip_smooth = [0, 0, 0, 0]
stop_down_knee_smooth = [0, 1, 2, 0]
# Set each servo parameters
servo_params = []
for servo in range(0, number_of_servos):
servo_params.append(0)
# Hip = True /// Set A = True /// Right = True
servo_params[0] = [True, True, True]
servo_params[1] = [True, False, True]
servo_params[2] = [True, True, True]
servo_params[3] = [True, False, False]
servo_params[4] = [True, True, False]
servo_params[5] = [True, False, False]
servo_params[6] = [False, True, True]
servo_params[7] = [False, False, True]
servo_params[8] = [False, True, True]
servo_params[9] = [False, False, False]
servo_params[10] = [False, True, False]
servo_params[11] = [False, False, False]
# Initialise the list of servo current angles
servo_current_position = []
for servo in range(0, number_of_servos):
servo_current_position.append(0)
# Set the starting angle for all of the hips
for hip in range(0, 6):
servo_current_position[hip] = hip_center
# Set the starting angle for all of the knees
for knee in range(6, 12):
servo_current_position[knee] = knee_center
# Initialise our list which stores the servo curves
servo_curves = []
for servo in range(0, number_of_servos):
servo_curves.append(0)
use_current_position = False
turning_speed_smooth = 0
print("Starting our loop!")
while True:
if robot_is_walking:
# Set our timer for the first loop
phase_start_time = time.time()
# Calculate the phase duration based on input
phase_range = phase_duration_max - phase_duration_min
phase_duration = (moving_speed * phase_range) + phase_duration_min
global turning_speed
for servo in range(
0, number_of_servos): # Generate curve for each servo
this_servo_current_position = servo_current_position[servo]
this_servo_params = servo_params[servo]
# servo number | start position | servo parameters | phase
servo_curves[
servo] = robot_leg_functions.generate_servo_movement_curve(
this_servo_current_position, this_servo_params, phase,
walk_forwards_hip_phase_order,
walk_forwards_hip_smooth,
walk_forwards_knee_phase_order,
walk_forwards_knee_smooth, phase_duration, hip_center,
knee_center, 2, turning_speed, use_current_position)
use_current_position = False
while True: # This loop cycles through each servo and moves it towards the target until the phase ends
# Sleep a bit so that we don't hammer the processor
time.sleep(0.005)
# Start a timer for the phase
current_time_from_zero = time.time() - phase_start_time
# Go through each servo
for servo in range(0, number_of_servos):
# Calculate how much we need to move based on time
angle_for_this_servo = servo_curves[servo].ease(
current_time_from_zero)
# Move the servo
kit.servo[servo].angle = angle_for_this_servo
# Record the current angle for each servo
servo_current_position[servo] = angle_for_this_servo
# When the phase ends
if phase_duration < current_time_from_zero:
# Reset the timer
phase_start_time = time.time()
# Move to the next phase
phase += 1
phase = phase % 4
# Break out and go to the next phase
break
if not robot_is_walking:
# Stop the robot
robot_is_stopping = True
break
if robot_is_stopping:
current_walking_phase = phase
phase = 0
use_current_position = True
# Check which phase we're in and which legs are up or down
if current_walking_phase == 0 or current_walking_phase == 1:
LegsWhichAreUp = True
LegsWhichAreDown = False
if current_walking_phase == 2 or current_walking_phase == 3:
LegsWhichAreUp = False
LegsWhichAreDown = True
# This is used to stop the steering
# turning_while_stopping = turning_speed
while True: # Cycle through each phase until the robot has finished stopping
if not robot_is_stopping:
break
# Set our timer for the first loop
phase_start_time = time.time()
for servo in range(
0, number_of_servos): # Generate curve for each servo
# If this servo is part of a set which is raised, used the raised stop gait
if servo_params[servo][1] == LegsWhichAreUp:
hip_phase_order = stop_raised_hip_phase_order
hip_smooth = stop_raised_hip_smooth
knee_phase_order = stop_raised_knee_phase_order
knee_smooth = stop_raised_knee_smooth
# If this servo is part of a set which is down, use the down stop gait
if servo_params[servo][1] == LegsWhichAreDown:
hip_phase_order = stop_down_hip_phase_order
hip_smooth = stop_down_hip_smooth
knee_phase_order = stop_down_knee_phase_order
knee_smooth = stop_down_knee_smooth
this_servo_current_position = servo_current_position[servo]
this_servo_params = servo_params[servo]
# servo number | start position | servo parameters | phase
servo_curves[
servo] = robot_leg_functions.generate_servo_movement_curve(
this_servo_current_position, this_servo_params,
phase, hip_phase_order, hip_smooth,
knee_phase_order, knee_smooth, phase_duration,
hip_center, knee_center, 0, 0,
use_current_position)
use_current_position = False
while True: # This loop cycles through each servo and moves it towards the target until the phase ends
# Sleep a bit so that we don't hammer the processor
time.sleep(0.005)
# Start a timer for the phase
current_time_from_zero = time.time() - phase_start_time
# Go through each servo
for servo in range(0, number_of_servos):
# Calculate how much we need to move based on time
angle_for_this_servo = servo_curves[servo].ease(
current_time_from_zero)
# Move the servo
kit.servo[servo].angle = angle_for_this_servo
# Record the current angle for each servo
servo_current_position[servo] = angle_for_this_servo
# When the phase ends
if phase_duration < current_time_from_zero:
# Reset the timer
phase_start_time = time.time()
if phase == 3:
robot_is_stopping = False
use_current_position = True
for servo in range(0, number_of_servos
): # Turn off all the servos
kit.servo[servo].angle = None
break
# Move to the next phase
phase += 1
break
def __init__(self, type, *args, **kwargs):
super(NvidiaRacecar, self).__init__(*args, **kwargs)
self.type = type
self.kit = ServoKit(channels=16, address=self.i2c_address)
self.steering_motor = self.kit.continuous_servo[self.steering_channel]
self.throttle_motor = self.kit.continuous_servo[self.throttle_channel]
"""Hi or Hola""" import time from adafruit_servokit import ServoKit import os # Set channels to the number of servo channels on your kit. # 8 for FeatherWing, 16 for Shield/HAT/Bonnet. kit = ServoKit(channels=16) kit = ServoKit(channels=16) kit.servo[12].angle = 0 #Servo right elbow initial position time.sleep(0.5) kit.servo[12].angle = 30 #Servo right shoulder up time.sleep(0.5) kit.servo[12].angle = 60 time.sleep(0.5) kit.servo[12].angle = 180 time.sleep(1) kit.servo[15].angle = 120 time.sleep(1) kit.servo[15].angle = 145 time.sleep(1) kit.servo[12].angle = 90 time.sleep(0.5) kit.servo[12].angle = 60 time.sleep(0.5) kit.servo[12].angle = 30 kit.servo[12].angle = 15 #Servo right elbow initial position
'LBK': (7, 110),
'LBA': (8, 0),
'RFH': (0, 100),
'RFK': (1, 70),
'RFA': (2, 180),
'RMH': (3, 100),
'RMK': (4, 70),
'RMA': (5, 180),
'RBH': (6, 100),
'RBK': (7, 70),
'RBA': (8, 180)
}
left_j = ['LFH', 'LFK', 'LFA', 'LMH', 'LMK', 'LMA', 'LBH', 'LBK', 'LBA']
kitL = ServoKit(channels=16, address=0x41)
kitR = ServoKit(channels=16, address=0x40)
class Hexapod:
def __init__(self):
self.left_front = Leg('left front', 'LFH', 'LFK', 'LFA')
self.right_front = Leg('right front', 'RFH', 'RFK', 'RFA')
self.left_middle = Leg('left middle', 'LMH', 'LMK', 'LMA')
self.right_middle = Leg('right middle', 'RMH', 'RMK', 'RMA')
self.left_back = Leg('left back', 'LBH', 'LBK', 'LBA')
self.right_back = Leg('right back', 'RBH', 'RBK', 'RBA')
self.legs = [
self.left_front, self.right_front, self.left_middle,
self.right_middle, self.left_back, self.right_back
from adafruit_servokit import ServoKit
import board
from adafruit_motor import servo, stepper, motor
_kit = ServoKit(channels=16, i2c=board.I2C())
# Thrusters are defined in Components.Drive
# MANIPULATOR
MANIP_ELBOW_SERVO_2 = _kit._items[8] = servo.Servo(_kit._pca.channels[8])
MANIP_ELBOW_SERVO = _kit._items[9] = servo.Servo(_kit._pca.channels[9])
MANIP_WRIST_SERVO = _kit._items[10] = servo.Servo(_kit._pca.channels[10])
MANIP_LEVEL_SERVO = _kit._items[11] = servo.Servo(_kit._pca.channels[11])
MANIP_CLAMP_SERVO = _kit._items[12] = servo.Servo(_kit._pca.channels[12])
MANIP_SERVOS = {
"MANIP_ELBOW_SERVO": MANIP_ELBOW_SERVO,
"MANIP_ELBOW_SERVO_2": MANIP_ELBOW_SERVO_2,
"MANIP_WRIST_SERVO": MANIP_WRIST_SERVO,
"MANIP_LEVEL_SERVO": MANIP_LEVEL_SERVO,
"MANIP_CLAMP_SERVO": MANIP_CLAMP_SERVO
}
# Manip Servo Mods (Rated pulse width) with modifications to fit our wanted sensitivity
MANIP_ELBOW_SERVO.set_pulse_width_range(500,2500)
MANIP_ELBOW_SERVO_2.set_pulse_width_range(500,2500)
MANIP_WRIST_SERVO.set_pulse_width_range(600,2400)
MANIP_LEVEL_SERVO.set_pulse_width_range(500,2500)
MANIP_CLAMP_SERVO.set_pulse_width_range(500,2500)
# Thruster Mods (Experimentally found pulse width because specs lied to us :) (1100->1900 base))
# (in reality its probably a library thing but i dont want to debug/rewrite servo.continuous_servo :P )
def __init__(self, num=16, mimimum_pulse=450, maximum_pulse=2450, kill_angle=90): self.kill_angle = kill_angle self.kit = ServoKit(channels=16) self.servos = [Servo(self.kit.servo[x], x) for x in range(16)] for servo in self.servos: servo.kit_servo.set_pulse_width_range(mimimum_pulse, maximum_pulse)
def __init__(self):
super().__init__()
'''
deeptcar module initialize
'''
self.flag = False
self.cv_mode = 0
self.motor = CobitCarMotorL9110()
self.servo = ServoKit(channels=16)
self.servo_offset = 0
self.cv_throttle = 0
self.dp_throttle = 0
self.driveFlag = False
'''
Window layout
'''
#elf.setGeometry(100,100, 800, 800)
self.setWindowTitle("Deeptcar Operation Demo")
# buttons - DC motor test
self.motor_test_btn = QPushButton(self)
self.motor_test_btn.setText("DC motor test")
self.motor_test_btn.clicked.connect(self.test_DC_motor)
self.motor_test_btn.setToolTip(
"Testing DC geared motor on-off 3 times")
# buttons - set steering servo at center
self.servo_center_btn = QPushButton(self)
self.servo_center_btn.setText("steering wheel center")
self.servo_center_btn.clicked.connect(self.servo_to_center)
self.servo_center_btn.setToolTip("Adjusting steering wheel to center")
# buttons - servo motor test
self.servo_test_btn = QPushButton(self)
self.servo_test_btn.setText("servo motor test")
self.servo_test_btn.clicked.connect(self.test_servo_motor)
self.servo_test_btn.setToolTip("Testing servo motor on-off 3 times")
# servo trim control
self.servo_trim_label = QLabel('0', self)
self.servo_trim_label.setAlignment(Qt.AlignCenter)
self.servo_trim_label.setMinimumWidth(100)
self.servo_trim_label.setText("0")
self.servo_trim_sld = QSlider(Qt.Horizontal, self)
self.servo_trim_sld.setRange(-20, 20)
self.servo_trim_sld.valueChanged[int].connect(self.sld_change_value)
# radio botton
self.radio_normal = QRadioButton("Normal camera view", self)
self.radio_normal.setChecked(True)
self.radio_normal.clicked.connect(self.cv_normal)
self.radio_mask = QRadioButton("Red color masking", self)
self.radio_mask.setChecked(False)
self.radio_mask.clicked.connect(self.cv_mask)
self.radio_edge = QRadioButton("Canny edge detect", self)
self.radio_edge.setChecked(False)
self.radio_edge.clicked.connect(self.cv_canny)
self.radio_crop = QRadioButton("Crop necessary region")
self.radio_crop.setChecked(False)
self.radio_crop.clicked.connect(self.cv_crop)
self.radio_detect_line = QRadioButton("Detect line segments")
self.radio_detect_line.setChecked(False)
self.radio_detect_line.clicked.connect(self.cv_detect)
self.radio_slope_lane = QRadioButton("Get lane slope")
self.radio_slope_lane.setChecked(False)
self.radio_slope_lane.clicked.connect(self.cv_slope)
self.radio_draw_steering = QRadioButton("Draw steering angle")
self.radio_draw_steering.setChecked(False)
self.radio_draw_steering.clicked.connect(self.cv_steering)
self.radio_deep_steering = QRadioButton("Draw deep steering angle")
self.radio_deep_steering.setChecked(False)
self.radio_deep_steering.clicked.connect(self.cv_deep_steering)
# create the label that holds the image
self.disply_width = 320
self.display_height = 240
self.image_label = QLabel(self)
self.image_label.move(0, 0)
self.image_label.resize(self.disply_width, self.display_height)
# buttons - OpenCV lane driving
self.cv_lane_drive_start_btn = QPushButton(self)
self.cv_lane_drive_start_btn.setText(
"OpenCV lane detect driving start")
self.cv_lane_drive_start_btn.clicked.connect(
self.opencv_lane_drive_start)
self.cv_lane_drive_start_btn.setToolTip(
"OpenCV lane detecting driving start")
# buttons - OpenCV lane driving
self.cv_lane_drive_stop_btn = QPushButton(self)
self.cv_lane_drive_stop_btn.setText("OpenCV lane detect driving stop")
self.cv_lane_drive_stop_btn.clicked.connect(
self.opencv_lane_drive_stop)
self.cv_lane_drive_stop_btn.setToolTip(
"OpenCV lane detecting driving stop")
# buttons - Deep lane driving
self.dp_lane_drive_start_btn = QPushButton(self)
self.dp_lane_drive_start_btn.setText(
"Deep learning lane detect driving start")
self.dp_lane_drive_start_btn.clicked.connect(
self.deep_lane_drive_start)
self.dp_lane_drive_start_btn.setToolTip(
"Deep leanring lane detecting driving start")
# buttons - Deep lane driving
self.dp_lane_drive_stop_btn = QPushButton(self)
self.dp_lane_drive_stop_btn.setText(
"Deep learning lane detect driving stop")
self.dp_lane_drive_stop_btn.clicked.connect(self.deep_lane_drive_stop)
self.dp_lane_drive_stop_btn.setToolTip(
"Deep learning lane detecting driving stop")
# cv throttle control
self.sld_cv_throttle = QSlider(Qt.Horizontal, self)
self.sld_cv_throttle.setRange(0, 50)
self.sld_cv_throttle.valueChanged[int].connect(
self.sld_cv_throttle_value)
# deep throttle label
self.cv_throttle_label = QLabel('0', self)
self.cv_throttle_label.setAlignment(Qt.AlignCenter)
self.cv_throttle_label.setMinimumWidth(100)
self.cv_throttle_label.setText("0")
# dp throttle control
self.sld_dp_throttle = QSlider(Qt.Horizontal, self)
self.sld_dp_throttle.setRange(0, 50)
self.sld_dp_throttle.valueChanged[int].connect(
self.sld_dp_throttle_value)
# deep throttle label
self.deep_throttle_label = QLabel('0', self)
self.deep_throttle_label.setAlignment(Qt.AlignCenter)
self.deep_throttle_label.setMinimumWidth(100)
self.deep_throttle_label.setText("0")
# create a vertical box layout and add the two labels
vbox = QVBoxLayout()
hbox = QHBoxLayout()
h_cv_box = QHBoxLayout()
v_cv_radio_box = QVBoxLayout()
v_cv_motor_box = QVBoxLayout()
v_cv_throttle_box = QVBoxLayout()
h_cv_motor_throttle_box = QHBoxLayout()
v_dp_motor_box = QVBoxLayout()
v_dp_throttle_box = QVBoxLayout()
h_dp_motor_throttle_box = QHBoxLayout()
vbox.addWidget(self.motor_test_btn)
vbox.addWidget(self.servo_center_btn)
hbox.addWidget(self.servo_trim_label)
hbox.addWidget(self.servo_trim_sld)
vbox.addLayout(hbox)
vbox.addWidget(self.servo_trim_sld)
vbox.addWidget(self.servo_test_btn)
v_cv_radio_box.addWidget(self.radio_normal)
v_cv_radio_box.addWidget(self.radio_mask)
v_cv_radio_box.addWidget(self.radio_edge)
v_cv_radio_box.addWidget(self.radio_crop)
v_cv_radio_box.addWidget(self.radio_detect_line)
v_cv_radio_box.addWidget(self.radio_slope_lane)
v_cv_radio_box.addWidget(self.radio_draw_steering)
v_cv_radio_box.addWidget(self.radio_deep_steering)
h_cv_box.addLayout(v_cv_radio_box)
h_cv_box.addWidget(self.image_label)
vbox.addLayout(h_cv_box)
v_cv_motor_box.addWidget(self.cv_lane_drive_start_btn)
v_cv_motor_box.addWidget(self.cv_lane_drive_stop_btn)
v_cv_throttle_box.addWidget(self.sld_cv_throttle)
v_cv_throttle_box.addWidget(self.cv_throttle_label)
h_cv_motor_throttle_box.addLayout(v_cv_motor_box)
h_cv_motor_throttle_box.addLayout(v_cv_throttle_box)
v_dp_motor_box.addWidget(self.dp_lane_drive_start_btn)
v_dp_motor_box.addWidget(self.dp_lane_drive_stop_btn)
v_dp_throttle_box.addWidget(self.sld_dp_throttle)
v_dp_throttle_box.addWidget(self.deep_throttle_label)
h_dp_motor_throttle_box.addLayout(v_dp_motor_box)
h_dp_motor_throttle_box.addLayout(v_dp_throttle_box)
vbox.addLayout(h_cv_motor_throttle_box)
vbox.addLayout(h_dp_motor_throttle_box)
vbox.addWidget(self.dp_lane_drive_start_btn)
vbox.addWidget(self.dp_lane_drive_stop_btn)
# set the vbox layout as the widgets layout
self.setLayout(vbox)
# create the video capture thread
self.thread = VideoThread()
# connect its signal to the update_image slot
self.thread.change_pixmap_signal.connect(self.update_image)
# start the thread
self.thread.start()
import time
import RPi.GPIO as GPIO
from adafruit_servokit import ServoKit
'''GPIO.setmode(GPIO.BCM)
GPIO.setup(11,GPIO.OUT)
servo1=GPIO.PWM(11,50)
servo1.start(2)'''
h = ServoKit(channels=16)
#servo1.ChangeDutyCycle(12)
#kit.servo[0].angle
init = [0, 90, 20, 0, 180, 160, 170, 180, 60, 0, 0, 150]
limitLo = [0, 0, 20, 0, 0, 40, 0, 0, 60, 0, 0, 30]
limitHi = [35, 180, 180, 180, 180, 160, 170, 180, 180, 180, 180, 150]
cur = init
def changeDeg(pin, newDegree):
maxChange = 0
pinSize = len(pin)
for i in range(0, pinSize):
maxChange = max(abs(cur[pin[i]] - newDegree[i]), maxChange)
for deg in range(0, maxChange, 5):
for i in range(0, pinSize):
if cur[pin[i]] < newDegree[i]:
cur[pin[i]] += 5
elif cur[pin[i]] > newDegree[i]:
cur[pin[i]] -= 5
# import libraries import board # stepper import RPi.GPIO as GPIO import time import random from pygame import mixer # sound # from adafruit_motor import stepper # stepper # from adafruit_motorkit import MotorKit # stepper from adafruit_servokit import ServoKit # servo # Set channels to the number of servo channels on your kit. # 8 for FeatherWing, 16 for Shield/HAT/Bonnet. # address = siehe Nummer auf Servo Hat => 0x[Nummer] # kit = ServoKit(channels=16) kit46 = ServoKit(address=0x46, channels=16) kit47 = ServoKit(address=0x47, channels=16) # servo kit 0x47 Pins: # 0-2 -> Rumpf # 3 -> Arm # 4 -> Hand # 5 -> Kopf #servo kit 0x46 PINs: # 0/2/4 -> Rumpf # 6 -> Arm # 8 -> Kopf # Parameter Rumpf47 Rumpf47_sek = .05 # Wartezeit nach Winkel anfahren defnieren Rumpf47_maxValue = 28 # max value for range
