#https://jimirobot.tw/esp32-micropython-tutorial-pwm-control-202/
#https://github.com/pvanallen/esp32-getstarted/blob/master/docs/servo.md
import time
import machine
from machine import Timer
from servo import Servo
import StateMachine as mm
import math
SM1=mm.StateMachine(36,'伺服馬達_1')#36*5=180
servo_pin1 = machine.Pin(4)
Servo1 = Servo(servo_pin1)
Servo1.write_angle(0)
servo_pin2 = machine.Pin(5)
Servo2 = Servo(servo_pin2)
Servo2.write_angle(0)
servo_pin3 = machine.Pin(18)
Servo3 = Servo(servo_pin3)
Servo3.write_angle(0)
servo_pin4 = machine.Pin(19)
Servo4 = Servo(servo_pin4)
Servo4.write_angle(0)
# 伺服馬達_1::0-180每隔30度動一次,1sec
# 伺服馬達_2::180-0每隔45度動一次,0.5sec
def ServoFun1(x):
#SM1.ShowState()
dt=3.14/180*(SM1.Count*5)
#------------------
deg1=int(math.sin(dt)*180)
Servo1.write_angle(deg1)
#---------------------------------
deg2=int(abs(math.sin(dt+45)*180))
#print(deg1,deg2)
Servo2.write_angle(deg2)
#---------------------------------
deg3=int(abs(math.sin(dt+90)*180))
Servo3.write_angle(deg3)
#---------------------------------
deg4=int(abs(math.sin(dt+135)*180))
Servo4.write_angle(deg4)
#-------------------------------
SM1.Next()
tim0 = Timer(0) #timer ID=0
tim0.init(period=100, mode=Timer.PERIODIC, callback=ServoFun1)