#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)