標題: openmv機械臂 [打印本頁]
作者: 55555977    時間: 2020-2-24 12:29
標題: openmv機械臂

  1. import sensor, image, time
  2. from pid import PID
  3. from pyb import Servo

  5. pan_servo=Servo(1)
  6. #tilt_servo=Servo(2)
  7. s1 = Servo(3) # P9 Only for OpenMV3 M7
  8. myservo=s1.angle
  9. s2 = Servo(2) # P9 Only for OpenMV3 M7
  10. myservow=s2.angle

  12. red_threshold  =  (90, 30, -8, -43, 21, 65)

  14. time.sleep(800)
  15. myservo(40)
  16. time.sleep(1000)
  17. myservo(-50)
  18. time.sleep(1000)
  19. myservo(10)
  20. time.sleep(1000)

  22. #for i in range(5):
  23. myservow(20)
  24. time.sleep(800)


  27. pan_pid = PID(p=0.09, i=0, imax=90) #脫機運行或者禁用圖像傳輸，使用這個PID
  28. tilt_pid = PID(p=0.05, i=0, imax=90) #脫機運行或者禁用圖像傳輸，使用這個PID

  30. #pan_pid = PID(p=0.1, i=0, imax=90)#在線調(diào)試使用這個PID
  31. #tilt_pid = PID(p=0.1, i=0, imax=90)#在線調(diào)試使用這個PID

  33. sensor.reset() # Initialize the camera sensor.
  34. sensor.set_pixformat(sensor.RGB565) # use RGB565.
  35. sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
  36. sensor.skip_frames(10) # Let new settings take affect.
  37. sensor.set_auto_whitebal(False) # turn this off.
  38. clock = time.clock() # Tracks FPS.

  40. K=913#the value should be measured

  42. def find_max(blobs):
  43.     max_size=0
  44.     for blob in blobs:
  45.         if blob[2]*blob[3] > max_size:
  46.             max_blob=blob
  47.             max_size = blob[2]*blob[3]
  48.     return max_blob

  50. while(True):
  51.     clock.tick() # Track elapsed milliseconds between snapshots().
  52.     img = sensor.snapshot() # Take a picture and return the image.
  53.     blobs = img.find_blobs([red_threshold])
  54.     if blobs:
  55.         max_blob = find_max(blobs)
  56.         pan_error = max_blob.cx()-img.width()/2
  57.         tilt_error = max_blob.cy()-img.height()/2
  58.         #print("pan_error: ", pan_error)
  59.         img.draw_rectangle(max_blob.rect()) # rect
  60.         img.draw_cross(max_blob.cx(), max_blob.cy()) # cx,
  61.         #print(tilt_servo.angle())
  62.         #print(pan_servo.angle())

  64.         pan_output=pan_pid.get_pid(pan_error,1)/2
  65.         tilt_output=tilt_pid.get_pid(tilt_error,1)
  66.         #print("pan_output",pan_output)
  67.         pan_servo.angle(pan_servo.angle()+pan_output)
  68.         #tilt_servo.angle(tilt_servo.angle()-tilt_output)

  70.         if len(blobs) == 1:
  71.             b = blobs[0]
  72.             Lm = (b[2]+b[3])/2
  73.             length = K/Lm
  74.             print(length)
  75.             print(max_blob.cx(),max_blob.cy())

  77.             widtha=(img.width()/2)*11/10
  78.             widthb=(img.width()/2)*9/10
  79.             heighta=(img.height()/2)*11/10
  80.             heightb=(img.height()/2)*9/10
  81.             if widthb<max_blob.cx()< widtha or heightb< max_blob.cy() < heighta:
  82.                 print(231)
  83.                 if length < 15:
  84.                     print (123)
  85.                     for t in range(4):
  86.                         myservow(70)
  87.                         time.sleep(50)

  89.                     if widthb<max_blob.cx()< widtha or heightb< max_blob.cy() < heighta and length < 13 :
  90.                         while(True):
  91.                             myservo(-40)
  92.                             time.sleep(600)


復(fù)制代碼

作者: aktuan007    時間: 2021-1-24 14:40
這個貌似只有一點啊，能給個完整的不兄弟



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1