-
Notifications
You must be signed in to change notification settings - Fork 1
/
buggy.py
176 lines (151 loc) · 6.04 KB
/
buggy.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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
#!/usr/bin/python3.4
#####################################################
## This is the buggyBot ##
## the first robotics Project from dandrews7396 ##
#####################################################
from datetime import datetime
import time
import os
import numpy as np
import explorerhat as eh
import cv2
import scipy
from picamera import PiCamera
from io import BytesIO, StringIO
import matplotlib.image as mpimg
import matplotlib.pyplot as plt
from mpu6050 import mpu6050
## Set-up a class that forms the basis for our buggyBot this will allow us to update
## the bot as we go.
class buggyBot():
"""A buggy that's going to roam over the house. A lot of these will be None."""
def __init__(self):
self.start_time = None # Start system time
self.time = None # Start the navigation time
self.total_time = None # Total time passed
self.img = None # No image to begin with
self.img_array = np.empty((240, 320, 3), dtype=np.uint8) # Image will also be saved as np array, so no need to convert.
self.yaw = None # Not yet initialised
self.pitch = None # Not yet initialised
self.roll = None # Not yet initialised
self.vel = None # Not yet initialised
self.steer = None # Not yet initialised
self.motor1 = eh.motor.one
self.motor2 = eh.motor.two
self.max_vel = 100
## Add in address using sudo i2cdetect -y 1
self.mpu_sensor = mpu6050(0x68)
self.x_acc_ctr = None ## These will need
self.y_acc_ctr = None ## Populating on flat, level
self.z_acc_ctr = None ## When stationary.
self.camera = PiCamera()
self.buggy_vision = np.zeros((160,230,3), dtype = np.float)
self.world_map = np.zeros((200, 200, 3), dtype = np.float)
self._img_file = './images/'
def update(self):
""" Update buggyBot as time passes."""
if self.start_time == None:
self.start_time = time.time()
self.total_time = 0
if self.mpu_sensor.read_accel_range != 2:
self.mpu_sensor.set_accel_range(0x00)
if self.mpu_sensor.read_gyro_range != 250:
self.mpu_sensor.set_gyro_range(0x00)
self.camera.resolution = (320, 240)
self.camera.framerate = 24
self.camera.rotation = 180
self.img, self.img_array = self._take_picture()
self.display_image(init=True)
else:
tot_time = time.time() - self.start_time
if np.isfinite(tot_time):
self.total_time = tot_time
## If using an arduino, see `accelArdData.py` for helper code
mpu_data = self.mpu_sensor.get_all_data()
## Output mpu_data to console, for debug
print(mpu_data)
mpu_temp = mpu_data[0]
mpu_accel = mpu_data[1]
mpu_gyro = mpu_data[2]
self.pitch = self._calc_pitch(mpu_accel)
self.roll = self._calc_roll(mpu_accel)
self.img, self.img_array = self._take_picture()
self.display_image()
def _calc_pitch(self, accel_data):
""" Calculate pitch angle of buggyBot."""
x = accel_data["x"]
y = accel_data["y"]
z = accel_data["z"]
denom = np.sqrt(y**2 + z**2)
return np.arctan2(x, denom)
def _calc_roll(self, accel_data):
""" Calculate roll angle of buggyBot."""
x = accel_data["x"]
y = accel_data["y"]
z = accel_data["z"]
denom = np.sqrt(x**2 + z**2)
return np.arctan2(y, denom)
def _take_picture(self):
""" Take picture as both standard image an np array."""
timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3]
array_out = np.empty((240, 320, 3), dtype=np.uint8)
self.camera.capture(array_out, 'rgb')
std_img = self.camera.capture(self._img_file + '{}.jpg'.format(timestamp))
return std_img, array_out
def display_image(self, init=False):
"""Display latest image, overlay with IMU data"""
plt.clf()
images = sorted(os.listdir(self._img_file))
imgpath = self._img_file + images[-1]
img = mpimg.imread(imgpath)
plt.imshow(img)
if self.pitch and self.roll:
txt = 'Pitch: {:.3f}\nRoll: {:.3f}'.format(self.pitch, self.roll)
overlay = plt.text(310, 50, txt,
horizontalalignment='right', color='white')
plt.axis('off')
plt.draw()
if init:
plt.show(block=False)
def forwards(self, speed=100):
"""
This method is taken straight from the gpiozero library.
Simply calls the forward method for both motors via explorerhat
Speed is defaulted to 100, to match explorerhat default.
"""
self.motor1.forwards(speed)
self.motor2.forwards(speed)
def backwards(self, speed=100):
"""
Again taken from gpiozero, calls explorerhat motor `backwards()`
method for both motors.
speed is again set to 100, as per the explorerhat default.
"""
self.motor1.backwards(speed)
self.motor2.backwards(speed)
def stop(self):
"""
Calls speed = 0 on both motors, via the explorerhat
`motor.stop()` method.
"""
self.motor1.stop()
self.motor2.stop()
## These following methods will need to be augmented
## with either an internal or external function to
## match a required heading, once an integrated
## heading sensor has been installed.
## Can be used 'as is' for an RC solution.
def turn_left(self, speed=100):
"""
Call motor methods in opposite directions,
in order to turn left.
left backwards, right forwards. same speed.
"""
self.motor1.backwards(speed)
self.motor2.forwards(speed)
def turn_right(self, speed=100):
"""
As before, opposite direction.
"""
self.motor1.forwards(speed)
self.motor2.backwards(speed)