robotArmInvKin

#Inverse Kinematics v3 for 2 joint robot arm by chris garrard
#arm lengths a1, a2
#takes x,y coordinates of destination object
#and incrementally changes the arm angles to reach it
# outputs the arm angle values for the way points

#import libraries
import math
import time
import pygame
pygame.init()

# define event handlers for simulation
def draw(mainDisplay):
    mainDisplay.fill(blue)
    global x1, y1, xE, yE
    pygame.draw.circle(mainDisplay, ball_color, ball_pos, Ball_Radius)
    pygame.draw.line(mainDisplay, green, (xO,yO), (x1,y1), 10)
    pygame.draw.line(mainDisplay, red, (x1,y1), (xE,yE), 10)

def plot(q1, q2):

    global x1, y1, xE, yE
    x1 = a1 * math.cos(q1)
    y1 = a1 * math.sin(q1)
    theta = q2 + q1
    xE = x1 + a2 *  math.cos(theta) + xO
    yE = y1 + a2 * math.sin(theta) + yO
    x1 = x1 + xO
    y1 = y1 + yO

# Intitialize Global Variables
Width = 500
Height = 500

white = (240,240,255)
black = (40,40,40)
red = (212,64,28)
green = (40,181,73)
blue = (21,59,176)
xEnd = 0
yEnd = 0
xO = 250
yO = 250
a1 = 80
a2 = 80
wayPt1 = []
wayPt2 = []
Ball_Radius = 5
ball_color = white
q1 = 0
q2 = 0
q3 = 0
q4 = 0
xE = 0
yE = 0
theta = 0
q2Start = math.pi / 2
q1Start = 0

# input destination with reach checking
while (xEnd**2 + yEnd**2) > (a1 + a2)**2 or (xEnd**2 + yEnd**2) == 0:
    xEnd = int(input("enter xEnd: "))
    yEnd = int(input("enter yEnd: "))
ball_pos = [xEnd + xO, yEnd + yO]

# setup display
# Set up display area
mainDisplay = pygame.display.set_mode((Width,Height))

mainDisplay.fill(blue)
pygame.display.set_caption('inverseKinematics 2joint')



#use math.atan2(y, x) to calculate arm angles for end point

q2 = math.acos((xEnd**2 + yEnd**2 - a1*a1 - a2*a2)/(2*a1*a2))

q1 = (math.atan2((yEnd),(xEnd))) - (math.atan2((a2*math.sin(q2)),(a1 + (a2*math.cos(q2)))))

q3 = math.degrees(q2)
q4 = math.degrees(q1)

q2End = q2
q1End = q1
# calculate arm increments
delq2 = ((q2End - q2Start) / 100)
delq1 = ((q1End - q1Start) / 100)

print(int(math.degrees(delq2)))
print(int(math.degrees(delq1)))


print(int(math.degrees(q2End)))
print(int(math.degrees(q1End)))

# starting pose
q1 = q1Start
q2 = q2Start

# Main Loop
for i in range(0, 101):
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            pygame.quit()
            quit()

    plot(q1, q2)
    q1 += delq1
    q2 += delq2
    wayPt1.append(int(math.degrees(q1)))
    wayPt2.append(int(math.degrees(q2)))

    draw(mainDisplay)
    pygame.time.delay(20)

    pygame.display.update()
print(wayPt2, wayPt1)