from pidev.Cyprus_Commands import Cyprus_Commands_RPi as ccrpi
ccrpi.open_spi()
from pidev import stepper
##led = stepper(port = 1, speed = 50)
##for i in range(1, 255):
## print("lup")
ccrpi.write_pwm(1, ccrpi.COMPARE_MODE, ccrpi.LESS_THAN)
ccrpi.write_pwm(1, ccrpi.PERIOD, .001)
ccrpi.write_pwm(1, ccrpi.COMPARE, .001)
ccrpi.write_pwm(2, ccrpi.COMPARE_MODE, ccrpi.LESS_THAN)
ccrpi.write_pwm(2, ccrpi.PERIOD, .001)
ccrpi.write_pwm(2, ccrpi.COMPARE, .001)
## ccrpi.set_servo_speed(1, -0.5)
motor_2 = stepper(port=0, speed=10, micro_steps=128)
motor_2.home(0)
print("homed motor 2")
sleep(.5)
home_pos_2 = 24.123
motor_2.go_to_position(home_pos_2)
increment = .035
current_pos_x = home_pos_2
current_pos_y = home_pos_1
ccrpi.open_spi()
print("opened spi")
##ccrpi.write_gpio(0)
####
ccrpi.setup_servo()
ccrpi.write_pwm(6, 1, .5)
clamp = lambda n, min_n, max_n: max(min(max_n, n), min_n)
try:
while True:
pygame.event.pump()
joy_val_x = joystick.get_axis(HORIZONTAL_AXIS)
joy_val_y = joystick.get_axis(VERTICAL_AXIS)
current_pos_y = current_pos_y + joy_val_y * increment
current_pos_x = current_pos_x + joy_val_x * increment
current_pos_y = clamp(current_pos_y, home_pos_1 - 13, home_pos_1 + 13)
current_pos_x = clamp(current_pos_x, home_pos_2 - 15.77,
home_pos_2 + 15.77)
motor_1.start_go_to_position(current_pos_y)
motor_2.start_go_to_position(current_pos_x)