import sys
import smbus
import math
import time
from time import sleep # time module
import RPi.GPIO as GPIO
from Servo import Servo
from Gyro import Gyro
GPIO.setmode(GPIO.BCM)
servo_1 = Servo(18, GPIO)
servo_2 = Servo(17, GPIO)
gyro = Gyro()
try:
while (1):
accel_x, accel_y, accel_z = gyro.get_accel_data_g()
x_angle = gyro.get_x_rotation(accel_x, accel_y, accel_z)
y_angle = gyro.get_y_rotation(accel_x, accel_y, accel_z)
servo_1.motor_ctrl(x_angle)
servo_2.motor_ctrl(y_angle)
except KeyboardInterrupt:
print(" == stop ==")
servo_1.stop()
servo_2.stop()
GPIO.cleanup()
