Supertastic RoboPi-4leg-Dog

import time
import math
from adafruit_servokit import ServoKit

# Initialize the PCA9685 controller (16 channels)
kit = ServoKit(channels=16)

# Define which channels your 8 servos are connected to
SERVO_CHANNELS = [0, 1, 2, 3, 4, 5, 6, 7]
NUM_SERVOS = len(SERVO_CHANNELS)

# Movement parameters
CENTER_ANGLE = 90    # The neutral/center position of the servo
AMPLITUDE = 45       # Movement range: +/- 45 degrees (90 degrees total)
# Frequency calculation: to achieve 90 degrees/second speed, 
# a full cycle (180 degrees back and forth) should take 2 seconds.
CYCLE_DURATION = 2.0 

print("Robot Controller is running...")
print("Press Ctrl+C to safely stop the robot and release resources.")

try:
    while True:
        current_time = time.time()
        
        for i in range(NUM_SERVOS):
            # Applying phase shift:
            # Servos 1, 3, 5, 7 (indices 0, 2, 4, 6) start at 0 phase
            # Servos 2, 4, 6, 8 (indices 1, 3, 5, 7) are shifted by 90 degrees (pi/2)
            if i % 2 == 0:
                phase_shift = 0
            else:
                phase_shift = math.pi / 2  # 90 degrees shift
            
            # Calculate the target angle using a Sine wave for smooth motion
            # Formula: center + amplitude * sin(2 * pi * frequency * time + phase)
            angle = CENTER_ANGLE + AMPLITUDE * math.sin(
                (2 * math.pi / CYCLE_DURATION) * current_time + phase_shift
            )
            
            # Send the command to the specific servo channel
            kit.servo[SERVO_CHANNELS[i]].angle = angle
            
        # Small sleep to prevent CPU overclocking and I2C bus congestion
        time.sleep(0.01)

except KeyboardInterrupt:
    print("\n[STOP] Keyboard-Interrupt detected.")
    print("Resetting all servos to the center position (90)...")
    
    # Emergency reset: Move all servos to neutral and let them 'rest'
    for channel in SERVO_CHANNELS:
        kit.servo[channel].angle = CENTER_ANGLE
        
    print("Resources released. Bye!")