# we have an RP2040
#https://docs.micropython.org/en/latest/rp2/quickref.html#pins-and-gpio
#https://how2electronics.com/pwm-usage-in-raspberry-pi-pico-breathing-led-example/
#https://docs.wokwi.com/parts/wokwi-servo



#import classes
from machine import Pin, Timer, PWM    #importing pin, and timer class
from time import sleep
import sys
import time




def Main():
    #set up variables
    time_2 = 2
    time_sleep = 1
    time_1 = 1
        ################################################################
# this function reads the tuple
#drives the nominated servoe[0]
#rotating the servo to the nominated angle [1]
#waiting for the set time [2]
    def Servo_Move():   # reads the tuple and drives the nominated servo
        print(Arm_Servo_Write[0])
        print(Arm_Servo_Write[1])
        print(Arm_Servo_Write[2])
        Servo_Pin = (((Arm_Servo_Write[0])*2))
        Servo_Angle = ((Arm_Servo_Write[1])*67)
        Servo_Wait = (Arm_Servo_Write[2])
        print(Servo_Pin)
        print(Servo_Angle)
        print(Servo_Wait)
        time.sleep(Servo_Wait)

        Servo_Pin_Out = PWM(Pin(Servo_Pin))
        Servo_Pin_Out.freq(50)
        Servo_Pin_Out.duty_u16(Servo_Angle)
################################################################
#def Servo_Reset

################################################################
    #servo movment commands
    Arm_Servo_Write = (1,45,time_1)
    Servo_Move()
    time.sleep(0.001)
    Arm_Servo_Write = (2, 100, time_1)
    Servo_Move()
    time.sleep(0.001)
    Arm_Servo_Write = (3, 25, time_1)
    Servo_Move()
    time.sleep(0.001)
    time.sleep(time_sleep)
    Arm_Servo_Write = (4, 100, time_1)
    Servo_Move()
    time.sleep(0.001)
    Arm_Servo_Write = (5, 35, time_1)
    Servo_Move()
    time.sleep(0.001)
    Arm_Servo_Write = (6, 60, time_1)
    Servo_Move()
    time.sleep(0.001)
####################################################################
T==1
while T==1:
  Main()




print("hello terry")

RED_LED= Pin(16, Pin.OUT)              # GPIO16 as LED output
RED_LED.value(0)
sleep(1)
RED_LED.value(1)              #RED_LED is on
sleep(1)
RED_LED.value(0)