选中内容(绿色)时除了会搜索文章名,还会搜索文章内容
点击结果中的文章名进入文章界面后可以按Ctrl+F在页面内搜索
  • 版权:CC BY-SA 4.0
  • 创建:2020-08-20
  • 更新:2020-08-31
在 K210 上使用MaixPy 编写一个简单的云台程序, 实现人脸跟随


效果视频

硬件

  • 可以运行 MaixPy 的 k210 开发板
  • 两轴舵机,扭力尽量大一点,会更稳,上面视频使用的20KG扭力的舵机, 参数:
    • 频率: 50 (Hz)
    • 周期T: 1/50 = 0.02s = 20ms
    • 占空比 duty: [0.5ms, 2.5ms] -> [0.025, 0.125] -> [2.5%, 12.5%]
  • 硬件连接
    • IO24 <—> pitch(舵机俯仰轴)
    • IO25 <—> roll (舵机横滚轴,这里实际是控制的航向轴)

实现

  • 使用人脸检测模型, 可以在这里下载到
  • 使用 kflash_gui 烧录模型到 0x300000
  • 运行下面的代码即可

源代码

代码在这里: https://github.com/sipeed/MaixPy_scripts/tree/master/demo/gimbal

  1. import time, sys
  2. from machine import Timer,PWM
  3. from fpioa_manager import board_info
  4. from math import pi
  5. from machine import UART
  6. class Servo:
  7. def __init__(self, pwm, dir=50, duty_min=2.5, duty_max=12.5):
  8. self.value = dir
  9. self.pwm = pwm
  10. self.duty_min = duty_min
  11. self.duty_max = duty_max
  12. self.duty_range = duty_max -duty_min
  13. self.enable(True)
  14. self.pwm.duty(self.value/100*self.duty_range+self.duty_min)
  15. def enable(self, en):
  16. if en:
  17. self.pwm.enable()
  18. else:
  19. self.pwm.disable()
  20. def dir(self, percentage):
  21. if percentage > 100:
  22. percentage = 100
  23. elif percentage < 0:
  24. percentage = 0
  25. self.pwm.duty(percentage/100*self.duty_range+self.duty_min)
  26. def drive(self, inc):
  27. self.value += inc
  28. if self.value > 100:
  29. self.value = 100
  30. elif self.value < 0:
  31. self.value = 0
  32. self.pwm.duty(self.value/100*self.duty_range+self.duty_min)
  33. class PID:
  34. _kp = _ki = _kd = _integrator = _imax = 0
  35. _last_error = _last_t = 0
  36. _RC = 1/(2 * pi * 20)
  37. def __init__(self, p=0, i=0, d=0, imax=0):
  38. self._kp = float(p)
  39. self._ki = float(i)
  40. self._kd = float(d)
  41. self._imax = abs(imax)
  42. self._last_derivative = None
  43. def get_pid(self, error, scaler):
  44. tnow = time.ticks_ms()
  45. dt = tnow - self._last_t
  46. output = 0
  47. if self._last_t == 0 or dt > 1000:
  48. dt = 0
  49. self.reset_I()
  50. self._last_t = tnow
  51. delta_time = float(dt) / float(1000)
  52. output += error * self._kp
  53. if abs(self._kd) > 0 and dt > 0:
  54. if self._last_derivative == None:
  55. derivative = 0
  56. self._last_derivative = 0
  57. else:
  58. derivative = (error - self._last_error) / delta_time
  59. derivative = self._last_derivative + \
  60. ((delta_time / (self._RC + delta_time)) * \
  61. (derivative - self._last_derivative))
  62. self._last_error = error
  63. self._last_derivative = derivative
  64. output += self._kd * derivative
  65. output *= scaler
  66. if abs(self._ki) > 0 and dt > 0:
  67. self._integrator += (error * self._ki) * scaler * delta_time
  68. if self._integrator < -self._imax: self._integrator = -self._imax
  69. elif self._integrator > self._imax: self._integrator = self._imax
  70. output += self._integrator
  71. return output
  72. def reset_I(self):
  73. self._integrator = 0
  74. self._last_derivative = None
  75. class Gimbal:
  76. def __init__(self, pitch, pid_pitch, roll=None, pid_roll=None, yaw=None, pid_yaw=None):
  77. self._pitch = pitch
  78. self._roll = roll
  79. self._yaw = yaw
  80. self._pid_pitch = pid_pitch
  81. self._pid_roll = pid_roll
  82. self._pid_yaw = pid_yaw
  83. def set_out(self, pitch, roll, yaw=None):
  84. pass
  85. def run(self, pitch_err, roll_err=50, yaw_err=50, pitch_reverse=False, roll_reverse=False, yaw_reverse=False):
  86. out = self._pid_pitch.get_pid(pitch_err, 1)
  87. # print("err: {}, out: {}".format(pitch_err, out))
  88. if pitch_reverse:
  89. out = - out
  90. self._pitch.drive(out)
  91. if self._roll:
  92. out = self._pid_roll.get_pid(roll_err, 1)
  93. if roll_reverse:
  94. out = - out
  95. self._roll.drive(out)
  96. if self._yaw:
  97. out = self._pid_yaw.get_pid(yaw_err, 1)
  98. if yaw_reverse:
  99. out = - out
  100. self._yaw.drive(out)
  101. if __name__ == "__main__":
  102. '''
  103. servo:
  104. freq: 50 (Hz)
  105. T: 1/50 = 0.02s = 20ms
  106. duty: [0.5ms, 2.5ms] -> [0.025, 0.125] -> [2.5%, 12.5%]
  107. pin:
  108. IO24 <--> pitch
  109. IO25 <--> roll
  110. '''
  111. init_pitch = 80 # init position, value: [0, 100], means minimum angle to maxmum angle of servo
  112. init_roll = 50 # 50 means middle
  113. sensor_hmirror = False
  114. sensor_vflip = False
  115. lcd_rotation = 2
  116. lcd_mirror = True
  117. pitch_pid = [0.23, 0, 0.015, 0] # P I D I_max
  118. roll_pid = [0.23, 0, 0.015, 0] # P I D I_max
  119. target_err_range = 10 # target error output range, default [0, 10]
  120. target_ignore_limit = 0.02 # when target error < target_err_range*target_ignore_limit , set target error to 0
  121. pitch_reverse = False # reverse out value direction
  122. roll_reverse = True # ..
  123. import sensor,image,lcd
  124. import KPU as kpu
  125. class Target():
  126. def __init__(self, out_range=10, ignore_limit=0.02, hmirror=False, vflip=False, lcd_rotation=2, lcd_mirror=True):
  127. self.pitch = 0
  128. self.roll = 0
  129. self.out_range = out_range
  130. self.ignore = ignore_limit
  131. self.task_fd = kpu.load(0x300000) # face model addr in flash
  132. anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
  133. kpu.init_yolo2(self.task_fd, 0.5, 0.3, 5, anchor)
  134. lcd.init()
  135. lcd.rotation(lcd_rotation)
  136. lcd.mirror(lcd_mirror)
  137. sensor.reset()
  138. sensor.set_pixformat(sensor.RGB565)
  139. sensor.set_framesize(sensor.QVGA)
  140. if hmirror:
  141. sensor.set_hmirror(1)
  142. if vflip:
  143. sensor.set_vflip(1)
  144. def get_target_err(self):
  145. img = sensor.snapshot()
  146. code = kpu.run_yolo2(self.task_fd, img)
  147. if code:
  148. max_area = 0
  149. max_i = 0
  150. for i, j in enumerate(code):
  151. a = j.w()*j.h()
  152. if a > max_area:
  153. max_i = i
  154. max_area = a
  155. img = img.draw_rectangle(code[max_i].rect())
  156. self.pitch = (code[max_i].y() + code[max_i].h() / 2)/240*self.out_range*2 - self.out_range
  157. self.roll = (code[max_i].x() + code[max_i].w() / 2)/320*self.out_range*2 - self.out_range
  158. # limit
  159. if abs(self.pitch) < self.out_range*self.ignore:
  160. self.pitch = 0
  161. if abs(self.roll) < self.out_range*self.ignore:
  162. self.roll = 0
  163. img = img.draw_cross(160, 120)
  164. lcd.display(img)
  165. return (self.pitch, self.roll)
  166. else:
  167. img = img.draw_cross(160, 120)
  168. lcd.display(img)
  169. return (0, 0)
  170. target = Target(target_err_range, target_ignore_limit, sensor_hmirror, sensor_vflip, lcd_rotation, lcd_mirror)
  171. tim0 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
  172. tim1 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
  173. pitch_pwm = PWM(tim0, freq=50, duty=0, pin=24)
  174. roll_pwm = PWM(tim1, freq=50, duty=0, pin=25)
  175. pitch = Servo(pitch_pwm, dir=init_pitch)
  176. roll = Servo(roll_pwm, dir=init_roll)
  177. pid_pitch = PID(p=pitch_pid[0], i=pitch_pid[1], d=pitch_pid[2], imax=pitch_pid[3])
  178. pid_roll = PID(p=roll_pid[0], i=roll_pid[1], d=roll_pid[2], imax=roll_pid[3])
  179. gimbal = Gimbal(pitch, pid_pitch, roll, pid_roll)
  180. target_pitch = init_pitch
  181. target_roll = init_roll
  182. t = time.ticks_ms()
  183. _dir = 0
  184. t0 = time.ticks_ms()
  185. stdin = UART.repl_uart()
  186. while 1:
  187. # get target error
  188. err_pitch, err_roll = target.get_target_err()
  189. # interval limit to > 10ms
  190. if time.ticks_ms() - t0 < 10:
  191. continue
  192. t0 = time.ticks_ms()
  193. # run
  194. gimbal.run(err_pitch, err_roll, pitch_reverse = pitch_reverse, roll_reverse=roll_reverse)
文章有误?有想法想讨论?查看或者发起勘误/讨论 主题
(发起评论需要先登录 github)

/wallpaper/wallhaven-6omkp7.jpg