Servo motory

[admin]

Je docela dobré checknout následující video:
https://www.youtube.com/watch?v=ddlDgUymbxc
které popisuje funkci servo motorů. Dalo by říct, že je jazyk python nativním jazykem pro Raspberry Pi. Mj. se v něm snadno manipuluje s GPIO a proto je využit i v ukázkových videích. Pro ovládání dvou servo motorů na kterých je umístěna kamera používám následující skript.

Kód: Vybrat vše

try:
  fileLastPos = open(sFilename, "r")

  try:
    p1last = int(float(fileLastPos.readline()))
    p2last = int(float(fileLastPos.readline()))
  finally:
    fileLastPos.close()

except IOError:
  print "Error while opening file " + sFilename + "."

cmdargs = str(sys.argv)
arg_count = len(sys.argv)

print ("Args: %s " % cmdargs)
print ("Arg1: %s " % sys.argv[1])
print ("Arg2: %s " % sys.argv[2])

if (len(sys.argv) > 2):

  hor = float(sys.argv[1])
  ver = float(sys.argv[2])
 
  p1min = 4.2
  p1center = 6.7
  p1max = 10
#     4,2|10                  +80               5,8      0,03411
  p1real = p1min + ((90 - ver) * ((p1max - p1min) / 170))

  p1delta = abs(hor - p1last)
  p1time = 0.0025 * p1delta
  print("p1time: " + str(p1time))
 
  p2min = 3.5
  p2center = 7.0
  p2max = 10.5
                         
  p2real = p2min + ((90 - hor) * ((p2max - p2min) / 180))

  p2delta = abs(ver - p2last)
  p2time = 0.0025 * p2delta
  print("p2time: " + str(p2time))
 
  print ["p1real: %0.2f" % p1real]
  print ["p2real: %0.2f" % p2real]

  GPIO.setmode(GPIO.BOARD)
 
  GPIO.setup(13,GPIO.OUT)
  GPIO.setup(11,GPIO.OUT)
 
  p1 = GPIO.PWM(13,50)
  p2 = GPIO.PWM(11,50)
  p1.start(7.5)
  p2.start(7.5)

  p1.ChangeDutyCycle(p1real)
  p2.ChangeDutyCycle(p2real)
  # time.sleep(0.45)
 
  time.sleep(max(p1time, p2time))

  p1.stop()
  p2.stop()

  GPIO.cleanup()

  try:
    fileLastPos = open(sFilename, "w")

    try:
      lastHor = hor
      lastVer = ver
      fileLastPos.write(str(hor) + "\n")
      fileLastPos.write(str(ver) + "\n")
    finally:
      fileLastPos.close()
  except IOError:
    print "Error while writing to file " + sFilename + "."
else:
  print("Nedostatek predanych parametru. Max: hor -90 az 90, ver -90 az 80 Cmd: servo.py hor ver")

Skript vypadá na první pohled složitě, ale ve skutečnosti až tak složitý není. Skript je kalibrovaný na mé servo a s jinými servo motory může fungovat odlišně. Poslední pozici servo motorů se ukládá do souboru /home/pi/servo/lastpos.dat a při přenastavení pozic se údaje ze souboru přečtou a tím lze zjistit rozdíl poslední a aktuální (požadované) polohy. To je dobré k tomu, že lze lépe časovat přesun pomocí time.sleep(max(p1time, p2time)) a to zajistí to, že se servo neklepe, když dorazí do požadované polohy. Z počátku je dobré začít jednoduchým skriptem, který je v uvedeném videu. K detailnímu popisu totho skriptu bych přistoupil až ve chvíli, kdy by to někdo potřeboval.