/
MasterI2CTest4.py
68 lines (55 loc) · 1.45 KB
/
MasterI2CTest4.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
import smbus
import time
import startup
from vision import vision
from NanoManager import NanoManager
from gpiozero import Button
from picamera import PiCamera
import RPi.GPIO as GPIO
def driveRobot(address, forward, strafe, dist):
bus.write_i2c_block_data(address, 0, [forward, strafe, dist])
waitTime = myNano.getWaitTime(myNano.convertInchesToSteps(dist))
print("waiting: " + str(waitTime))
time.sleep(waitTime)
# This sequence determines the cube's location relative to the robot's location
# then moves accordingly to center itself.
address1 = 10
address2 = 20
bus = smbus.SMBus(1)
camera = PiCamera()
camera.resolution = (600, 600)
camera.rotation = 180
myvision = vision()
myNano = NanoManager()
while True:
print('begin')
while (True):
# read image
camera.capture("center.jpg")
result = myvision.getCenter("center.jpg")
if (result == 1):
print("strafe right")
myNano.driveRobot(address1, 0, 1, 1)
elif (result == -1):
print("strafe left")
myNano.driveRobot(address1, 0, -1, 1)
elif (result == -2):
print("Block not found")
myNano.driveRobot(address1, -1, 0, 1)
else:
print("center")
myNano.moveArm(address2, 3, 70)
time.sleep(2)
break
print('while done')
dist = 12
forward = 1
turn = 0
strafe = 0
angle = 0
print('before signal')
myNano.driveRobot(address1, 1, 0, 12)
myNano.moveArm(address2, 2, 0)
myNano.moveArm(address2, 3, 160)
print('signal sent')
time.sleep(5000000)