Raspberry Pi Pico
アナログジョイスティックでサーボモーターを動作させるサンプルプログラムです。
アナログジョイスティックでサーボモーターを動作させるサンプルプログラムです。
信号線とIOの接続はサーボモーターの回転方向と角度によって適宜変更してください。
下記のプログラムはOLEDの表示のため、ssd1306.pyをインポートしてください。
OLEDを使用しない場合はプログラムの変更が必要です。
●使用部品
・Pico （またはPico W）
・電源 3V～5V（電池やUSBバッテリー）
・OLED （使わない場合はプログラムの変更が必要）
・サーボモーター SG90-HV、SG90など
・アナログジョイスティック
●配線図
●プログラム
from machine import Pin, ADC, PWM import time import ssd1306 #オンボードLEDの設定 led=Pin("LED",Pin.OUT) led.off() #モーター出力PWMピン設定1 MP1=PWM(Pin(1,Pin.OUT)) # PWM出力ピン番号を指定 MP1.freq(50) # PWM周波数を設定 #モーター出力PWMピン設定2 MP2=PWM(Pin(2,Pin.OUT)) # PWM出力ピン番号を指定 MP2.freq(50) # PWM周波数を設定 #ディスプレイの設定 i2c=machine.I2C(0,sda=Pin(8),scl=Pin(9)) display=ssd1306.SSD1306_I2C(128,32,i2c) #アナログスティックのピン設定 analogX=ADC(Pin(26)) analogY=ADC(Pin(27)) #アナログスティックの出力％を計算する関数 def math_axis_output1(data,dmin,dmax,dcen,eran): #dmin アナログスティックの最小値 #dmax アナログスティックの最大値 #dcen アナログスティックの中立値 #eran 中立値ハンチング除けのズラシ％量 #-----直線の式を計算(y=ax+b)----- #min50%-max0%のバイナリ値を求める Lmax=0 Lmin=50 La=(dmin-dcen)/(Lmax-Lmin) Lb=dcen-(La*Lmin) Lbase=La*(Lmin-eran)+Lb #例50%-2%=48%とか #min50%-max100%のバイナリ値を求める Umax=100 Umin=50 Ua=(dmax-dcen)/(Umax-Umin) Ub=dcen-(Ua*Umin) Ubase=Ua*(Umin+eran)+Ub #例50%+2%=52%とか #print("Lbase = "+str(Lbase)+" ,Ubase = "+str(Ubase)) #-----ハンチング考慮の直線の式を計算----- if Lbase<data<Ubase: #ハンチング量を考慮した中立範囲 reper=0 elif data<dcen: #プラス側のハンチング量を考慮した出力値を計算 Lmax=0 Lmin=Lmin-eran La=(100-0)/(dmin-Lbase) Lb=0-(La*Lbase) Lper=La*data+Lb reper=round(Lper) elif data>dcen: #マイナス側のハンチング量を考慮した出力値を計算 Umax=100 Umin=Umin+eran Ua=(100-0)/(dmax-Ubase) Ub=0-(Ua*Ubase) Uper=Ua*data+Ub Uper=-1*Uper reper=round(Uper) #print("data = "+str(data)+" ,return% = "+str(reper)) return str(reper) #出力％を戻す #出力からduty比に変更---<角度サーボ用> def math_output_duty_angle1(output): dmax=2.4 #最大値2.4ms pulse 最大角度を小さくする場合は値を小さくする dmin=1.45 #1.45ms pulse＝0度 0度位置を変える場合は値を調整する a=(dmax-dmin)/(100-0) #傾きを計算 b=dmin-0*a #切片を計算（dminを使うか1.45ms以上の固定値を使う） y=a*output+b #値を計算 rduty=y/20*65535 #16bit値に変換（20ms=50Hz） #print("y= "+str(y)+"duty= "+str(rduty)) return int(rduty) #出力からduty比に変更---<360度回転サーボ用> def math_output_duty_rotate1(output): dmax=2.2 #最大値2.4ms pulse 最大付近の変化しない領域を小さくする場合は値を小さくする dmin=1.47 #0度＝1.45ms pulse 回転が止まらない場合は値を±して調整する a=(dmax-dmin)/(100-0) #傾きを計算 b=dmin-0*a #切片を計算（dminを使うか1.45ms以上の固定値を使う） y=a*output+b #値を計算 rduty=y/20*65535 #16bit値に変換（20ms=50Hz） #print("y= "+str(y)+"duty= "+str(rduty)) return int(rduty) while True: led.on() #-----------アナログスティックの出力％を計算------------- #アナログスティックの値を読み込み変換 XValue = analogX.read_u16() YValue = analogY.read_u16() print(str(XValue)+","+str(YValue)) #X軸の出力％を計算----------------------------- dmin=150 #アナログスティックの最小値（最小値を調べて入力） dmax=65535 #アナログスティックの最大値（最大値を調べて入力） dcen=32765 #アナログスティックの中立値（中立値を調べて入力） eran=0.5 #中心値ハンチング除けのズラシ％量（適宜調整） data=XValue X1=math_axis_output1(data,dmin,dmax,dcen,eran) #Y軸の出力％を計算----------------------------- dmin=150 #アナログスティックの最小値（最小値を調べて入力） dmax=65535 #アナログスティックの最大値（最大値を調べて入力） dcen=32662 #アナログスティックの中立値（中立値を調べて入力） eran=0.5 #中心値ハンチング除けのズラシ％量（適宜調整） data=YValue Y1=math_axis_output1(data,dmin,dmax,dcen,eran) #出力％を送信用データ形式に変更 X1='x'+X1 Y1='y'+Y1 #出力データ確認 print(X1+','+Y1) #---------サーボモーターを動かす------------ #軸出力をpwm値に変換してサーボを動作させる dutydata=int(X1.replace("x",'')) MP1.duty_u16(int(math_output_duty_angle1(dutydata))) #軸出力をpwm値に変換してサーボを動作させる dutydata=int(Y1.replace("y",'')) # dutydata=dutydata*-1 #動作方向を反転させる場合は-1を掛ける MP2.duty_u16(int(math_output_duty_rotate1(dutydata))) #--------ディスプレイに表示----------- display.fill(0) display.text(str(XValue)+","+str(YValue),0,0,True) display.text(X1+","+Y1,0,10,True) display.show() # #loop処理の間隔を設定 # time.sleep_ms(10)
●プログラムの説明
ジョイスティックのアナログ抵抗値を中立位置を中心にしてy=ax+bで計算しているだけです。
アナログ抵抗値➡±100％に変換➡サーボ駆動用duty比に変換
100%値に変換する理由はモニター表示で見易いのとbluetooth送信時に送信文字数を減らせるためです。
角度サーボと回転サーボで処理を分けてますが調整する定数が異なるだけで計算は同じにしてます。後で処理を変えるかもしれないと思ったので分けておきました。
スティックの中立位置で抵抗値がハンチングするので対策として前後数％を0として計算しています。
コメント
コメントを投稿