Scribd
Upload
73 views4 pages

Leg RD

This document defines a Leg class for a hexapod robot. The class initializes leg properties like index number and angle offsets. It contains methods to find the leg's joint angles based on its position, return the current angles, move the leg based on the robot's speed, and check if the leg has reached its limit angles.

Uploaded by

api-302821890
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
73 views4 pages

Leg RD

This document defines a Leg class for a hexapod robot. The class initializes leg properties like index number and angle offsets. It contains methods to find the leg's joint angles based on its position, return the current angles, move the leg based on the robot's speed, and check if the leg has reached its limit angles.

Uploaded by

api-302821890
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 4

/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)

You might also like