/Users/jaycesheachow/Public/Leg_RD.

py
Saved: 12/16/15, 9:08:05 PM
1
2

Page 1 of 4
Printed For: Jayce Shea Chow

"""Leg Class for HExBOT"""

from math import *

3
4
5

class Leg():

6
7
8
9
10

def __init__(self, index):

self.lim = False
self.index = index
self.angleoffset = (60 * (index + 1)) % 360

# angle from horizontal

11
12

self.angles = []

13
14
15
16
17
18

#is even?
if index % 2 == 0:
self.even = True
else:
self.even = False

19
20
21
22
23
24
25
26
27
28
29
30
31

# current position of foot before offsets

if self.index > 3:
self.x = 5
else:
self.x = -5
if self.index == 1 or self.index == 6:
self.y = 3
elif self.index == 3 or self.index == 4:
self.y = -3
else:
self.y = 0

32
33
34
35
36

if self.even:
self.z = -4
else:
self.z = 0

37
38
39

# whether the leg is currently rising or lowering at the end of a st

self.switching = False

40
41

# whether the leg is currently on the ground and able to support wei

42
43
44
45

r = 3 # radius from center of hexbot to leg origin (theta servo) in

self.xoffset = r * cos(self.angleoffset*pi/180)
self.yoffset = r * sin(self.angleoffset*pi/180)

/Users/jaycesheachow/Public/Leg_RD.py
Saved: 12/16/15, 9:08:05 PM

Page 2 of 4
Printed For: Jayce Shea Chow

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

# the amount shifted out and forward the step pattern is,
# relative to a side leg. 0 for now, unless needed
'''
yoffset = 4
xoffset = -1
if self.index == 1:
self.stepyoffset = yoffset
self.stepxoffset = -xoffset
elif self.index == 3:
self.stepyoffset = -yoffset
self.stepxoffset = -xoffset
elif self.index == 4:
self.stepyoffset = -yoffset
self.stepxoffset = xoffset
elif self.index == 6:
self.stepyoffset = yoffset
self.stepxoffset = xoffset
else:
self.stepyoffset = 0
self.stepxoffset = 0
'''
# lengths of each leg link
self.L0 = 1
self.L1 = 4
self.L2 = 5

72
73
74
75
76
77
78

def findangles(self, x, y, z):

"""
Takes in the x, y, and z positions of the foot relative to the leg o
Returns the theta, phi, and psi angles.
"""
# move to correct horizontal position

79
80
81
82
83

xy_dist = sqrt(x**2 + y**2) - self.L0 # distance to the foot in the

abs_dist = sqrt(xy_dist**2 + z**2) # 3D distance to the end of the
angle_down = degrees(atan2(z, xy_dist)) # angle down from the xy pl
# law of cosines to find angles

84
85
86

v = degrees(acos(((self.L1**2) - (self.L2**2) + (abs_dist**2)) / (2

w = degrees(acos(((self.L1**2) + (self.L2**2) - (abs_dist**2)) / (2

87
88
89
90

tha = degrees(atan2(y, x)) - self.angleoffset

phi = (angle_down + v)
if phi >= 90:

/Users/jaycesheachow/Public/Leg_RD.py
Saved: 12/16/15, 9:08:05 PM
91
92

Page 3 of 4
Printed For: Jayce Shea Chow

phi = 90
psi = w

93
94
95
96
97

#make
tha =
phi =
psi =

range -180 to 180

(tha+180)%360-180
(phi+180)%360-180
(psi+180)%360-180

98
99

return [tha, phi, psi]

100
101
102
103
104
105
106
107

def returnangles(self):
"""
returns the theta, phi, and psi angles
that correspond to its current position
"""
x_real = self.x# + self.stepxoffset
y_real = self.y# + self.stepyoffset

108
109
110

angles = self.findangles(x_real, y_real, self.z)

return angles

111
112
113
114
115
116
117
118
119
120
121
122
123

def movespeed(self, dx, dy, z, ds, runrate):

"""
takes in the speed the hexbot is moving in inches per second
and the rate of the loop running in hertz
updates the leg.x, y, and z
automatically raises and lowers legs
returns array of 18 angles to send to arduino.
"""
persecond = runrate # times the loop runs per second
# multiplying by onground makes foot move in opposite direction to d
xmove = dx/float(persecond)
ymove = dy/float(persecond)

124
125
126
127

abs_x = self.x + self.xoffset

abs_y = self.y + self.yoffset

128
129
130
131
132

new_x = sqrt(abs_x**2 + abs_y**2)*cos(atan2(abs_y, abs_x)+(ds*pi/180

new_y = sqrt(abs_x**2 + abs_y**2)*sin(atan2(abs_y, abs_x)+(ds*pi/180
self.x = new_x - self.xoffset
self.y = new_y - self.yoffset

133
134
135

self.x += xmove

/Users/jaycesheachow/Public/Leg_RD.py
Saved: 12/16/15, 9:08:05 PM
136
137

Page 4 of 4
Printed For: Jayce Shea Chow

self.y += ymove
self.z = z

138
139
140

outangles = self.returnangles()
return outangles

141
142
143

def check_limit(self):
angles1 = self.returnangles()

144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161

if (((angles1[0]+180)%360-180 < -45 and (self.angles[0] - angles1[0]

((angles1[0]+180)%360-180 > 45 and (self.angles[0] - angles1[0])
(angles1[1] < 0 and (self.angles[1] - angles1[1]) > 0) or
(angles1[1] > 88 and (self.angles[1] - angles1[1]) < 0) or
(angles1[2] < 30 and (self.angles[2] - angles1[2]) > 3) or
(angles1[2] > 110 and (self.angles[2] - angles1[2]) < -3)):
print 'ahh'
print self.index
#print (angles1[0]+180)%360-180
#print self.angles[0] - angles1[0]
self.angles = angles1
self.lim = True
return True
else:
self.angles = angles1
self.lim = False
return False

162
163
164
165
166

if __name__ == '__main__':
leg = Leg(3)
print leg.findangles(-5, 0, 0)