manual.py

import pygame
from PCA9685 import PCA9685

def clamp(x):
    return max(2500, min(500, x))

pygame.init()

FPS = 30
clock = pygame.time.Clock()

screen = pygame.display.set_mode((1080, 720))

claws_position = 1000
yaw_position = 1000
up_down_position = 1000
forward_backward_postion = 1000

is_running = True

pwm = PCA9685(0x40, debug=False)
pwm.setPWMFreq(50)

while is_running: 
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            is_running = False
        
        if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_LEFT:
                yaw_position += 5
            if event.key == pygame.K_RIGHT:
                yaw_position -= 5
            if event.key == pygame.K_UP:
                forward_backward_postion += 5
            if event.key == pygame.K_DOWN:
                forward_backward_postion -= 5
            if event.key == pygame.K_w:
                up_down_position += 5
            if event.key == pygame.K_s:
                up_down_position -= 5
            if event.key == pygame.K_a:
                claws_position += 5
            if event.key == pygame.K_d:
                claws_position -= 5
    
    pwm.setServoPulse(0, clamp(claws_position))
    pwm.setServoPulse(1, clamp(yaw_position))
    pwm.setServoPulse(2, clamp(up_down_position))
    pwm.setServoPulse(3, clamp(forward_backward_postion))

    clock.tick(FPS)