sylvie-2024

Unnamed repository; edit this file 'description' to name the repository.
Log | Files | Refs | README | LICENSE

nanoik_v2.py (2917B)

      1 from math import pi
      2 from trianglesolver import solve, degree
      3 import turtle
      4 
      5 # eez = 0.5
      6 # eex = -0.2
      7 
      8 # lnk1 = 0.33
      9 # lnk2 = 0.29
     10 
     11 # eezL = 0.5625
     12 # eezR = 0.5575
     13 
     14 # foot_dist = 0.14
     15 
     16 ikResult = []
     17 
     18 # stage 1 variables
     19 
     20 a1 = 0
     21 b1 = 0
     22 c1 = 0
     23 
     24 A1 = 0
     25 B1 = 0
     26 C1 = 0
     27 
     28 # stage 2 variables
     29 
     30 a2 = 0
     31 b2 = 0
     32 c2 = 0
     33 
     34 A2 = 0 
     35 B2 = 0
     36 C2 = 0
     37 
     38 # stage 3 variables
     39 
     40 meta_a = 0
     41 meta_b = 0
     42 meta_c = 0
     43 
     44 def solveKinematicsSide(eez, eex, lnk1, lnk2):
     45     if eex < 0:
     46         ikResult = solveKinematicsNeg(eez, eex, lnk1, lnk2)
     47     else:
     48         ikResult = solveKinematicsPos(eez, eex, lnk1, lnk2)
     49 
     50     return [ikResult[0], ikResult[1], ikResult[2]]
     51 
     52 def solveKinematicsPos(eez, eex, lnk1, lnk2):
     53     # stage 1
     54     a,b,c,A,B,C = solve(a=eez, c=abs(eex), B=90*degree)
     55     
     56     b1 = b
     57     A1 = A / degree
     58     C1 = C / degree
     59 
     60     # stage 2
     61     a,b,c,A,B,C = solve(a=lnk2, b=b1, c=lnk1)
     62 
     63     A2 = A / degree
     64     B2 = B / degree
     65     C2 = C / degree
     66 
     67     # stage 3
     68     meta_a = (C1 + A2)
     69     meta_b = (180 - B2)
     70     meta_c = (C2 - C1)
     71 
     72     # drawRadar(meta_a, meta_b, meta_c, lnk1, lnk2, "red")
     73 
     74     return [meta_a, meta_b, meta_c]
     75 
     76 def solveKinematicsNeg(eez, eex, lnk1, lnk2):
     77     # stage 1
     78     a,b,c,A,B,C = solve(a=abs(eex), c=eez, B=90*degree)
     79 
     80     b1 = b 
     81     A1 = A / degree
     82     C1 = C / degree
     83 
     84     # stage 2
     85     a,b,c,A,B,C = solve(a=lnk2, b=b1, c=lnk1)
     86 
     87     A2 = A / degree
     88     B2 = B / degree
     89     C2 = C / degree
     90 
     91     # stage 3 
     92     meta_a = ((C1 + A2) - 90)
     93     meta_b = (180 - B2)
     94     meta_c = (A1 + C2)
     95 
     96     # drawRadar(meta_a, meta_b, meta_c, lnk1, lnk2, "red")
     97 
     98     return [meta_a, meta_b, meta_c]
     99 
    100 def solveKinematicsFront(eezL, eezR, foot_dist):
    101     # stage 1
    102     a,b,c,A,B,C = solve(a=foot_dist, c=(eezR + 0.115), B=90*degree)
    103 
    104     b1 = b
    105     A1 = A / degree
    106     C1 = C / degree
    107 
    108     # stage 2
    109     a,b,c,A,B,C = solve(a=foot_dist, b=b1, c=(eezL + 0.115))
    110 
    111     A2 = A / degree
    112     C2 = C / degree
    113 
    114     # stage 3
    115 
    116     foot_angle = 180 - (A1 + C2)
    117 
    118     # drawRadarFront(foot_angle_R, foot_angle_L, eezL, eezR, foot_dist, "green")
    119 
    120     return foot_angle
    121 
    122 def drawRadarSide(meta_a, meta_b, meta_c, lnk1, lnk2, pencol):
    123     board = turtle.Turtle()
    124 
    125     turtle.resetscreen()
    126     board.color(pencol)
    127 
    128     board.right(90) # draw base
    129     board.forward(40)
    130 
    131     board.left(meta_a)
    132     board.forward(lnk1 * 3)
    133 
    134     board.right(meta_b)
    135     board.forward(lnk2 * 3)
    136 
    137     board.left(meta_c)
    138     board.forward(40)
    139 
    140 def drawRadarFront(foot_angle, eezL, eezR, foot_dist, pencol):
    141     board = turtle.Turtle()
    142 
    143     turtle.resetscreen()
    144     board.color(pencol)
    145 
    146     board.left(90)
    147     board.forward(40)
    148 
    149     board.left(90 - foot_angle)
    150     board.forward((0.115 + eezR) * 3)
    151 
    152     board.right(90)
    153     board.forward(foot_dist * 3)
    154 
    155     board.right(90)
    156     board.forward((0.115 + eezL) * 3)
    157 
    158     board.right(90 - foot_angle)
    159     board.forward(40)
    160 
    161 # print(solveKinematics(eez, eex, lnk1, lnk2))
    162 # print(solveKinematicsFront(eezL, eezR, foot_dist))