鹹魚ZTMR實例—機械臂

1個舵機不過癮，來4個搞搞機械臂。

---

SG90舵機X4
範圍180°（-90°~90°）

引腳	說明
暗灰（棕色）	GND
紅色	5V
橙色	X1

PS2手柄

PS2手柄數字	代表舵機	機械臂狀態
5&7	2號&3號舵機	支架上下運動
6&8	1號舵機	機身左右轉
16(方塊)&14（圓圈）	4號舵機	爪開合

例程：“粗糙機械臂”
main.py

# main.py -- put your code here!
import ps2
from pyb import Servo,delay

#定義舵機
s1=Servo(1)
s2=Servo(2)
s3=Servo(3)
s4=Servo(4)
#定義舵機初始位置
s1.angle(0)
s2.angle(0)
s3.angle(0)
s4.angle(0)



def setServoTurn1(flag1):
    turn_angle1 = s1.angle()
    if flag1:
        #逆時針 值遞增 最大值90度
        turn_angle1 += 10 #每按一次轉10度
        if turn_angle1 <= 90:
            s1.angle(turn_angle1)
    else:
        #順時針 值遞減 最小值-90度
        turn_angle1 -= 10
        if turn_angle1 >= -90:
            s1.angle(turn_angle1)
         

def setServoTurn2(flag2):
    turn_angle2 = s2.angle()
    if flag2:
        #逆時針 值遞增 最大值90度
        turn_angle2 += 10 #每按一次轉10度
        if turn_angle2 <= 90:
            s2.angle(turn_angle2)          
    else:
        #順時針 值遞減 最小值-90度
        turn_angle2 -= 10
        if turn_angle2 >= -90:
            s2.angle(turn_angle2)
            

def setServoTurn3(flag3):
    turn_angle3 = s3.angle()
    if flag3:
        #逆時針 值遞增 最大值90度
        turn_angle3 += 10 #每按一次轉10度
        if turn_angle3 <= 90:
            s3.angle(turn_angle3)          
    else:
        #順時針 值遞減 最小值-90度
        turn_angle3 -= 10
        if turn_angle3 >= -90:
            s3.angle(turn_angle3)
            

def setServoTurn4(flag4):
    turn_angle4 = s4.angle()
    if flag4:
        #逆時針 值遞增 最大值90度
        turn_angle4 += 10 #每按一次轉10度
        if turn_angle4 <= 90:
            s4.angle(turn_angle4)
          
    else:
        #順時針 值遞減 最小值-90度
        turn_angle4 -= 10
        if turn_angle4 >= -90:
            s4.angle(turn_angle4)

#比較粗糙，因爲佔時沒有機械臂的套件測試
#不過4個舵機可以按照要求轉動了到時候微調即可


while True: 
    pyb.delay(100)
    print(ps2.ps2_key())  
    if (ps2.ps2_key()==5):        #5或7  舵機2和3轉
        setServoTurn2(True) 
        setServoTurn3(True)             
    elif (ps2.ps2_key()==7):
        setServoTurn2(False)
        setServoTurn3(False)
    elif (ps2.ps2_key()==6):      #6或8  舵機1轉
        setServoTurn1(True)
    elif (ps2.ps2_key()==8):
        setServoTurn1(False)
    elif (ps2.ps2_key()==16):     #16或14  舵機4轉（爪開合）
        setServoTurn4(True)
    elif (ps2.ps2_key()==14):
        setServoTurn4(False)

 

參考

ps2.py

hon
from machine import Pin
import time
def ps2_test():
  print('test ok')
di=Pin(Pin.cpu.C0,Pin.IN,Pin.PULL_DOWN)                     
do=Pin(Pin.cpu.C1,Pin.OUT, )                                             
cs=Pin(Pin.cpu.C2,Pin.OUT,)     #PULL_UP                                          
clk=Pin(Pin.cpu.C3,Pin.OUT,)   
#D0 16  dat=DI
#D2 04  cmd=DO
#D3  0  CS =CS
#D4  02  CLK=CLK                                                      
def DO_H():
  do.value(1)
def DO_L():
  do.value(0)
def CS_H ():
  cs.value(1)
def CS_L():
  cs.value(0)
def CLK_H ():
  clk.value(1)
def CLK_L():
  clk.value(0)

#常量按鈕
PSB_SELECT     = 1
PSB_L3         = 2
PSB_R3         = 3
PSB_START      = 4
PSB_PAD_UP     = 5
PSB_PAD_RIGHT  = 6
PSB_PAD_DOWN   = 7
PSB_PAD_LEFT   = 8
PSB_L2         = 9
PSB_R2         = 10
PSB_L1         = 11
PSB_R1         = 12
PSB_GREEN      = 13
PSB_RED        = 14
PSB_BLUE       = 15
PSB_PINK       = 16
PSB_TRIANGLE   = 13
PSB_CIRCLE     = 14
PSB_CROSS      = 15
PSB_SQUARE     = 26
# 左搖桿
PSS_RX = 5                
PSS_RY = 6
PSS_LX = 7
PSS_LY = 8
mask=[
PSB_SELECT,
    PSB_L3,
    PSB_R3 ,
    PSB_START,
    PSB_PAD_UP,
    PSB_PAD_RIGHT,
    PSB_PAD_DOWN,
    PSB_PAD_LEFT,
    PSB_L2,
    PSB_R2,
    PSB_L1,
    PSB_R1 ,
    PSB_GREEN,
    PSB_RED,
    PSB_BLUE,
    PSB_PINK]
comd=[0x01,0x42]
data=[0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00]
def ps2_init():
  CLK_H()
  DO_H()
  time.sleep_ms(10)
def ps2_cmd(cmd):
  global data
  data[1]=0
  for ref in (1,2,4,8,16,32,64,128):
    if ( ref & cmd):
     DO_H()
    else:
     DO_L()
    CLK_H ()
    time.sleep_us(50)
    CLK_L()
    time.sleep_us(50)
    CLK_H ()
    if(di.value()==1):
     data[1]=ref|data[1]
def ps2_red():
  global data
  global comd
  CS_L()
  ps2_cmd(comd[0])
  ps2_cmd(comd[1])
  CS_H()
  if(data[1]==57):
    return 0#red light
  else:
    return 1#not red
def ps2_read():
  global data
  global comd
  byte=0
  ref=0x01
  CS_L()
  ps2_cmd(comd[0])
  ps2_cmd(comd[1])
  for byte in (2,3,4,5,6,7,8):
    for ref in (1,2,4,8,16,32,64,128):
     CLK_H ()
     CLK_L()
     time.sleep_us(50)
     CLK_H ()
     if(di.value()==1):
      data[byte]= ref|data[byte]
    time.sleep_us(50)
  CS_H ()
def ps2_clear():#ok
  global data
  for i in range(0,9,1):
    data[i]=0

def ps2_andata(button):
  global data
  return data[button]
def ps2_key():
  global data
  global mask
  ps2_clear()
  ps2_read()
  handkey=(data[4]<<8)|data[3]
  for index in (0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15):
    if (( handkey&(1<<(mask[index]-1)))==0):
     return index+2
  return 0

到時候補充個機械臂的效果視頻（/捂臉笑）