Control 4dof armkit on MakerPi RP2040

 

# Ctrl4dofArmmakerpi.py

from machine import Pin,PWM,ADC

import utime

servo1 = PWM(Pin(12))

servo2 = PWM(Pin(13))

servo3 = PWM(Pin(14))

servo4 = PWM(Pin(15))

pot1 = ADC(26)

pot2 = ADC(27)

pot3 = ADC(28)

servo1.freq(50)

servo2.freq(50)

servo3.freq(50)

servo4.freq(50)

value4 = 5000

bluepressed = False

redpressed = False

bluebtn = Pin(16,Pin.IN)

redbtn = Pin(17,Pin.IN)

in_min = 0

in_max = 65535

out_min = 1000

out_max = 9000

def bluebtn_handler(pin):

    global bluepressed

    if not bluepressed:

        bluepressed=True

    global value4

    timer_reaction = utime.ticks_diff(utime.ticks_ms(),timer_start)

    value4 = value4 + 300

    if value4 > 9000:

        value4 = 9000

    #print(value4)

    

def redbtn_handler(pin):

    global redpressed

    if not redpressed:

        redpressed=True

    global value4

    timer_reaction = utime.ticks_diff(utime.ticks_ms(),timer_start)

    value4 = value4 - 300

    if value4 < 1000:

        value4 = 1000

    #print(value4)

        

servo4.duty_u16(int(5000))

while True:     

    value1 = pot1.read_u16()

    Servo1 = (value1 - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    servo1.duty_u16(int(Servo1))

    

    value2 = pot2.read_u16()

    Servo2 = (value2 - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    servo2.duty_u16(int(Servo2))

    

    value3 = pot3.read_u16()

    Servo3 = (value3 - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

    servo3.duty_u16(int(Servo3))

    

    timer_start = utime.ticks_ms()

    bluebtn.irq(trigger=Pin.IRQ_FALLING,handler=bluebtn_handler)

    redbtn.irq(trigger=Pin.IRQ_FALLING,handler=redbtn_handler)     

    servo4.duty_u16(int(value4))