File size: 1,980 Bytes
6e73cd3 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 |
import serial
import math
from time import sleep
ArduinoData = serial.Serial('/dev/cu.usbserial-1420', 9600)
sleep(5)
RANGE_OF_MOTION = 10
DISTANCE_PER_SECOND = 35
hypotenuse = 0
servo_angle = 0
def find_height(hypotenuse, angle):
height = math.sin(math.radians(angle))*hypotenuse
return height
def distance():
global hypotenuse, servo_angle
ArduinoData.write(b"distang")
while hypotenuse == 0:
try:
hypotenuse = int(ArduinoData.readline())
except ValueError:
hypotenuse = 0
servo_angle = int(ArduinoData.readline())-50
hypotenuse = hypotenuse
print("Servo angled at:", servo_angle, "Distance to Object:", hypotenuse)
distance = round(math.cos(math.radians(servo_angle))*hypotenuse)
print("Lateral Distance:", distance)
return distance
def wrist_angle(distance, height):
return math.degrees(math.atan(height/distance))
def absolute(num):
return ((num)**2)**0.5
def target(coordinates):
pass
def find(object):
pass
def grab():
pass
def go(direction, distance):
global servo_angle, hypotenuse
if direction == "forward":
height = find_height(hypotenuse, servo_angle)
if distance > RANGE_OF_MOTION:
send_distance = distance - RANGE_OF_MOTION
send_time = send_distance/DISTANCE_PER_SECOND
remaining_distance = distance-send_distance
send_angle = wrist_angle(remaining_distance, height)+50
print("Moving:", send_distance, "\t\tDistance to Object:", remaining_distance)
ArduinoData.write(bytes(f"forward {int(send_time*1000)}", 'utf-8'))
sleep(1.5)
ArduinoData.write(bytes(f"wrist_track {int(send_angle)}", "utf-8"))
if direction == "backward":
send_distance = distance - RANGE_OF_MOTION
send_time = send_distance/DISTANCE_PER_SECOND
ArduinoData.write(bytes(f"backward {int(send_time*1000)}", 'utf-8')) |