elif key.upper() == 'E': #increase z value
z_up()
if test() == False:
z_down()
elif key.upper() == 'Q': #decrease z value
z_down()
if test() == False:
z_up()
quit = False #for breaking the motor loop with the '1' key command
import math #to calculate all angle values and error
try: #if not connected to a RoboPi, it can still run
from bsmLib import RPL
RPL.init()
elbow_motor_speed = 300
shoulder_motor_speed = 300
distance_of_error = 3 #max distance arm can be away from intended point
max_error = distance_of_error / math.sqrt(
3) #to convert the error to the intiger unit of measurement
shoulder_range = 840 #range of read value for the shoulder POT
elbow_range = 775 #range of read value for the elbow POT
swivel_range = 800 #(NUMBER HERE: NEED TO FIND POT AND ITS RANGE FOR THIS) #range of read value for the swivel POT
shoulder_pul = 1 #shoulder pulse pin
shoulder_dir = 2 #shoulder direction pin
from bsmLib import RPL RPL.init() # Init RoboPiLib RPL.servoWrite(0, 1000) # Write to pin 0 value 1000
MIN = 1000 # Min servo speed
MAX = 2000 # Max servo speed
# Servo Pins
L = 0
R = 1
######################
## 0. Setup
######################
# Create tcp connection and listen
t = tcpServer(HOST, PORT)
t.listen()
RPL.init() # Init RoboPi hat connection
######################
## 1. drive
######################
def drive():
d = t.recv()
if d == "stop":
RPL.servoWrite(L, 0)
RPL.servoWrite(R, 0)
t.stop()
exit()
d = d.split(' ')
l = int(map(float(d[0]), -1, 1, MIN, MAX))
r = int(map(float(d[1]), -1, 1, MIN, MAX))
z = 0.0 #starting z value
gopen = 1
gclose = 1
speed = 1 #starting speed (whole number between 1 and 4)
def test(): #function for angle domains
reach_length = (x ** 2 + y ** 2 + z ** 2) ** 0.5
if reach_length > d_one + d_two or reach_length < d_one - d_two:
return False
import math #to calculate all angle values and error
try: #if not connected to a RoboPi, it can still run
from bsmLib import RPL #to pull all files needed to run the motors
RPL.init() #connect to RoboPi
elbow_motor_speed = 300
shoulder_motor_speed = 300
distance_of_error = 3 #max distance arm can be away from intended point
max_error = distance_of_error / math.sqrt(3) #to convert the error to the intiger unit of measurement
shoulder_range = 840 #range of read value for the shoulder POT
elbow_range = 775 #range of read value for the elbow POT
swivel_range = 800 #(NUMBER HERE: NEED TO FIND POT AND ITS RANGE FOR THIS) #range of read value for the swivel POT
shoulder_pul = 1 #shoulder pulse pin
shoulder_dir = 2 #shoulder direction pin
elbow_pul = 5 #elbow pulse pin