from threading import Thread
import numpy as np
import os, sys
import maestro
from haversine import RangeAndBearing
# serial setups
time.sleep(0.25)
RCSer = serial.Serial('/dev/ttyUSB0',baudrate = 115200) # varies on which is plugged first
time.sleep(0.25)
SensorSer = serial.Serial('/dev/ttyACM0',baudrate = 115200) # consistent
time.sleep(0.25)
# maestro setup
Maestro = maestro.Controller('/dev/ttyACM1')
SteerChannel = 6
MotorChannel = 8
# servo settings, the 4x mult is due to quarter microsecs
microSecMin = 4*750 # -- turns boat left
microSecMax = 4*2500 # -- turns boat right
Maestro.setRange(SteerChannel, microSecMin, microSecMax)
Maestro.setAccel(SteerChannel,254) # basically max
Maestro.setSpeed(SteerChannel,100) # 0-255 close to max but slightly unkown, look in maestro code for more info
PWMlow = 0
PWMhigh = 127
# steering params
MAXRIGHT = 20
MAXLEFT = -20
def createFolder(folder_name):
if not os.path.exists(folder_name):
os.makedirs(folder_name)
def limitValue(n, minn, maxn):
return max(min(maxn, n), minn)
# =====================================
# SERVO & SERIES
# =====================================
# Polotu
import maestro
servo = maestro.Controller()
import maestro
servo = maestro.Controller('/dev/ttyACM0')
servo.setRange(0, 1000 * 4, 2000 * 4)
servo.setSpeed(0, 45)
servo.setRange(1, 1000 * 4, 2500 * 4)
# Arduino
import serial
ser = serial.Serial(port='/dev/ttyTHS2', baudrate=115200) # open serial port
ser.flushInput()
ser.flushOutput()
# =====================================
# Init value
CAM_STOP = 6000
# VARIABLES
speedToggle = 2 # 2 = Full Speed; 1 = Slow Mode; 0 = Driving Disabled
currentThrower = 7600 # Initial thrower set to 50% power
# BOOLEANS
isStarted = False # Press Start to enable the robot
compressorEnabled = False
PressedY = False # Allows Y to be called on rising edge
throwerEnabled = False
pressedA = False # Allows A to be called on rising edge
pressedBack = False # Allows Back to be called on rising edge
j = xbox.Joystick()
motors = maestro.Controller()
drive = drive.DriveTrain(motors, LEFT_MOTORS, RIGHT_MOTORS)
try:
winch = PiIO.Spike(1)
compressor = PiIO.Spike(2)
cannonl = PiIO.Spike(3)
cannonr = PiIO.Spike(4)
upperLimit = PiIO.Switch(1)
lowerLimit = PiIO.Switch(2)
camLimit = PiIO.Switch(3)
pressureSw = PiIO.Switch(4)
except:
raise
# BEGIN STD_MSGS
from std_msgs.msg import Int32
# END STD_MSGS
# Maestro channel assignment
sharp = 0
# Sharp equation coefficients: dist(cm)= a / (Output-b) - c
a = 4350.3 # 5788
b = 33.4 # 8.44
c = -0.30 # 0.89
# Used for rospy.Rate (should equal or muliple of rate in 'control' node)
cycle = 0.5
rospy.init_node('sharp_node')
# BEGIN PUB
pub = rospy.Publisher('sharp_data', Int32, queue_size=10)
# END PUB
# BEGIN LOOP
s = m.Controller()
rate = rospy.Rate(1 / cycle)
while not rospy.is_shutdown():
Output = s.getPosition(sharp)
dist = a / (Output - b) - c
pub.publish(dist)
rate.sleep()
# END LOOP
# END ALL
import numpy as np
import random
import math
from mpl_toolkits.mplot3d import Axes3D #Complemento de la libreria de ploteo para 3d
import serial #Libreria de comunicación Serial
import maestro
from Adafruit_IO import Client, Data
ADAFRUIT_IO_USERNAME = "******"
ADAFRUIT_IO_KEY = "3dfaa1c4618c41718ca6725367e40892"
aio = Client(ADAFRUIT_IO_USERNAME, ADAFRUIT_IO_KEY)
#servo = maestro.Controller("/dev/ttyACM0") #Puerto COM para Linux
servo = maestro.Controller("COM4") #Puerto COM para Windows cambiar por el asignado
class Ui_MainWindow(object):
################### Configuración de GUI ####################
def setupUi(self, MainWindow):
MainWindow.setObjectName("MainWindow")
MainWindow.resize(1211, 569)
MainWindow.setMinimumSize(QtCore.QSize(881, 569))
MainWindow.setMaximumSize(QtCore.QSize(2000, 2000))
palette = QtGui.QPalette()
brush = QtGui.QBrush(QtGui.QColor(0, 0, 0))
brush.setStyle(QtCore.Qt.SolidPattern)
palette.setBrush(QtGui.QPalette.Active, QtGui.QPalette.WindowText, brush)
brush = QtGui.QBrush(QtGui.QColor(238, 238, 236))
global controller
# Set drive speed
controller.setTarget(PWM_SPEED_CHANNEL, int(msg.drive.speed))
# Set drive angle
controller.setTarget(PWM_TURN_CHANNEL, int(msg.drive.steering_angle))
pwm_debug_channel.publish( "Velocity Signal: " + \
str(msg.drive.speed) + " | "\
"Turn Signal: " + \
str(msg.drive.steering_angle))
# initialize servo controller
controller = maestro.Controller()
# speed config
controller.setRange(PWM_SPEED_CHANNEL, PWM_SPEED_MIN, PWM_SPEED_MAX)
controller.setSpeed(PWM_SPEED_CHANNEL, 0)
controller.setAccel(PWM_SPEED_CHANNEL, 0)
controller.setTarget(PWM_SPEED_CHANNEL, (PWM_SPEED_MAX + PWM_SPEED_MIN) // 2)
# turning config
controller.setRange(PWM_TURN_CHANNEL, PWM_TURN_RIGHT, PWM_TURN_LEFT)
controller.setSpeed(PWM_TURN_CHANNEL, 0)
controller.setAccel(PWM_TURN_CHANNEL, 0)
controller.setTarget(PWM_TURN_CHANNEL, (PWM_TURN_RIGHT + PWM_TURN_LEFT) // 2)
# initialize ros
rospy.init_node('pwm')
import threading import queue import target from findFace import FaceFinder MOTORS = 1 TURN = 2 BODY = 0 HEADTILT = 4 HEADTURN = 3 IP = '10.200.27.207' PORT = 5010 robot = maestro.Controller() body = 6000 headTurn = 6000 headTilt = 6000 motors = 6000 turn = 6000 amount = 400 state = 0 frameCount = 0 scanDir = 0 startTime = 0 hasTorqued = False returnTrip = False declare = True
from picamera import PiCamera
import time
import cv2
import numpy as np
import BoboGo as bg
import BoboFace as bf
import maestro
MOTORS = 1
TURN = 2
BODY = 0
HEADTILT = 4
HEADTURN = 3
bobo = maestro.Controller()
body = 6000
headTurn = 6000
headTilt = 6000
motors = 6000
turn = 6000
amount = 400
robotLabNight = ((22, 20, 60), (32, 255, 255))
physicsLab = ((10, 63, 100), (25, 255, 255))
clockworkOrange = robotLabNight
def calcWeight(x, y):
return np.exp(-(480 - y) / 0.01) * x
def do_ik(pos):
# https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-geometry/
q2 = np.arccos((pos[0]**2 + pos[1]**2 - a1**2 - a2**2) / (2 * a1 * a2))
q1 = np.arctan2(pos[1], pos[0]) - np.arctan2((a2 * np.sin(q2)),
(a1 + a2 * np.cos(q2)))
# 0 for q2 is actually 90*
return np.array([0, q1, q2 - np.pi / 2.]) * 180. / np.pi
n_servos = 3
servo_min = np.array([500, 500, 500])
servo_centers = np.array([1500, 1500, 1500])
servo_ms_per_deg = np.array([1000 / 90., 1000 / 90., 1000 / 90.])
if __name__ == "__main__":
servo = maestro.Controller(ttyStr='COM6')
while (1):
start_time = time.time()
t = time.time() - start_time
while t < max(time_list):
t = time.time() - start_time
ee_pos = ee_pos_generator(t)
pos = do_ik(ee_pos)
print("%f: %s -> %s" % (t, ee_pos, pos))
pos_in_ms = servo_centers + pos * servo_ms_per_deg
for k in range(n_servos):
# Commands in quarter-ms
servo.setTarget(k, int(pos_in_ms[k] * 4))
servo.close()
###############################################
## Open CV and Numpy integration ##
###############################################
import pyrealsense2 as rs
import numpy as np
import cv2
import maestro
# -*- coding: utf-8 -*-
#from espeak import espeak
import os
# Configure depth and color streams
servo_ = maestro.Controller('/dev/ttyACM1')
servo_.setAccel(0, 10) #set servo_ 0 acceleration to 4
servo_.setSpeed(0, 10) #set speed of servo_ 1
servo_.setTarget(1, 6100)
pipeline = rs.pipeline()
config = rs.config()
config.enable_device('746612070147')
# config.enable_stream(rs.stream.pose)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
print(config.can_resolve(pipeline))
# Start streaming
pipeline.start(config)
try:
#Este programa es el publicador de distancias que lee del el puerto serial (enviadas desde arduino)
import rospy
import time
import maestro
from std_msgs.msg import Int32
from std_msgs.msg import String
pololu = maestro.Controller()
sensor_dist = 0
a = 6787
b = 3
c = 4
pub1 = rospy.Publisher('/message1', Int32, queue_size = 1)
pub2 = rospy.Publisher('/message2', Int32, queue_size = 1)
try:
def callback(data):
motorL = -1
if(data.data == 'a'):
#read_distance(motorR, motorL)
Output = pololu.getPosition(sensor_dist)
dist = (a / (Output - b)) - c
#print(dist)
if(dist < 15):
motorR = -1
else:
motorR = 1
else:
motorR = 0
def main(ARGS):
# Load DeepSpeech model
if os.path.isdir(ARGS.model):
model_dir = ARGS.model
ARGS.model = os.path.join(model_dir, 'output_graph.pb')
ARGS.scorer = os.path.join(model_dir, ARGS.scorer)
print('Initializing model...')
logging.info("ARGS.model: %s", ARGS.model)
model = deepspeech.Model(ARGS.model)
if ARGS.scorer:
logging.info("ARGS.scorer: %s", ARGS.scorer)
model.enableExternalScorer(ARGS.scorer)
# Wrapping the robot loop in a try/finally structure makes sure that the robot stops
# moving if your code errors out or the robot loop completes.
try:
# Maestro control initialization
m = maestro.Controller(
) # for Aljon's PC, the value here should be the string 'COM4'
# Mode text = 0 vs Mode voice = 1
mode = 0
while True:
if (mode == 0):
command = input("> ").strip()
# Change mode
if (command == "mode 1"):
print("Now in Mode 1 [voice recognition]")
mode = 1
# Jump
elif (command == "jump"):
m.runScriptSub(0) # run idle/reset sequence
# while (m.getMovingState): time.sleep(0.5)
print("Preparing to jump...")
m.runScriptSub(5) # run crouching sequence
# while (m.getMovingState): time.sleep(0.5)
print("Ready to jump!")
m.runScriptSub(6) # run jumping sequence
# while (m.getMovingState): time.sleep(0.5)
print("JUMP!!!")
# Walk Forward
elif (command == "walk forward"):
m.runScriptSub(0) # run idle/reset sequence
# while (m.getMovingState): time.sleep(0.5)
print("Walking... press Ctrl+C to stop walking.")
try:
steps = 0
while (m.getMovingState):
steps += 1
print(f"Steps taken = {steps}")
m.runScriptSub(1) # run forwards walking sequence
time.sleep(0.5)
except KeyboardInterrupt:
print('Done walking!')
# Walk Backward
elif (command == "walk backward"):
m.runScriptSub(0) # run idle/reset sequence
# while (m.getMovingState): time.sleep(0.5)
print("Walking... press Ctrl+C to stop walking.")
try:
steps = 0
while (m.getMovingState):
steps += 1
print(f"Steps taken = {steps}")
m.runScriptSub(2) # run backwards walking sequence
time.sleep(0.5)
except KeyboardInterrupt:
print('Done walking!')
# Turn right
elif (command == "turn right"):
pass
# Turn left
elif (command == "turn left"):
pass
# Quit
elif (command == "quit"):
print("Shutting down Robot Dog...")
m.runScriptSub(5) # run sitting sequence
# while (m.getMovingState): time.sleep(0.5)
break
# Invalid input
else:
print("Try again, I didn't recognize that command.")
elif (mode == 1):
try:
# Start audio with VAD
vad_audio = VADAudio(
aggressiveness=ARGS.vad_aggressiveness,
device=ARGS.device,
input_rate=ARGS.rate,
file=ARGS.file)
print(
"NOTE: Press Ctrl+C or say 'quit' to exit voice recognition at any time."
)
print("Listening...")
frames = vad_audio.vad_collector()
# Stream from microphone to DeepSpeech using VAD
spinner = None
if not ARGS.nospinner:
spinner = Halo(spinner='line')
stream_context = model.createStream()
wav_data = bytearray()
for frame in frames:
if frame is not None:
if spinner: spinner.start()
logging.debug("streaming frame")
stream_context.feedAudioContent(
np.frombuffer(frame, np.int16))
if ARGS.savewav: wav_data.extend(frame)
else:
if spinner: spinner.stop()
logging.debug("end utterence")
if ARGS.savewav:
vad_audio.write_wav(
os.path.join(
ARGS.savewav,
datetime.now().strftime(
"savewav_%Y-%m-%d_%H-%M-%S_%f.wav")
), wav_data)
wav_data = bytearray()
# Record words that were said
command = stream_context.finishStream()
print("Recognized: %s" % command)
# Do same commands here, based on voice input...
# Jump
if ("jump" in command):
m.runScriptSub(0) # run idle/reset sequence
# while (m.getMovingState): time.sleep(0.5)
print("Preparing to jump...")
m.runScriptSub(5) # run crouching sequence
# while (m.getMovingState): time.sleep(0.5)
print("Ready to jump!")
m.runScriptSub(6) # run jumping sequence
# while (m.getMovingState): time.sleep(0.5)
print("JUMP!!!")
# Walk Forward
elif ("walk" in command and "forward" in command):
m.runScriptSub(0) # run idle/reset sequence
# while (m.getMovingState): time.sleep(0.5)
print(
"Walking... press Ctrl+C to stop walking.")
try:
steps = 0
while (m.getMovingState):
steps += 1
print(f"Steps taken = {steps}")
m.runScriptSub(
1) # run forwards walking sequence
time.sleep(0.5)
except KeyboardInterrupt:
print('Done walking!')
# Walk Backward
elif ("walk" in command and "back" in command):
m.runScriptSub(0) # run idle/reset sequence
# while (m.getMovingState): time.sleep(0.5)
print(
"Walking... press Ctrl+C to stop walking.")
try:
steps = 0
while (m.getMovingState):
steps += 1
print(f"Steps taken = {steps}")
m.runScriptSub(
2
) # run backwards walking sequence
time.sleep(0.5)
except KeyboardInterrupt:
print('Done walking!')
# Turn right
elif ("turn" in command and "right" in command):
pass
# Turn left
elif ("turn" in command and "left" in command):
pass
# Invalid input
else:
print(
"Try again, I didn't recognize that command."
)
# Listen again
stream_context = model.createStream()
print("Listening...")
except KeyboardInterrupt:
print("Back to Mode = 0 [text commands]")
mode = 0
finally:
m.stopScript() #stops the current Maestro sequence
m.close() #cleanly close USB serial port
parser.add_argument('--defdriver',
action='store_true',
help='use default system video driver instead of DSHOW')
parser.add_argument('--serport',
type=str,
default='COM3',
help='serial port to use for servo; default: COM3')
args = parser.parse_args()
# which output to use on servo
servoOut = 0
# default position
servoX = args.center
has_servo = True
try:
servo = maestro.Controller(args.serport)
except:
print('Could not connect to controller on port', args.serport)
has_servo = False
# reset servo to initial position
if has_servo:
servo.setTarget(servoOut, servoX)
pygame.mixer.init()
pygame.mixer.music.load('pew.wav')
# load yolov3 model
model = load_model('model.h5')
print(model.summary())
# this is the image size that the model expects
input_w, input_h = 416, 416
coxa_angle = self.get_coxa_angle(body_xyz, body_angle)
femur_angle, tibia_angle = self.get_femur_tibia_angles(
body_xyz, body_angle)
pod.setTarget(self.servos[0], angle_to_steps(coxa_angle))
pod.setTarget(self.servos[1], angle_to_steps(femur_angle))
pod.setTarget(self.servos[2], angle_to_steps(tibia_angle))
def initialize(self):
pod.setTarget(self.servos[0], angle_to_steps(0))
pod.setTarget(self.servos[1], angle_to_steps(0))
pod.setTarget(self.servos[2], angle_to_steps(0))
##### MAIN #####
pod = maestro.Controller()
legs = []
legs.append(Leg((40, 70, 0), (75, 130, 0), (0, 6, 12)))
legs.append(Leg((65, 0, 0), (150, 0, 0), (1, 7, 13)))
legs.append(Leg((40, -70, 0), (75, -130, 0), (2, 8, 14)))
legs.append(Leg((-40, 70, 0), (-75, 130, 0), (3, 9, 15)))
legs.append(Leg((-65, 0, 0), (-150, 0, 0), (4, 10, 16)))
legs.append(Leg((-40, -70, 0), (-75, -130, 0), (5, 11, 17)))
enable = True
if enable:
for leg in legs:
leg.initialize()
time.sleep(0.5)
dance = False
import serial
from numpy import interp
#
import maestro
servos = maestro.Controller("COM8")
servos.setAccel(0, 3) #set servo 0 acceleration to 4
servos.setSpeed(0, 80)
servos.setRange(0, 4000, 10000)
rotations = {0: True, 6: True, 12: True, 3: False, 9: False, 16: False}
print(servos.getMax(0), servos.getMin(0))
angulo = 240
servos.setTarget(0, 6000)
#[servos.setTarget(servo,1*(-1 if not rotations[servo] else 1)) for servo in rotations]
[servos.getPosition(servo) for servo in [0, 3, 6, 9, 12, 16]]
servos.close
all_servo_names = ", ".join(self.servos.keys())
expressions = ", ".join(self.expressions.keys())
return f"Face has servos: {all_servo_names}.\nFace has expressions: {expressions}"
def get_expression_names(self):
return list(self.expressions.keys())
def perform_expression(self, exp_name, default_positions=True):
staged_movements = {}
# if default positions are desired for unspecified servos, then stage all resets
if default_positions:
for name in self.servos.keys():
staged_movements[name] = self.servo_descriptions[name][
'default_position']
#Update staged movements with those explicitly set in the expression
for s_name, pos in self.expressions[exp_name].items():
staged_movements[s_name] = pos
# Execute all movements
for s_name, pos in staged_movements.items():
self.servos[s_name].set_sem_pos(pos)
if __name__ == "__main__":
import maestro
face = Face(maestro.Controller())
face.perform_expression('meh')
print(face)
def __init__(self):
self.tango = maestro.Controller()
self.setup_targets()
IP = '10.200.39.59'
PORT = 5010
self.client = ClientSocket(IP, PORT)
"""
Some magic
NB: [setTarget(target)] takes quarter microseconds, NOT MICROSECONDS.
"""
import maestro
import time
import sys
import serial
if __name__ == "__main__":
servo = maestro.Controller(ttyStr="/dev/ttyACM0")
for i in range(24):
servo.setTarget(0, 4 * 500)
time.sleep(1)
servo.setTarget(0, 4000)
time.sleep(1)
servo.close
print("end")
import maestro
import time
serport = 'COM3'
try:
servo = maestro.Controller(serport)
except:
print('Could not connect to controller on port', serport)
exit(1)
while True:
servo.setTarget(0, 4000)
time.sleep(1)
servo.setTarget(0, 5000)
time.sleep(1)
servo.setTarget(0, 6000)
time.sleep(1)
servo.setTarget(0, 7000)
time.sleep(1)
servo.setTarget(0, 8000)
time.sleep(1)
if servo.isMoving(0):
print('moving')
else:
print('stopped')
print(servo.getPosition(0))
servo.close()
def callback(msg):
print "speed_left= ", msg.data
s = m.Controller()
speed = msg.data
s.setTarget(izq, speed)
import maestro
import time
my_maestro = maestro.Controller("/dev/ttyACM1")
my_maestro.setTarget(target=5000, channel=0)
time.sleep(1)
my_maestro.setTarget(target=7000, channel=0)
import maestro
from time import sleep
servo = maestro.Controller("/dev/ttyACM0")
servo.setAccel(0,0) #set servo 0 acceleration to 0 (unrestrained)
servo.setTarget(0,6000) #set servo to move to center position
sleep(2)
servo.setAccel(1,0) #set servo 1 acceleration to 0 (unrestrained)
servo.setTarget(1,6000) #set servo to move to center position
sleep(2)
servo.setAccel(2,0) #set servo 2 acceleration to 0 (unrestrained)
servo.setTarget(2,6000) #set servo to move to center position
sleep(2)
servo.setAccel(3,0) #set servo 3 acceleration to 0 (unrestrained)
servo.setTarget(3,6000) #set servo to move to center position
sleep(5)
servo.setTarget(0, 6200)
servo.setTarget(1, 6200)
servo.setTarget(2, 6200)
servo.setTarget(3, 6200)
sleep(3)
servo.setTarget(0, 6000)
servo.setTarget(1, 6000)
servo.setTarget(2, 6000)
servo.setTarget(3, 6000)
import maestro tango = maestro.Controller() tango.setTarget(1, 5200)
import maestro
import time
servo = maestro.Controller('/dev/ttyAMA0')
servo.runScriptSub(0)
print("sub0")
GPIO.setup(23, GPIO.OUT)
GPIO.setup(24, GPIO.OUT)
except:
pass
GPIO.output(18, GPIO.HIGH)
GPIO.output(23, GPIO.HIGH)
GPIO.output(24, GPIO.HIGH)
import maestro
from tornado.options import define, options
define("port", default=8000, help="run on the given port", type=int)
arm_l = maestro.Controller('/dev/ttyACM2', config_file="ArmR.json")
arm_r = maestro.Controller('/dev/ttyACM0', config_file="ArmL.json")
pwm_vector = {"target_pwm_l": [], "target_pwm_r": []}
def update_positions():
cmd = 'updatePosition'
try:
param = {
'pwmvalL': arm_l.get_all_positions(),
'pwmvalR': arm_r.get_all_positions()
}
msg = {"cmd": cmd, "param": param}
logging.info("update_positions, msg = {}".format(msg))
except:
# param = {'pwmvalL': arm_l.get_all_positions(),
# -*- coding: utf-8 -*-
"""
Created on Thu Oct 18 04:18:11 2018
## Pololu Maestro Exploration for DRM
@author: Dave
"""
print 'Pololu Maestro Testing'
# Code from https://github.com/frc4564/maestro
import serial
import maestro
servo = maestro.Controller('COM22')
#servo = maestro.Controller(m)
print 'set accel'
servo.setAccel(5, 4) #set servo 0 acceleration to 4
print 'set position'
servo.setTarget(5, 6000) #set servo to move to center position
print 'set speed'
servo.setSpeed(5, 10) #set speed of servo 1
print 'get position'
x = servo.getPosition(5) #get the current position of servo 1
print 'close'
servo.close()
time.sleep(1)
elif (laser_pos[1] < desy):
print(4)
new_posX = s.setTarget(R_ver, cur_posY + inc)
time.sleep(1)
ret, img = cap.read()
warped_img = getWarp(img, M)
laser_pos = get_laser_pos(warped_img)
time.sleep(1)
print("new pos: ", laser_pos)
return
##############################################################################
s = maestro.Controller('COM6')
# assigning channels to the servos
'''R_ver =
L_hor =
L_ver ='''
R_ver = 7
R_hor = 8
cap = cv.VideoCapture(0)
ret, img = cap.read()
M = getWarpMatrix(img)
input("press any key to continue: ")
s.setAccel(R_hor, 0)
s.setSpeed(R_hor, 0)
s.setTarget(R_hor, 6000)
import time
import maestro
servo = maestro.Controller("COM13")
servo.setAccel(0, 4) #set servo 0 acceleration to 4
servo.setSpeed(0, 10) #set speed of servo 1
servo.setAccel(1, 4) #set servo 0 acceleration to 4
servo.setSpeed(1, 10) #set speed of servo 1
servo.setAccel(2, 4) #set servo 0 acceleration to 4
servo.setSpeed(2, 10) #set speed of servo 1
servo.setAccel(3, 4) #set servo 0 acceleration to 4
servo.setSpeed(3, 10) #set speed of servo 1
servo.setAccel(4, 0) #set servo 0 acceleration to 4
servo.setSpeed(4, 0) #set speed of servo 1
#save arm
servo.setTarget(3, 5560)
time.sleep(2)
servo.setTarget(1, 7920)
servo.setTarget(2, 5380)
time.sleep(2)
servo.setTarget(0, 5268)
#take pencil
servo.setTarget(0, 5300)
time.sleep(3)
import ASV_Functions as F
import ASV_Parameters as P
import numpy as np
#import laser2
import maestro
import time, sys, tty, termios
#import tracking as T
#hello
#get_Camera_Distance = F.get_Camera_Distance()
max_value = 2000 * 4 #maximum speed desired for clockwise rotation
min_value = 1000 * 4 #minimum speed desired for anti-clockwise rotation
servo = maestro.Controller()
servo.setRange(0, min_value, max_value)
servo.setRange(1, min_value, max_value)
servo.setRange(2, min_value, max_value)
#Initializing motor
servo.setTarget(0, 6000)
servo.setTarget(1, 6000)
servo.setTarget(2, 6000)
print("start, controls, speed or stop")
#def start():
# T.webcam()
# T.track()
#foam.webcam()
#while(foam.detection == True): #need to add when object is detected
# if camera_centre != object_centre: #need to include frame centre from foam.py
import maestro maestro.Controller().setTarget(4, 6500)
