Mercurial > tote-had

annotate python/robot.py @ 19:455a46355e72 default tip

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add freq to i2c init
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Thu, 14 Apr 2016 00:05:31 +0200
parents 5c99ed631554
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
5
60ccacc24cab First draft of robot.py
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
1 from machine import I2C, Pin
9
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
2 from pyb import delay
5
60ccacc24cab First draft of robot.py
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
3 import ustruct
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
4 import math
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
5
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
6
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
7 I2C_ADDRESS = 0x09
5
60ccacc24cab First draft of robot.py
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
8
60ccacc24cab First draft of robot.py
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
9
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
10 class Leg:
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
11 base = 70 # cm from the center of the robot to the (0,0) of the leg
5
60ccacc24cab First draft of robot.py
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
12
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
13 def __init__(self, i2c, index, x_dir, y_dir):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
14 self.index = index
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
15 self.i2c = i2c
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
16 self.x = 0
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
17 self.y = 0
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
18 self.z = 0
8
7993a8f67e4c Fixing the gait
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 7
diff changeset
19 self.x_dir = x_dir
7993a8f67e4c Fixing the gait
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 7
diff changeset
20 self.y_dir = y_dir
5
60ccacc24cab First draft of robot.py
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents:
diff changeset
21
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
22 def move_to(self, x, y, z):
9
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
23 self.x = min(127, max(-127, int(x)))
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
24 self.y = min(127, max(-127, int(y)))
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
25 self.z = min(127, max(-127, int(z)))
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
26
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
27 def move_by(self, dx=0, dy=0, dz=0):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
28 self.move_to(self.x + dx, self.y + dy, self.z + dz)
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
29
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
30 def rotate_by(self, radians):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
31 x = self.x + self.base
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
32 y = self.y + self.base
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
33 nx = x * math.cos(radians) - y * math.sin(radians) - self.base
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
34 ny = x * math.sin(radians) + y * math.cos(radians) - self.base
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
35 self.move_to(nx, ny, self.z)
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
36
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
37
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
38 class Robot:
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
39 def __init__(self):
19
455a46355e72 Add freq to i2c init
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 11
diff changeset
40 self.i2c = I2C(sda=Pin(0), scl=Pin(2), freq=10000)
7
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
41 self.legs = (
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
42 Leg(self.i2c, 0, -1, 1),
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
43 Leg(self.i2c, 1, -1, -1),
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
44 Leg(self.i2c, 2, 1, -1),
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
45 Leg(self.i2c, 3, 1, 1),
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
46 )
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
47
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
48 def move_by(self, dx=0, dy=0, dz=0):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
49 for leg in self.legs:
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
50 leg.move_by(dx * leg.x_dir, dy * leg.y_dir, dz)
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
51
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
52 def rotate_by(self, radians):
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
53 for leg in self.legs:
6417d860a1ca Add gait.py with the basic creep
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 5
diff changeset
54 leg.rotate_by(radians)
9
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
55
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
56 def update(self):
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
57 data = ustruct.pack(
11
5c99ed631554 Don't send "dirty" flags
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
58 "Bbbbbbbbbbbbb", 0,
5c99ed631554 Don't send "dirty" flags
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
59 self.legs[0].x, self.legs[0].y, self.legs[0].z,
5c99ed631554 Don't send "dirty" flags
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
60 self.legs[1].x, self.legs[1].y, self.legs[1].z,
5c99ed631554 Don't send "dirty" flags
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
61 self.legs[2].x, self.legs[2].y, self.legs[2].z,
5c99ed631554 Don't send "dirty" flags
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 9
diff changeset
62 self.legs[3].x, self.legs[3].y, self.legs[3].z,
9
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
63 )
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
64 while True:
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
65 try:
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
66 self.i2c.writeto(I2C_ADDRESS, data)
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
67 except OSError:
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
68 print("OSError!")
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
69 else:
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
70 break
f33a3b3f3445 Try to get i2c to work
Radomir Dopieralski <sheep-devel@sheep.art.pl>
parents: 8
diff changeset
71 delay(20)