回答編集履歴
3
修正
answer
CHANGED
@@ -1,8 +1,10 @@
|
|
1
1
|
### ※これは回答ではありません(質問の延長です。)
|
2
2
|
まず、Gyro.pyを含めたコードを示します。
|
3
|
-
Gyro.pyについて atan() ---> atan2() に変更しました。
|
4
|
-
|
3
|
+
変更1回目
|
5
4
|
GUI.pyとImage_Processing.pyについて img_src0関係を削除しました。
|
5
|
+
変更2回目
|
6
|
+
Gyro.pyに関しましては消させていただきます。(文字数確保のため)
|
7
|
+
Image_Processing.pyに関しまして、try-except文の追加、及びhsv変換前にロール角ピッチ角のログをとるようにしました。
|
6
8
|
なお、すべてのコードにおいて関係のなさそうなところを省略させていただきます。
|
7
9
|
```python
|
8
10
|
#GUI.py
|
@@ -60,15 +62,7 @@
|
|
60
62
|
def camera_on(self):
|
61
63
|
self.camera = 1
|
62
64
|
|
63
|
-
#サーボモータ(正面カメラ)のsetup
|
64
|
-
Servo_pin=18
|
65
|
-
GPIO.setmode(GPIO.BCM)
|
66
|
-
GPIO.setup(Servo_pin, GPIO.OUT)
|
67
|
-
self.Servo = GPIO.PWM(Servo_pin, 50)
|
68
|
-
self.Servo.start(0)
|
69
|
-
|
70
65
|
self.camera_set()
|
71
|
-
|
72
66
|
timer = QTimer(self.view1)
|
73
67
|
timer.timeout.connect(self.camera_set)
|
74
68
|
timer.start(300)
|
@@ -92,19 +86,7 @@
|
|
92
86
|
|
93
87
|
if ret == False:
|
94
88
|
return
|
95
|
-
|
96
|
-
|
89
|
+
|
97
|
-
#角度からデューティ比を求める
|
98
|
-
duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180
|
99
|
-
#デューティ比を変更
|
100
|
-
self.Servo.ChangeDutyCycle(duty)
|
101
|
-
|
102
|
-
#ピッチ角分サーボを回転
|
103
|
-
servo_angle(round(self.Pitch))
|
104
|
-
#サーボモータをストップ
|
105
|
-
self.Servo.stop()
|
106
|
-
|
107
|
-
#画像回転
|
108
90
|
height = cv_img.shape[0]
|
109
91
|
width = cv_img.shape[1]
|
110
92
|
center = (int(width/2),int(height/2))
|
@@ -158,11 +140,6 @@
|
|
158
140
|
if self.auto == 0 or self.camera == 0:
|
159
141
|
return
|
160
142
|
|
161
|
-
data= open('/home/pi/Desktop/AutoMove/GyroData.txt','w') #open(ファイル名指定, どんな仕事をさせるか) 'a'→同じファイル名のファイルが存在していた時、既存ファイルに追記
|
162
|
-
# 'w'→同じファイル名のファイルが存在していた時、上書き保存
|
163
|
-
data.write( "ロール角:" + str(round(self.Roll)) + "、ピッチ角:" + str(round(self.Pitch)) + "\n") #文字列(str)p_vaiue3を書き込み+改行
|
164
|
-
data.close #ファイルを閉じる
|
165
|
-
|
166
143
|
variable, elapsed_time = Image_Processing.I_P(self.auto,self.img_src,self.Roll)
|
167
144
|
|
168
145
|
#モーターを動かすプログラム。画像処理(Image_Processing.py)にて得られた変数(variable)を基にモータの出力を変更
|
@@ -190,109 +167,53 @@
|
|
190
167
|
import time
|
191
168
|
import Value
|
192
169
|
|
193
|
-
def I_P(auto,img_src,Roll):
|
170
|
+
def I_P(auto,img_src,Roll,Pitch):
|
171
|
+
try:
|
194
|
-
|
172
|
+
t1 = time.time()
|
195
|
-
|
173
|
+
if auto == 0:
|
196
|
-
|
174
|
+
return
|
197
|
-
|
175
|
+
F=0
|
176
|
+
|
177
|
+
r=str(round(Roll))
|
178
|
+
p=str(round(Pitch))
|
198
179
|
|
199
|
-
|
180
|
+
img_src=cv2.resize(img_src,(180,60))
|
200
|
-
|
181
|
+
img_src=img_src[20:60, 20-round(75*math.cos(math.radians(90-Roll))):140-round(75*math.cos(math.radians(90-Roll)))]
|
201
|
-
#HSV変換
|
202
|
-
hsv = cv2.cvtColor(img_src, cv2.COLOR_BGR2HSV)
|
203
|
-
GREEN_min = np.array([30, 64, 30])
|
204
|
-
GREEN_max = np.array([90, 255,255])
|
205
|
-
img_mask = cv2.inRange(hsv,GREEN_min ,GREEN_max)
|
206
|
-
|
207
|
-
neiborhood8 = np.array([[1, 1, 1],[1, 1, 1],[1, 1, 1]],np.uint8)
|
208
|
-
img_erosion = cv2.erode(img_mask,neiborhood8,iterations=2)
|
209
|
-
img_erosion = cv2.erode(img_erosion,neiborhood8,iterations=2)
|
210
|
-
img_dilation = cv2.dilate(img_erosion,neiborhood8,iterations=2)
|
211
|
-
img_dilation = cv2.dilate(img_dilation,neiborhood8,iterations=2)
|
212
|
-
img_dst = cv2.bitwise_and(img_src, img_src, mask=img_dilation)
|
213
|
-
img_dst2 = cv2.bitwise_and(img_src, img_src, mask=img_mask)
|
214
182
|
|
215
|
-
|
183
|
+
WRIGHT=open('/home/pi/Desktop/PAR.txt','a')
|
216
|
-
|
184
|
+
WRIGHT=("P:"+p+"、R:"+r+"\n")
|
185
|
+
WRIGHT.close
|
217
186
|
|
187
|
+
#HSV変換
|
188
|
+
hsv = cv2.cvtColor(img_src, cv2.COLOR_BGR2HSV)
|
189
|
+
GREEN_min = np.array([30, 64, 30])
|
190
|
+
GREEN_max = np.array([90, 255,255])
|
191
|
+
img_mask = cv2.inRange(hsv,GREEN_min ,GREEN_max)
|
218
192
|
|
193
|
+
neiborhood8 = np.array([[1, 1, 1],[1, 1, 1],[1, 1, 1]],np.uint8)
|
194
|
+
img_erosion = cv2.erode(img_mask,neiborhood8,iterations=2)
|
195
|
+
img_erosion = cv2.erode(img_erosion,neiborhood8,iterations=2)
|
196
|
+
img_dilation = cv2.dilate(img_erosion,neiborhood8,iterations=2)
|
197
|
+
img_dilation = cv2.dilate(img_dilation,neiborhood8,iterations=2)
|
198
|
+
img_dst = cv2.bitwise_and(img_src, img_src, mask=img_dilation)
|
199
|
+
img_dst2 = cv2.bitwise_and(img_src, img_src, mask=img_mask)
|
219
200
|
|
220
|
-
t2 = time.time()
|
221
|
-
|
201
|
+
#Value.pyで変数(variable)を計算
|
202
|
+
row_L , row_R ,variable = Value.Get_MotorParameter(img_mask,1 ,F)
|
222
203
|
|
223
|
-
|
204
|
+
|
224
|
-
```
|
225
|
-
```python
|
226
|
-
#Gyro.py
|
227
|
-
import smbus
|
228
|
-
import math
|
229
|
-
from time import sleep
|
230
205
|
|
206
|
+
t2 = time.time()
|
231
|
-
|
207
|
+
elapsed_time = t2 - t1
|
232
208
|
|
233
|
-
ACCEL_XOUT = 0x3b
|
234
|
-
|
209
|
+
return variable ,elapsed_time
|
235
|
-
ACCEL_ZOUT = 0x3f
|
236
|
-
TEMP_OUT = 0x41
|
237
|
-
GYRO_XOUT = 0x43
|
238
|
-
GYRO_YOUT = 0x45
|
239
|
-
GYRO_ZOUT = 0x47
|
240
210
|
|
211
|
+
except:
|
212
|
+
print("Error")
|
213
|
+
WRIGHT=open('/home/pi/Desktop/PAR.txt','a')
|
214
|
+
WRIGHT=("ERROR"+"\n")
|
241
|
-
|
215
|
+
WRIGHT.close
|
242
|
-
PWR_MGMT_2 = 0x6c
|
243
|
-
|
244
|
-
bus = smbus.SMBus(1)
|
245
|
-
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0)
|
246
|
-
|
247
|
-
|
248
|
-
def read_word(adr):
|
249
|
-
high = bus.read_byte_data(DEV_ADDR, adr)
|
250
|
-
low = bus.read_byte_data(DEV_ADDR, adr+1)
|
251
|
-
val = (high << 8) + low
|
252
|
-
|
216
|
+
variable=0
|
253
|
-
|
254
|
-
|
217
|
+
elapsed_time=0
|
255
|
-
def read_word_sensor(adr):
|
256
|
-
val = read_word(adr)
|
257
|
-
if (val >= 0x8000): # minus
|
258
|
-
return
|
218
|
+
return variable ,elapsed_time
|
259
|
-
else: # plus
|
260
|
-
return val
|
261
|
-
|
262
|
-
|
263
|
-
def get_temp():
|
264
|
-
temp = read_word_sensor(TEMP_OUT)
|
265
|
-
x = temp / 340 + 36.53 # data sheet(register map)記載の計算式.
|
266
|
-
return x
|
267
|
-
|
268
|
-
|
269
|
-
def getGyro():
|
270
|
-
x = read_word_sensor(GYRO_XOUT)/ 131.0
|
271
|
-
y = read_word_sensor(GYRO_YOUT)/ 131.0
|
272
|
-
z = read_word_sensor(GYRO_ZOUT)/ 131.0
|
273
|
-
return [x, y, z]
|
274
|
-
|
275
|
-
|
276
|
-
def getAccel():
|
277
|
-
x = read_word_sensor(ACCEL_XOUT)/ 16384.0
|
278
|
-
y= read_word_sensor(ACCEL_YOUT)/ 16384.0
|
279
|
-
z= read_word_sensor(ACCEL_ZOUT)/ 16384.0
|
280
|
-
return [x, y, z]
|
281
|
-
|
282
|
-
|
283
|
-
|
284
|
-
|
285
|
-
def GyroSensor(Roll, Pitch):
|
286
|
-
ax, ay, az = getAccel()
|
287
|
-
gx, gy, gz = getGyro()
|
288
|
-
|
289
|
-
#print ('{0:4.3f}, {0:4.3f}, {0:4.3f}, {0:4.3f}, {0:4.3f}, {0:4.3f},' .format(gx, gy, gz, ax, ay, az))
|
290
|
-
Roll = math.atan2(ay,az) * 57.324 #57.324 = 360/2π であり、rad から ° に変換
|
291
|
-
Pitch = math.atan2(-ax , math.sqrt( ay* ay+ az*az ) ) * 57.324
|
292
|
-
|
293
|
-
#pitch = math.atan(-ax / (ay*math.sin(roll) + az*math.cos(roll)))
|
294
|
-
|
295
|
-
#print('{0:4.3f}, {0:4.3f},' .format(pitch, roll))
|
296
|
-
|
297
|
-
return Pitch, Roll
|
298
219
|
```
|
2
修正
answer
CHANGED
@@ -287,8 +287,8 @@
|
|
287
287
|
gx, gy, gz = getGyro()
|
288
288
|
|
289
289
|
#print ('{0:4.3f}, {0:4.3f}, {0:4.3f}, {0:4.3f}, {0:4.3f}, {0:4.3f},' .format(gx, gy, gz, ax, ay, az))
|
290
|
-
Roll = math.atan2(ay
|
290
|
+
Roll = math.atan2(ay,az) * 57.324 #57.324 = 360/2π であり、rad から ° に変換
|
291
|
-
Pitch = math.atan2(-ax
|
291
|
+
Pitch = math.atan2(-ax , math.sqrt( ay* ay+ az*az ) ) * 57.324
|
292
292
|
|
293
293
|
#pitch = math.atan(-ax / (ay*math.sin(roll) + az*math.cos(roll)))
|
294
294
|
|
1
修正
answer
CHANGED
@@ -139,7 +139,7 @@
|
|
139
139
|
self.auto = 1
|
140
140
|
GPIO.cleanup()
|
141
141
|
motor.init_GPIO()
|
142
|
-
self.
|
142
|
+
self.pin1 , self.pin2 , self.pin3 , self.pin4 , self.pin5 , self.pin6 = motor.init_GPIO()
|
143
143
|
self.auto_move_set()
|
144
144
|
repeatTime = 10
|
145
145
|
timer = QTimer(self.view1)
|
@@ -149,7 +149,7 @@
|
|
149
149
|
def auto_move_off(self):
|
150
150
|
|
151
151
|
self.auto = 0
|
152
|
-
self.
|
152
|
+
self.motor_S()
|
153
153
|
GPIO.cleanup()
|
154
154
|
print("cleanup")
|
155
155
|
|
@@ -166,7 +166,7 @@
|
|
166
166
|
variable, elapsed_time = Image_Processing.I_P(self.auto,self.img_src,self.Roll)
|
167
167
|
|
168
168
|
#モーターを動かすプログラム。画像処理(Image_Processing.py)にて得られた変数(variable)を基にモータの出力を変更
|
169
|
-
self.Duty_R , self.Duty_L = motor.
|
169
|
+
self.Duty_R , self.Duty_L = motor.motor_start(p_vaiue3,self.pin1 , self.pin2 , self.pin3 , self.pin4 , self.pin5 , self.pin6 )
|
170
170
|
|
171
171
|
variable = str(round(variable,3))
|
172
172
|
self.textbox6.setText(variable)
|
@@ -194,9 +194,7 @@
|
|
194
194
|
t1 = time.time()
|
195
195
|
if auto == 0:
|
196
196
|
return
|
197
|
-
F
|
197
|
+
F=0
|
198
|
-
row_mean_p = row_mean_p2 = h3 = 0
|
199
|
-
row_mean_p3 = 320
|
200
198
|
|
201
199
|
img_src=cv2.resize(img_src,(180,60))
|
202
200
|
img_src=img_src[20:60, 20-round(75*math.cos(math.radians(90-Roll))):140-round(75*math.cos(math.radians(90-Roll)))]
|
@@ -205,7 +203,7 @@
|
|
205
203
|
GREEN_min = np.array([30, 64, 30])
|
206
204
|
GREEN_max = np.array([90, 255,255])
|
207
205
|
img_mask = cv2.inRange(hsv,GREEN_min ,GREEN_max)
|
208
|
-
|
206
|
+
|
209
207
|
neiborhood8 = np.array([[1, 1, 1],[1, 1, 1],[1, 1, 1]],np.uint8)
|
210
208
|
img_erosion = cv2.erode(img_mask,neiborhood8,iterations=2)
|
211
209
|
img_erosion = cv2.erode(img_erosion,neiborhood8,iterations=2)
|
@@ -215,9 +213,9 @@
|
|
215
213
|
img_dst2 = cv2.bitwise_and(img_src, img_src, mask=img_mask)
|
216
214
|
|
217
215
|
#Value.pyで変数(variable)を計算
|
218
|
-
|
216
|
+
row_L , row_R ,variable = Value.Get_MotorParameter(img_mask,1 ,F)
|
219
217
|
|
220
|
-
|
218
|
+
|
221
219
|
|
222
220
|
t2 = time.time()
|
223
221
|
elapsed_time = t2 - t1
|