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)