File size: 3,706 Bytes
6e73cd3 714d948 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 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 |
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():
"""To find all components of quadrilateral ABCD, given sides AB, BC, CD, and angles A, B, and C, we can use the following steps:
Find the length of side AD.
We can use the law of cosines to find the length of AD:
AD^2 = AB^2 + BC^2 - 2 * AB * BC * cos(C)
AD = sqrt(AB^2 + BC^2 - 2 * AB * BC * cos(C))
AD = sqrt(192^2 + 116^2 - 2 * 192 * 116 * cos(118))
AD = 154.7
Find the angles of triangle ABC.
We can use the law of sines to find the angles of triangle ABC:
sin(A) / BC = sin(B) / AC
sin(C) / AB = sin(A) / AC
AC = sin(A) * BC / sin(B)
AC = sin(100) * 116 / sin(95)
AC = 120.7
sin(C) / AC = sin(B) / AB
BC = sin(C) * AC / sin(B)
BC = sin(118) * 120.7 / sin(95)
BC = 126.8
Now that we know the lengths of all sides of triangle ABC, we can use the law of cosines to find the angles B and C:
cos(B) = (AC^2 + AB^2 - BC^2) / (2 * AC * AB)
B = acos((AC^2 + AB^2 - BC^2) / (2 * AC * AB))
B = 92.9°
cos(C) = (BC^2 + AB^2 - AC^2) / (2 * BC * AB)
C = acos((BC^2 + AB^2 - AC^2) / (2 * BC * AB))
C = 119°
Find the angle of triangle ADC.
The angle of triangle ADC is the sum of angles A and B, minus 180 degrees:
ADC = A + B - 180°
ADC = 100 + 92.9 - 180°
ADC = -11.1°
Now that we know all of the components of quadrilateral ABCD, we can keep decreasing side CD by 1 unit until it is left with 70, and find the angles A, B, and C every time we decrease 1 unit.
To do this, we can use the following steps:
Update the length of side CD.
Find the new length of side AD using the law of cosines.
Find the new angles of triangle ABC using the law of sines and law of cosines.
Find the new angle of triangle ADC by subtracting 180 degrees from the sum of angles A and B.
We can repeat these steps until CD is equal to 70."""
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')) |