from dcmotor2 import dcmotor import RPi.GPIO as GPIO import time motor = dcmotor(14, 15, 18) motor.rotate(250) time.sleep(5) motor.rotate(500) time.sleep(5) motor.rotate(1000) time.sleep(5) motor.brake()
from adxl345 import ADXL345 from mpu6050 import MPU6050 from dcmotor2 import dcmotor from pid import PID import math import time adxl345 = ADXL345() mpu6050 = MPU6050() lmotor = dcmotor(14, 15, 18) rmotor = dcmotor(23, 24, 12) pid = PID(2.0, 0, 0, 0, 0, 1024, -1024) pid.setPoint(0.0) xa = 0 za = 0 while True: for i in range(5): axesa = adxl345.getAxes(True) axesm = mpu6050.getAxes() xa = xa + axesa['x'] + axesm['x'] za = za + axesa['z'] + axesm['z'] x = xa/10 z = za/10 ang = math.atan2(z, math.fabs(x)) print ang sped = pid.update(ang) print sped lmotor.rotate(sped) rmotor.rotate(sped) time.sleep(1)