teratail header banner
teratail header banner
質問するログイン新規登録

回答編集履歴

3

修正

2020/12/01 06:55

投稿

pythonnoob1
pythonnoob1

スコア18

answer CHANGED
@@ -1,8 +1,10 @@
1
1
  ### ※これは回答ではありません(質問の延長です。)
2
2
  まず、Gyro.pyを含めたコードを示します。
3
- Gyro.pyについて atan() ---> atan2() に変更しました。
4
- GUI.pyについて ログをとるようにしました。
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
- def servo_angle(angle):
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
- t1 = time.time()
172
+ t1 = time.time()
195
- if auto == 0:
173
+ if auto == 0:
196
- return
174
+ return
197
- F=0
175
+ F=0
176
+
177
+ r=str(round(Roll))
178
+ p=str(round(Pitch))
198
179
 
199
- img_src=cv2.resize(img_src,(180,60))
180
+ img_src=cv2.resize(img_src,(180,60))
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)))]
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
- #Value.pyで変数(variable)を計算
183
+ WRIGHT=open('/home/pi/Desktop/PAR.txt','a')
216
- row_L , row_R ,variable = Value.Get_MotorParameter(img_mask,1 ,F)
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
- elapsed_time = t2 - t1
201
+ #Value.pyで変数(variable)を計算
202
+ row_L , row_R ,variable = Value.Get_MotorParameter(img_mask,1 ,F)
222
203
 
223
- return variable ,elapsed_time
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
- DEV_ADDR = 0x68
207
+ elapsed_time = t2 - t1
232
208
 
233
- ACCEL_XOUT = 0x3b
234
- ACCEL_YOUT = 0x3d
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
- PWR_MGMT_1 = 0x6b
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
- return val
216
+ variable=0
253
-
254
- # Sensor data read
217
+ elapsed_time=0
255
- def read_word_sensor(adr):
256
- val = read_word(adr)
257
- if (val >= 0x8000): # minus
258
- return -((65535 - val) + 1)
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

修正

2020/12/01 06:55

投稿

pythonnoob1
pythonnoob1

スコア18

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/az) * 57.324 #57.324 = 360/2π であり、rad から ° に変換
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
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

修正

2020/11/17 12:02

投稿

pythonnoob1
pythonnoob1

スコア18

answer CHANGED
@@ -139,7 +139,7 @@
139
139
  self.auto = 1
140
140
  GPIO.cleanup()
141
141
  motor.init_GPIO()
142
- self.p18 , self.p17 , self.p11 , self.p9 , self.p27 , self.p22 = motor.init_GPIO_PWM()
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.manual_motor_S()
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.motor_PWM(p_vaiue3,self.p18 , self.p17 , self.p11 , self.p9 , self.p27 , self.p22 )
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 = F2 = F3 = P = 0
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
- # 8近傍の定義
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
- row_mean_L , row_mean_R ,variable = Value.Get_MotorParameter(img_mask,1 ,F3)
216
+ row_L , row_R ,variable = Value.Get_MotorParameter(img_mask,1 ,F)
219
217
 
220
- F3 = int(variable)
218
+
221
219
 
222
220
  t2 = time.time()
223
221
  elapsed_time = t2 - t1