import RPi.GPIO as GPIO
import array
from adafruit_servokit import ServoKit
kit = ServoKit(channels=16) #Activate all the channels
#GPIO.setmode(GPIO.BCM)
#Maximun range from 550 to 2400
#Recommended library 700 to 2380
#Best for some of them 550 2400
#Suitable for the sixteen 550 to 2300
##Definitions for movement
m1=array.array('i',[0,0,0,0,26,13,12,6,21,20,19,16])
for i in range(0,16): #Declaration for the servomotors and gearmotors
if(i>3 and i<12): #Motors
kit.servo[i].set_pulse_width_range(0,19988)
kit.servo[i].actuation_range = 100
else: #Servomotors
kit.servo[i].set_pulse_width_range(550,2300)
kit.servo[i].actuation_range = 190
for j in range(len(m1)): #Output declaration for the GearMotors
GPIO.setup(m1[j],GPIO.OUT)
def motor(dir,number,speed):
if (number>3 and number<12):
kit.servo[number].angle = speed
else:
print('The gearmotor does not exist')
import numpy as np import time from adafruit_servokit import ServoKit print(cv.__version__) timeMark = time.time() # 时间标记 dtFIL = 0 # 低通滤波，表示时间变化 width = 640 # 800 640 1280 1920 height = 480 # 600 480 720 1080 flip = 2 # 设置翻转 font = cv.FONT_HERSHEY_SIMPLEX # 设置字体 kit = ServoKit(channels=16) # 视频驱动通道有16个 tilt = 90 # 倾斜角度 pan = 90 # 水平角度 dTilt = 10 # 移动刻度 dPan = 1 # 移动刻度 kit.servo[0].angle = pan # 水平舵机-cam1 kit.servo[1].angle = tilt # 倾斜舵机-cam1 kit.servo[2].angle = pan # 水平舵机-cam2 kit.servo[3].angle = tilt # 倾斜舵机-cam2 # G streamer 是从摄像机源到显示器源的通道 # 树莓派摄像头捕获,用！隔开命令;
from adafruit_servokit import ServoKit
from requests import get
import json
settings = {
'api_key': 'your_api_key',
'zip_code': '94112',
'country_code': 'us',
'temp_unit': 'imperial'
} #unit can be metric, imperial, or kelvin
BASE_URL = "http://api.openweathermap.org/data/2.5/weather?appid={0}&zip={1},{2}&units={3}"
# Set channels to the number of servo channels on your kit.
# 8 for FeatherWing, 16 for Shield/HAT/Bonnet.
kit = ServoKit(channels=16)
def arm_down():
kit.servo[3].angle = 180 #handclosed
kit.servo[2].angle = 0
kit.servo[1].angle = 180
kit.servo[0].angle = 90
time.sleep(10)
def arm_up():
kit.servo[3].angle = 180 #handclosed
kit.servo[2].angle = 0
kit.servo[1].angle = 90 #move arm vertically
kit.servo[0].angle = 90
def main():
kit = ServoKit(channels=16)
t = threading.Thread(target=worker)
t.start()
timestamp = time.clock()
drive_pins = DrivePins(pwm1=yoda_pins.PWM1,
pwm2=yoda_pins.PWM2,
in1=yoda_pins.IN1,
in2=yoda_pins.IN2,
in3=yoda_pins.IN3,
in4=yoda_pins.IN4)
GPIO.setmode(GPIO.BCM)
# Setup for sonar.
GPIO.setup(yoda_pins.TRIG, GPIO.OUT)
GPIO.setup(yoda_pins.ECHO, GPIO.IN)
# sonar_thread = multiprocessing.Process(target=sonar_worker)
# sonar_thread.start()
# Setup for drive.
GPIO.setup(drive_pins.pwm1, GPIO.OUT)
GPIO.setup(drive_pins.pwm2, GPIO.OUT)
GPIO.setup(drive_pins.in1, GPIO.OUT)
GPIO.setup(drive_pins.in2, GPIO.OUT)
GPIO.setup(drive_pins.in3, GPIO.OUT)
GPIO.setup(drive_pins.in4, GPIO.OUT)
# Default to drive forward
GPIO.output(drive_pins.in1, GPIO.LOW)
GPIO.output(drive_pins.in2, GPIO.HIGH)
GPIO.output(drive_pins.in3, GPIO.HIGH)
GPIO.output(drive_pins.in4, GPIO.LOW)
pwm_freq = 100
max_duty = 50
left = GPIO.PWM(drive_pins.pwm1, pwm_freq)
right = GPIO.PWM(drive_pins.pwm2, pwm_freq)
left.start(0) # Start at 0% duty cycle
right.start(0) # Start at 0% duty cycle
max_accel = 6000
left_drive_commands = DriveCommands(current_command=0,
previous_command=0,
dt=0.01,
accel_limit=max_accel)
right_drive_commands = DriveCommands(current_command=0,
previous_command=0,
dt=0.01,
accel_limit=max_accel)
all_commands = AllCommands(left=left_drive_commands,
right=right_drive_commands)
left_command = 0
right_command = 0
limit_rate = 0.75
while True:
# print(sonar_distance_mm)
if event_deque:
commands = event_deque.popleft()
left_command = commands[0]
right_command = commands[1]
# print(f"left: {left_command} right: {right_command}")
# Writes at 100Hz
if (time.clock() - timestamp) >= 0.01:
sonar_distance_mm = get_sonar()
all_commands = DriveAuto(right_command * limit_rate,
left_command * limit_rate, left, right,
drive_pins, all_commands,
sonar_distance_mm)
def __init__(self):
# Load parameters
subname = "janus" #TODO unhardcode
# Initalise I2C
self.i2c = busio.I2C(board.SCL, board.SDA)
# Initalise Depth Sensor
self.C = initalizePressureSensor(self.i2c)
self.pub_odom = rospy.Publisher('depth_odom',
PoseWithCovarianceStamped,
queue_size=1)
self.pub_map = rospy.Publisher('depth_map',
PoseWithCovarianceStamped,
queue_size=1)
self.pub_odom_data = PoseWithCovarianceStamped()
self.pub_odom_data.pose.covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.pub_odom_data.header.frame_id = subname + "/description/depth_odom_frame"
self.pub_map_data = PoseWithCovarianceStamped()
self.pub_map_data.pose.covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.pub_map_data.header.frame_id = subname + "/description/depth_map_frame"
# Initalise IMU
self.imu = adafruit_bno055.BNO055(self.i2c)
# Setup orientation
self.imu.mode = adafruit_bno055.CONFIG_MODE
# AXIS_REMAP_CONFIG AXIS_REMAP_SIGN
# ADDRESS 0x41 0x42
# VALUE 0x21 0x01
self.imu._write_register(0x41, 0x24)
self.imu._write_register(0x42, 0x03)
self.imu.mode = adafruit_bno055.NDOF_MODE
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.imu_data = Imu()
self.imu_data.header.frame_id = subname + "/description/imu_link"
self.imu_data.orientation_covariance = [
1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6
]
self.imu_data.angular_velocity_covariance = [
1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6
]
self.imu_data.linear_acceleration_covariance = [
1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6
]
self.imu_temp_pub = rospy.Publisher('imu_temp_c',
Float64,
queue_size=1)
self.imu_temp_data = Float64()
# Initalise PWM Controller
self.motor = ServoKit(channels=8)
rospy.loginfo("Initalizing ECSs...")
for motor_name in motor_map.keys():
motor_id = motor_map[motor_name]
rospy.loginfo(motor_name)
# We have to shift the pulse range from (1100, 1900) because
# the PCA9685 produces 1440uS pulses at 0.5 throttle using
# the correct range
self.motor.servo[motor_id].set_pulse_width_range(1160, 1960)
time.sleep(0.2)
self.motor_command_sub = rospy.Subscriber(
'motor_controllers/pololu_control/command',
Float64MultiArray,
self.motor_command_callback,
queue_size=1)
# Initalize Battery Monitor
# Create the ADC object using the I2C bus
ads = ADS.ADS1115(self.i2c)
# Create single-ended input on channel 0
self.battery_monitor_channel = AnalogIn(ads, ADS.P0)
self.bat_pub = rospy.Publisher('battery_voltage',
Float64,
queue_size=1)
self.bat_data = Float64()
# Start Battery Voltage Publisher
battery_thread = threading.Thread(target=self.battery_publisher)
battery_thread.start()
# Setup Depth calibrator
depth_offset_data = None
try:
with open("depth_offset.yaml", "r") as stream:
depth_offset_data = yaml.safe_load(stream)
rospy.set_param('/janus/depth_offset',
depth_offset_data['depth_offset'])
except:
rospy.logwarn("No depth saved!")
# service to activate depth calibration
s = rospy.Service('calibrate_depth', CalibrateDepth,
self.calibrate_depth)
# Start Depth Publisher
depth_thread = threading.Thread(target=self.depth_publisher)
depth_thread.start()
# Start IMU Publisher
imu_thread = threading.Thread(target=self.imu_publisher)
imu_thread.start()
rospy.loginfo("Initalization Complete")
# Run until we kill the core
rospy.spin()
70: 70 --
'''
import time
import sys
from adafruit_servokit import ServoKit
# setup
chnl = 16
i2cv = None
addr = 64
clck = 25000000
freq = 50
PCA9685 = ServoKit(channels=chnl,
i2c=i2cv,
address=addr,
reference_clock_speed=clck,
frequency=freq)
servos = [ #[ pwmin, pwmax, angmin, angmax, pcfreq, 'type' ]
[1, [500, 2500, 0, 180, 20, '4.8-6v', 'MG90S AranaCorp, Towerpro ']],
[2, [900, 2100, 0, 102, 20, '4.8-6v', 'HS-55 SubMicro, Gripper']],
[3, [900, 2100, 0, 165, 20, '4.8-6v', 'HS-81 Micro, sample']],
[4, [900, 2100, 0, 180, 20, '4.8-6v', 'HS311 Standard, Gripper']],
[5, [1000, 2000, 0, 180, 20, '4.8-6v', 'MG995 armature, TowerPro']],
[6, [500, 2500, 0, 180, 20, '3-7.2v', 'MG995 armature, TianKongRC']]
]
channelsUsed = 3
timeSlices = 0.01
angIncrements = 1
#!/usr/bin/env python3
# Python ROS
#import rospy
from time import sleep
from adafruit_servokit import ServoKit
kitL = ServoKit(channels=16, address=0x40)
kitR = 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
jp = {
'LFH': (0, 100),
'LFK': (1, 110),
'LFA': (2, 0),
'LMH': (3, 100),
'LMK': (4, 110),
'LMA': (5, 0),
'LBH': (6, 100),
'LBK': (7, 110),
'LBA': (8, 0),
'RFH': (0, 100),
'RFK': (1, 70),
'RFA': (2, 180),
'RMH': (3, 100),
'RMK': (4, 70),
import cv2
from adafruit_servokit import ServoKit
from cobit_opencv_lane_detect import CobitOpencvLaneDetect
from cobit_car_motor_l9110 import CobitCarMotorL9110
cv_detector = CobitOpencvLaneDetect()
motor = CobitCarMotorL9110()
servo = ServoKit(channels=16)
SCREEN_WIDTH = 320
SCREEN_HEIGHT = 240
cap = cv2.VideoCapture(0)
cap.set(3, int(SCREEN_WIDTH))
cap.set(4, int(SCREEN_HEIGHT))
# Below code works normally for Pi camera V2.1
# But for ELP webcam, it doesn't work.
#fourcc = cv2.VideoWriter_fourcc(*'XVID')
fourcc = cv2.VideoWriter_fourcc('M','J','P','G')
video_orig = cv2.VideoWriter('./data/car_video.avi', fourcc, 20.0, (SCREEN_WIDTH, SCREEN_HEIGHT))
#video_lane = cv2.VideoWriter('./data/car_video_lane.avi', fourcc, 20.0, (SCREEN_WIDTH, SCREEN_HEIGHT))
servo_offset = 15
for i in range(3):
from adafruit_servokit import ServoKit
import sys
kit = ServoKit(channels=16)
kit.frequency = 50
while True:
data = input().split(':')
kit.servo[int(data[0])].angle = int(data[1])
def __init__(self):
self.kit = kit = ServoKit(channels=16)
self.alpha = 0 # degrees for base servo
self.beta = 0 # degrees for main arm servo
self.gamma = 0 # degrees for side arm servo
self.delta = 0 # degrees for clamp servo
import time
from adafruit_servokit import ServoKit
kit = [
ServoKit(address=0x40, channels=16),
ServoKit(address=0x41, channels=16)
]
for c in range(16):
kit[0].servo[c].actuation_range = 270
kit[0].servo[c].set_pulse_width_range(500, 2500)
kit[1].servo[c].actuation_range = 270
kit[1].servo[c].set_pulse_width_range(500, 2500)
speed = 0.125
RB = []
RB.extend((kit[0].servo[0], kit[0].servo[1], kit[0].servo[2]))
RM = []
RM.extend((kit[0].servo[3], kit[0].servo[4], kit[0].servo[5]))
RF = []
RF.extend((kit[0].servo[6], kit[0].servo[7], kit[0].servo[8]))
LF = []
LF.extend((kit[1].servo[0], kit[1].servo[1], kit[1].servo[2]))
LM = []
LM.extend((kit[1].servo[3], kit[1].servo[4], kit[1].servo[5]))
LB = []
LB.extend((kit[1].servo[6], kit[1].servo[7], kit[1].servo[8]))
joint0 = [LB[0], LM[0], LF[0], RB[0], RM[0], RF[0]]
from adafruit_servokit import ServoKit
from adafruit_motorkit import MotorKit
from inputs import get_gamepad
throttle_gain = 0.75
steering_offset = 0.25 #not used yet
import time
motor = MotorKit()
steering = ServoKit(channels=16)
#when you run the program, it will take around 10s to initialize everything
def TeleOp():
while 1:
events = get_gamepad()
for event in events:
if event.code != 'ABS_X':
pass #this may not be the 'correct' way to do this, but this makes the steering servo ignore any other inputs
else:
steeringvalue = event.state / 1.43 #for scaling the 0-255 axis range to a 0-180 degree scale
steering.servo[0].angle = steeringvalue
print(steeringvalue)
if event.code != 'BTN_TRIGGER':
pass
else:
fwdthrottlevalue = event.state * throttle_gain
motor.motor3.throttle = fwdthrottlevalue
print(fwdthrottlevalue)
def do_something(logf, configf):
'''
Main routine.
'''
#
# setup logging
#
logger = logging.getLogger('weasleyclock')
logger.setLevel(logging.INFO)
formatstr = '%(asctime)s - %(name)s - %(levelname)s - %(message)s'
formatter = logging.Formatter(formatstr)
handler = logging.handlers.RotatingFileHandler(logf,
maxBytes=1049600,
backupCount=10)
handler.setLevel(logging.INFO)
handler.setFormatter(formatter)
logger.addHandler(handler)
# Old logger, without log rotation
# fh = logging.FileHandler(logf)
# fh.setLevel(logging.INFO)
# fh.setFormatter(formatter)
# logger.addHandler(fh)
# read config file
with open(configf) as json_data_file:
try:
config_data = json.load(json_data_file)
except json.JSONDecodeError as parse_error:
print("JSON decode failed. [" + parse_error.msg + "]")
print("error at pos: ", parse_error.pos, " line: ",
parse_error.lineno)
sys.exit(1)
# connect to MQTT server
host = config_data['mqtt_host']
port = config_data['mqtt_port'] if 'mqtt_port' in config_data else 8883
topic = config_data[
'mqtt_topic'] if 'mqtt_topic' in config_data else 'weasleyclock/#'
logger.info("connecting to host " + host + ":" + str(port) + " topic " +
topic)
if debug_p:
print("connecting to host " + host + ":" + str(port) + " topic " +
topic)
# configure servos and zero clock hands
kit = ServoKit(channels=16)
for (hand, servo) in config_data['channel'].items():
# default (good starting point for hs785hb servo)
pulsewidth_min = 685
pulsewidth_max = 2070
actuation_range = 2160
# get per servo/channel configuration
if 'channel_config' in config_data:
if str(servo) in config_data['channel_config']:
channel_config = config_data['channel_config'][str(servo)]
if 'pulsewidth_min' in channel_config:
pulsewidth_min = int(channel_config['pulsewidth_min'])
if 'pulsewidth_max' in channel_config:
pulsewidth_max = int(channel_config['pulsewidth_max'])
if 'actuation_range' in channel_config:
actuation_range = int(channel_config['actuation_range'])
kit.servo[servo].actuation_range = actuation_range
kit.servo[servo].set_pulse_width_range(pulsewidth_min, pulsewidth_max)
clockdata = {
'logger': logger,
'host': host,
'port': port,
'topic': topic,
'kit': kit,
'config_data': config_data,
}
# how to mqtt in python see https://pypi.org/project/paho-mqtt/
mqttc = mqtt.Client(client_id='weasleyclockd',
clean_session=True,
userdata=clockdata)
mqttc.username_pw_set(config_data['mqtt_user'],
config_data['mqtt_password'])
# create callbacks
mqttc.on_connect = on_connect
mqttc.on_message = on_message
if port == 4883 or port == 4884 or port == 8883 or port == 8884:
mqttc.tls_set('/etc/ssl/certs/ca-certificates.crt')
while (True):
try:
mqttc.connect(host, port, 60)
break
except Exception as e:
# Connection failure.
userdata['logger'].error("connect() failed: {}".format(e))
time.sleep(60)
mqttc.loop_forever()
import socket
import pickle
from adafruit_servokit import ServoKit
LOCALHOST = "0.0.0.0"
PORT = 6699
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.bind((LOCALHOST, PORT))
server.listen(1)
kit = ServoKit(channels=16, address=96)
print("Server started")
print("Waiting for client request..")
while True:
clientConnection, clientAddress = server.accept()
print("Connected clinet :", clientAddress)
data = clientConnection.recv(1024)
print("From Client :", data.decode())
clientConnection.send(bytes("Successfully Connected to Server!!", 'UTF-8'))
while True:
try:
data = clientConnection.recv(1024)
except:
kit.continuous_servo[2].throttle = -0.1
kit.continuous_servo[3].throttle = -0.1
kit.continuous_servo[4].throttle = -0.1
kit.continuous_servo[0].throttle = -0.1
kit.continuous_servo[1].throttle = -0.1
exit()
recd = pickle.loads(data)
def init():
global kit
kit = ServoKit(channels=16)
import time
from adafruit_servokit import ServoKit
yes = ServoKit(channels=16)
yes.servo[0].angle = 0
time.sleep(0.5)
for i in range(0, 35):
yes.servo[0].angle = i
time.sleep(0.5)
time.sleep(0.5)
for i in range(34, 0, -1):
yes.servo[0].angle = i
time.sleep(0.5)
print('done')
import busio
import time
import sys
def sweepTest():
sweep = range(0,180)
for degree in sweep :
kit.servo[15].angle=degree
# kit.servo[1].angle=degree
# time.sleep(0.01)
time.sleep(0.5)
sweep = range(180,0, -1)
for degree in sweep :
kit.servo[15].angle=degree
# 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 ...
#angle = int(sys.argv[1])
print("Initializing Servos")
i2c_bus0=(busio.I2C(board.SCL_1, board.SDA_1))
print("Initializing ServoKit")
kit = ServoKit(channels=16, i2c=i2c_bus0)
# kit[0] is the bottom servo
# kit[1] is the top servo
print("Done initializing")
sweepTest()
#! /usr/bin/env python3
import board
import rospy
import busio
from adafruit_servokit import ServoKit
from arman_controller.srv import ArmanDriverRequest, ArmanDriverRequestResponse
i2c_addr = busio.I2C(board.SCL, board.SDA)
kit = ServoKit(channels=16, i2c=i2c_addr)
def update_joints(req):
kit.servo[0].angle = req.joint0
kit.servo[1].angle = req.joint1
kit.servo[2].angle = req.joint2
kit.servo[3].angle = req.joint3
kit.servo[4].angle = req.joint4
return ArmanDriverRequestResponse(1)
if __name__ == '__main__':
rospy.init_node('arman_driver')
s = rospy.service('arman_driver', ArmanDriverRequest, update_joints)
rospy.spin()
"""
@authors: Luis Juarez Mercedes
Pedro Vidal Arias
"""
"""
Enable Actuators
"""
from adafruit_servokit import ServoKit
motor = ServoKit(channels=16)
# Set pulse width range
motor.servo[0].set_pulse_width_range(600, 2400) # Gripper
motor.servo[1].set_pulse_width_range(500, 2500) # Axis1
motor.servo[2].set_pulse_width_range(500, 2500) # Axis2
motor.servo[3].set_pulse_width_range(500, 2500) # Axis3
motor.servo[4].set_pulse_width_range(500, 2500) # Axis4
motor.servo[5].set_pulse_width_range(500, 2500) # Axis5
motor.servo[6].set_pulse_width_range(500, 2500) # Axis6
# Set actuation range
motor.servo[0].actuation_range = 180 # Gripper
motor.servo[1].actuation_range = 270 # Axis1
motor.servo[2].actuation_range = 270 # Axis2
motor.servo[3].actuation_range = 270 # Axis3
motor.servo[4].actuation_range = 270 # Axis4
motor.servo[5].actuation_range = 270 # Axis5
motor.servo[6].actuation_range = 270 # Axis6
def robot_walk_forwards():
kit = ServoKit(channels=16)
number_of_servos = 12
global robot_is_walking
robot_is_walking = False
# Define hip positions
hip_center = 90
hip_forwards = 60
hip_backwards = 120
# Define knee positions
knee_center = 80
knee_up = 40
knee_down = 90
# Initialise the starting positions for the hips
hip_current_angle_a = hip_center
hip_current_angle_b = hip_center
knee_current_angle_a = knee_center
knee_current_angle_b = knee_center
# Center the legs
robot_leg_functions.center_servos(hip_center, knee_center, kit)
# Pause before starting walk
time.sleep(2)
# Phases
phase_duration = 0.3
pause_between_phases = 0
number_of_phases = 4
current_phase = 0
# Walk forwads 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
]
hip_set_a = [0, 2, 4]
hip_set_b = [1, 3, 5]
knee_set_a = [6, 8, 10]
knee_set_b = [7, 9, 11]
right_hips = [0, 1, 2]
left_hips = [3, 4, 5]
print("Starting our loop!")
while True:
if robot_is_walking:
# Reset our timer
phase_start_time = time.time()
for phase in range(number_of_phases):
# Generate smooth curves for 'set A' of legs
hip_curve_a = LinearInOut(
start=hip_current_angle_a,
end=walk_forwards_hip_phase_order[phase],
duration=phase_duration)
knee_curve_a = CubicEaseInOut(
start=knee_current_angle_a,
end=walk_forwards_knee_phase_order[phase],
duration=phase_duration)
# Advance the phase by 2 for the alternate legs
phase_b = phase + 2
if phase_b > number_of_phases - 1:
phase_b = phase_b - 4
# Generate smooth curves for 'set B' of legs
hip_curve_b = LinearInOut(
start=hip_current_angle_b,
end=walk_forwards_hip_phase_order[phase_b],
duration=phase_duration)
knee_curve_b = CubicEaseInOut(
start=knee_current_angle_b,
end=walk_forwards_knee_phase_order[phase_b],
duration=phase_duration)
# hip_curve = CubicEaseInOut(start=hip_current_angle, end=walk_forwards_hip_phase_order[phase], duration=phase_duration)
# knee_curve = CubicEaseInOut(start=knee_current_angle, end=walk_forwards_knee_phase_order[phase], duration=phase_duration)
while True:
# 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 hip in set A
for servo in hip_set_a:
# Calculate how much we need to move based on time
angle_for_this_servo = hip_curve_a.ease(
current_time_from_zero)
# If it's a left hip then flip the angle
if servo in left_hips:
hip_offset = angle_for_this_servo - hip_center
angle_for_this_servo = hip_center - hip_offset
# Move the hip
kit.servo[servo].angle = angle_for_this_servo
for servo in hip_set_b:
# Calculate how much we need to move based on time
angle_for_this_servo = hip_curve_b.ease(
current_time_from_zero)
# If it's a left hip then flip the angle
if servo in left_hips:
hip_offset = angle_for_this_servo - hip_center
angle_for_this_servo = hip_center - hip_offset
# Move the hip
kit.servo[servo].angle = angle_for_this_servo
# Calculate and move the knees
for servo in knee_set_a:
angle_for_this_servo = knee_curve_a.ease(
current_time_from_zero)
kit.servo[servo].angle = angle_for_this_servo
for servo in knee_set_b:
angle_for_this_servo = knee_curve_b.ease(
current_time_from_zero)
kit.servo[servo].angle = angle_for_this_servo
# When the phase ends
if phase_duration < current_time_from_zero or not robot_is_walking:
# Set the current angle to the target angle for each sevo type
hip_current_angle_a = walk_forwards_hip_phase_order[
phase]
hip_current_angle_b = walk_forwards_hip_phase_order[
phase_b]
knee_current_angle_a = walk_forwards_knee_phase_order[
phase]
knee_current_angle_b = walk_forwards_knee_phase_order[
phase_b]
# Reset the timer
phase_start_time = time.time()
# Break out and go to the next phase
break
from adafruit_servokit import ServoKit
from dcservo import DogCamServoBase
# Don't export ServoLib
__all__ = ("DogCamServoAda")
# Bring in global instance
ServoLib = ServoKit(channels=16)
class DogCamServoAda(DogCamServoBase):
def __init__(self,
InName,
InPin,
ZeroAngle=0.0,
Steps=1.0,
LowerBounds=0.0,
UpperBounds=180.0,
PulseWidthMin=1000,
PulseWidthMax=2000):
ServoLib.servo[InPin].actuation_range = UpperBounds
ServoLib.servo[InPin].set_pulse_width_range(PulseWidthMin,
PulseWidthMax)
super().__init__(InName,
InPin,
InZeroAngle=ZeroAngle,
InSteps=Steps,
InLowerBounds=LowerBounds,
InUpperBounds=UpperBounds)
class Order:
connectedServoAmount = 6
#[[sevo angle set as 0, translation direction(-1,+1)]]
angleTranslation = [[90,1],[90,1],[130,1],[40,1],[90,1],[0,1]]
kit = ServoKit(channels=16)
def __init__(self,orderId,movementTarget,animationDuration):
super().__init__()
#self.parkArm()
self.orderId = orderId
self.TargetPosition = []
self.interupt = False
self.animationDuration = animationDuration
self.movementTarget = movementTarget
self.orderComplete = False
def run(self):
#self.animateToPosition(self.movementTarget,self.animationDuration)
self.setPosition(self.movementTarget)
def parkArm(self):
parkAngles = [0,0,20,-10,0,70]
for i in range(self.connectedServoAmount):
self.kit.servo[i].angle = self.translateAngle(i,parkAngles[i])
def translateAngle(self,index,angle):
servoAngle= self.angleTranslation[index][0]+ angle*self.angleTranslation[index][1]
return servoAngle
def translateAngleArray(self,angleArray):
servoAngleArray=[]
for i in range(len(angleArray)):
servoAngleArray.append(self.translateAngle(i,angleArray[i]))
return servoAngleArray
def setPosition(self, angleArray):
for i in range(len(angleArray)):
self.kit.servo[i].angle = self.translateAngle(i,angleArray[i])
def getCurrentServoPositions(self):
servoPositions =[]
for i in range(self.connectedServoAmount):
servoPositions.append(self.kit.servo[i].angle)
return servoPositions
def animateToPosition(self,targetAngleArray,animationDuration):
servoTargets = self.translateAngleArray(targetAngleArray)
currentPositions = self.getCurrentServoPositions()
movementPerMs = []
for i in range(len(servoTargets)):
difference = servoTargets[i] - currentPositions[i]
movementPerMs.append(difference/animationDuration)
startTime = time.perf_counter()
while (time.perf_counter()-startTime)*10<animationDuration:
for j in range(len(movementPerMs)):
self.kit.servo[j].angle=self.kit.servo[j].angle+movementPerMs[j]
time.sleep(0.1)
self.orderComplete = True
print(f"Order {self.orderId} complete")
def __init__(self, *args, **kwargs):
super(NvidiaRacecar, self).__init__(*args, **kwargs)
self.kit = ServoKit(channels=16, address=self.i2c_address1)
self.motor = ServoKit(channels=16, address=self.i2c_address2)
self.motor._pca.frequency = 1600
self.steering_motor = self.kit.continuous_servo[self.steering_channel]
def __init__(self):
self.kit = ServoKit(channels=16)
self.arms = Arms(kit, ARM_CHANNEL_1, ARM_CHANNEL_2, ARM_PIN_1,
ARM_PIN_2)
self.base = Base(kit, BASE_CHANNEL_1, BASE_CHANNEL_2)
def __init__(self, *args, **kwargs):
super(NvidiaRacecar, self).__init__(*args, **kwargs)
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]
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]}
# """)
HAVE_SK = True
except ImportError:
HAVE_SK = False
import struct
import myo
from adafruit_motorkit import MotorKit
mkit = Motorkit()
stepper_position = 0
from adafruit_motor import stepper
from adafruit_servokit import ServoKit
skit = ServoKit(channels=16,address=0x41)
num_motor = 5
#Initialize Myo object
m = myo.Myo(myo.NNClassifier(), sys.argv[1] if len(sys.argv) >= 2 else None)
m.add_raw_pose_handler(print)
m.connect()
loop_num = 0
var = 1
while var == 1 :
m.run()
loop_num = loop_num+1;
def __init__(self, config):
self.output_mappings = {}
self.config = config
self.kit = ServoKit(channels=16)
#Libraries
import time
import sys
from adafruit_servokit import ServoKit
from time import sleep
#robotarm
#Constants
nbPCAServo = 3
#Parameters
MIN_IMP = [500, 500, 500]
MAX_IMP = [2500, 2500, 2500]
MIN_ANG = [0, 0, 0]
MAX_ANG = [180, 180, 180]
#Objects
pca = ServoKit(channels=16)
#while True:
# print('Distance to nearest object is', sensor.distance,'m')
# sleep(1)
def init():
for i in range(nbPCAServo):
pca.servo[i + 1].set_pulse_width_range(MIN_IMP[i], MAX_IMP[i])
# function main
def main():
global angle
def follower():
global send_status_flag, send_status_flag_lock
global direction, current_address, action, send_status_flag, command
global good_to_go_loading, good_to_go_unloading, get_drive, stop
global dash_file_name, error_type
def change_flag(flag):
if flag:
flag = False
else:
flag = True
return flag
def Motor_Steer(speed, steering, stop=False):
global ccw
# servo[0] -> left, 1 -> forward
# servo[1] -> right, -1 -> forward
if ccw:
param = 1.0
else:
param = 0.8
if stop == True:
kit.continuous_servo[0].throttle = 0
kit.continuous_servo[1].throttle = 0
return
elif steering == 0:
kit.continuous_servo[0].throttle = speed
kit.continuous_servo[1].throttle = -1 * speed * param
return
elif steering > 0:
steering = 100 - steering
kit.continuous_servo[0].throttle = speed
kit.continuous_servo[
1].throttle = -1 * speed * steering / 100 * param
return
elif steering < 0:
steering = steering * -1
steering = 100 - steering
kit.continuous_servo[0].throttle = speed * steering / 100
kit.continuous_servo[1].throttle = -1 * speed * param
return
def turn(ccw):
if ccw:
if address == 0:
kit.continuous_servo[0].throttle = 1
kit.continuous_servo[1].throttle = 1
time.sleep(1.15)
kit.continuous_servo[0].throttle = 0
kit.continuous_servo[1].throttle = 0
elif address == 5 or address == 2:
kit.continuous_servo[0].throttle = 1
kit.continuous_servo[1].throttle = 1
time.sleep(0.9)
elif address == 1:
kit.continuous_servo[0].throttle = -1
kit.continuous_servo[1].throttle = -1
time.sleep(0.9)
elif address == 3:
kit.continuous_servo[0].throttle = 1
kit.continuous_servo[1].throttle = 1
time.sleep(1.25)
else:
kit.continuous_servo[0].throttle = 1
kit.continuous_servo[1].throttle = 1
time.sleep(1.15)
else:
if address == 0:
kit.continuous_servo[0].throttle = -1
kit.continuous_servo[1].throttle = -1
time.sleep(1.12)
kit.continuous_servo[0].throttle = 0
kit.continuous_servo[1].throttle = 0
elif address == 4:
kit.continuous_servo[0].throttle = -1
kit.continuous_servo[1].throttle = -1
time.sleep(1.2)
elif address == 5:
kit.continuous_servo[0].throttle = -1
kit.continuous_servo[1].throttle = -1
time.sleep(0.9)
elif address == 6:
kit.continuous_servo[0].throttle = -1
kit.continuous_servo[1].throttle = -1
time.sleep(1.3)
else:
kit.continuous_servo[0].throttle = -1
kit.continuous_servo[1].throttle = -1
time.sleep(1.12)
class Address:
def __init__(self, id, msg):
self.id = id
self.msg = msg
def get_stop(self):
global action, stop, get_drive, good_to_go_loading, good_to_go_unloading
if self.id == operating_drive:
if self.id == address:
stop = True
Motor_Steer(0.4, (error * kp) + (ang * ap), True)
if self.id == 0:
action = "loading"
if good_to_go_loading:
stop = False
get_drive = True
good_to_go_loading = False
print("Loading Confirm!!!", stop)
else:
action = "unloading"
if good_to_go_unloading:
stop = False
get_drive = True
good_to_go_unloading = False
print("Unloading Confirm!!!")
address0 = Address(0, False)
address1 = Address(1, False)
address2 = Address(2, False)
address3 = Address(3, False)
address4 = Address(4, False)
address5 = Address(5, False)
address6 = Address(6, False)
# Follower
# Make flags
operating_drive = None
get_drive = True
ccw = True
mmode_flag = False
stop = True
current_path_id = None
current_path = None
# get_flag, ccw = make_TF(2, True)
# start, good_to_go_loading, good_to_go_unloading, mmode_flag = make_TF(4, False)
# # receive_command_flag = True
# stop0 = True
# stop1, stop2, stop3, stop4, stop5, stop6, msg0, msg1, msg2, msg3, msg4, msg5, msg6, turn0, turn1, turn2, turn3, turn4, turn5, turn6 = make_TF(20, False)
stop_trigger = False
# turn_flag = False
# loading_block = False
# operating_path = 0
# loading_get_flag = False
next_path = command['path']
path_id = command['path_id']
message = command['message']
# Start PiCam
camera = PiCamera()
camera.resolution = (320, 240)
rawCapture = PiRGBArray(camera, size=(320, 240))
time.sleep(0.1)
# Start Servo motors
from adafruit_servokit import ServoKit
kit = ServoKit(channels=16)
# Initialize variables
start_time = time.time()
kp = 0.75 # off line
ap = 1.0 # off angle
x_last = 160
y_last = 120
# videoFile1 = './video001.avi'
# fourcc = cv2.VideoWriter_fourcc(*'DIVX')
# out = cv2.VideoWriter(videoFile1, fourcc, 9.0, (320,240))
address = 0
road = 0
address0_time = 0
area_box = 10000.0
ang_list = []
dash_memory = np.zeros((2400, 320, 3))
dash_block_flag = False
start_time = time.time()
counter = 0
# Main Loop
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
counter += 1
# Command handler
path_id = command['path_id']
if command['path'] is not None: # and path_id != current_path_id:
next_path = command['path']
# current_path_id = path_id
if command['message'] == 'loading_complete':
good_to_go_loading = True
print('get loading complete!!')
command['message'] = None
if command['message'] == 'unloading_complete':
good_to_go_unloading = True
print('get unloading complete!!')
command['message'] = None
if mmode_flag: # M-mode handler
action = "M-mode"
Motor_Steer(0.4, (error * kp) + (ang * ap), True)
stop = True
elif area_box < 5000.0: # obstacle handler
Motor_Steer(0.4, (error * kp) + (ang * ap), True)
print('obstacle ahead: ', area_box)
action = 'obstacle'
stop = True
else:
stop = False
# stop False handler
address0.get_stop()
address1.get_stop()
address2.get_stop()
address3.get_stop()
address4.get_stop()
address5.get_stop()
address6.get_stop()
# Path handler
if get_drive:
if path_id != current_path_id:
current_path = list(next_path)
current_path_id = np.copy(path_id)
if len(current_path) > 0:
operating_drive = current_path.pop(0)
print("Next drive: ", operating_drive)
print("stop: ", stop)
get_drive = False
else:
operating_drive = 0
get_drive = False
# Image handler
image = frame.array
# out.write(image)
roi = image[60:239, 0:319]
hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
# yellow_lower = np.array([22, 60, 200], np.uint8)
yellow_lower = np.array([22, 0, 150], np.uint8)
yellow_upper = np.array([60, 255, 255], np.uint8)
yellow_line = cv2.inRange(hsv, yellow_lower, yellow_upper)
# yellow_line = cv2.inRange(image, (0, 0, 0), (75, 75, 75))
red_lower = np.array([170, 120, 70], np.uint8)
red_upper = np.array([180, 255, 255], np.uint8)
stopsign = cv2.inRange(hsv, red_lower, red_upper)
kernel = np.ones((3, 3), np.uint8)
# yellow_line = cv2.erode(yellow_line, kernel, iterations=3)
# yellow_line = cv2.dilate(yellow_line, kernel, iterations=3)
# stopsign = cv2.erode(stopsign, kernel, iterations=3)
# stopsign = cv2.dilate(stopsign, kernel, iterations=5)
contours_blk, hierarchy_blk = cv2.findContours(yellow_line.copy(),
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
contours_red, hierarchy_red = cv2.findContours(stopsign.copy(),
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
# Detect only main Yellow line, discard noise
contours_blk_len = len(contours_blk)
if contours_blk_len > 0:
if contours_blk_len == 1:
blackbox = cv2.minAreaRect(contours_blk[0])
else:
canditates = []
off_bottom = 0
for con_num in range(contours_blk_len):
blackbox = cv2.minAreaRect(contours_blk[con_num])
(x_min, y_min), (w_min, h_min), ang = blackbox
box = cv2.boxPoints(blackbox)
(x_box, y_box) = box[0]
if y_box > 238:
off_bottom += 1
canditates.append((y_box, con_num, x_min, y_min))
canditates = sorted(canditates)
if off_bottom > 1:
canditates_off_bottom = []
for con_num in range((contours_blk_len - off_bottom),
contours_blk_len):
(y_highest, con_highest, x_min,
y_min) = canditates[con_num]
total_distance = (abs(x_min - x_last)**2 +
abs(y_min - y_last)**2)**0.5
canditates_off_bottom.append(
(total_distance, con_highest))
canditates_off_bottom = sorted(canditates_off_bottom)
(total_distance, con_highest) = canditates_off_bottom[0]
blackbox = cv2.minAreaRect(contours_blk[con_highest])
else:
(y_highest, con_highest, x_min,
y_min) = canditates[contours_blk_len - 1]
blackbox = cv2.minAreaRect(contours_blk[con_highest])
# Get shift and ang error to trace the line
(x_min, y_min), (w_min, h_min), ang = blackbox
x_last = x_min
y_last = y_min
if ang < -45:
ang = 90 + ang
if w_min < h_min and ang > 0:
ang = (90 - ang) * -1
if w_min > h_min and ang < 0:
ang = 90 + ang
setpoint = 160
error = int(x_min - setpoint)
ang = int(ang)
# Get Address, based on the ang
if not mmode_flag: # to avoid false detection
if ccw:
if address == 0 and ang > 40:
print("error: turn to ccw")
turn(ccw)
elif address == 0 and ang < -60:
road = 101
elif road == 101 and ang > -60:
print("address: 101")
address = 1
road = 0
elif address == 1 and ang > -10:
print("address: 102")
address = 2
elif address == 2 and ang < -60:
print("address: 103")
address = 3
elif address == 3 and ang > -30:
road = 34
elif road == 34 and ang < -65:
print("address: 203")
address = 4
road = 0
elif address == 4 and ang > 20:
print("address: 202")
address = 5
elif address == 5 and ang < -65:
print("address: 201")
address = 6
else:
if address == 0 and ang < -40:
print("error: turn to cw")
turn(ccw)
elif address == 0 and ang > 60:
road = 201
elif road == 201 and ang < 60:
print("address: 201")
address = 6
road = 0
elif address == 6 and ang < -20:
print("address: 202")
address = 5
elif address == 5 and ang > 65:
print("address: 203")
address = 4
elif address == 4 and ang < 35:
road = 43
elif road == 43 and ang > 65:
print("address: 103")
address = 3
road = 0
elif address == 3 and ang < 10:
print("address: 102")
address = 2
elif address == 2 and ang > 65:
print("address: 101")
address = 1
# Stop sign handler
if not mmode_flag:
if len(contours_red) > 0:
print("stopsign: ", time.time() - start_time)
stop_trigger = True
if not (len(contours_red) > 0) and stop_trigger:
print("address: LZ")
address = 0
stop_trigger = False
# Obstacle handler
_, wh_box, _ = blackbox
w_box, h_box = wh_box
area_box = w_box * h_box
# Stop handler
if operating_drive == 9:
print("turn!")
turn(ccw)
ccw = change_flag(ccw)
get_drive = True
else:
address0.get_stop()
address1.get_stop()
address2.get_stop()
address3.get_stop()
address4.get_stop()
address5.get_stop()
address6.get_stop()
if not stop:
action = "moving"
# print("moving!!")
Motor_Steer(0.4, (error * kp) + (ang * ap))
# Send robot status
if counter % 5 == 0:
send_status_flag_lock.acquire()
send_status_flag = True
send_status_flag_lock.release()
if ccw:
direction = 1
else:
direction = -1
if current_address != address:
current_address = address
elif action != "loading" and action != "unloading":
current_address = 999
# Draw parameters on display
box = cv2.boxPoints(blackbox)
box = np.int0(box)
# cv2.drawContours(image, [box], 0, (0, 0, 255), 3)
# cv2.putText(image, str(ang), (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
# cv2.putText(image, str(error), (10, 320), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
# cv2.line(image, (int(x_min), 190), (int(x_min), 230), (255, 0, 0), 3)
# cv2.drawContours(image, contours_red, -1, (0,255,0), 3)
cv2.imshow("original with line", image)
rawCapture.truncate(0)
# Ang error logger
ang_list += [[time.time() - start_time, ang]]
if counter % 10 == 0:
dash_memory = dash_memory[0:2160]
dash_memory = np.vstack((image, dash_memory))
# Key Binding
key = cv2.waitKey(1) & 0xFF
# Manually Confirm
if key == ord("y"):
if action == "loading":
good_to_go_loading = True
elif action == "unloading":
good_to_go_unloading = True
# M-mode on/off
elif key == ord("z"):
mmode_flag = True
action = "M-mode"
print("M-mode On")
if not dash_block_flag:
# now = datetime.now()
# mmode_start = now.strftime("%Y-%m-%d %H:%M:%S")
error_type = 'manual'
np.save("./dash_cam/{}.npy".format(counter), dash_memory)
dash_file_name = "{}.npy".format(counter)
dash_block_flag = True
elif key == ord("x"):
mmode_flag = False
print("M-mode Off")
dash_block_flag = False
# Terminate process
elif key == ord("q"):
kit.continuous_servo[0].throttle = 0
kit.continuous_servo[1].throttle = 0
break
# Save data
# out.release()
# cv2.destroyAllWindows()
finish_time = time.time()
fps = counter / (finish_time - start_time)
print("fps = " + str(fps))
ang_list = np.array(ang_list)
