Hi all,
I am having some trouble reading ultrasonic sensors and controlling servos at the same time. I think it has to do with reading and writing to pins in the same block of code. Everything works fine on its own, however when I try and send it complicated servo commands it starts doing whatever it feels like doing. Anyone have a solution?
Python code below, arduino attached.
Thank you!
#import servo
import time
import math
import serial
usbport = '/dev/tty.usbmodem1411' #usb from arduino
#usbport = '/dev/tty.usbmodem14141' #usb with extension cable
ser = serial.Serial(usbport, 9600, timeout=0)
def move(servo, angle):
if (0 <= angle <= 180):
ser.write(chr(255))
ser.write(chr(servo))
ser.write(chr(angle))
else:
print("Servo angle must be an integer between 0 and 180.\n")
def vectoradd(x1,y1,x2,y2,r):
powers=dict()
motors=[]
x1 *= -1
x2 *= -1
theta=math.atan2(y2-y1,x2-x1)
a=math.cos(theta)-math.sin(theta)
b=math.cos(theta)+math.sin(theta)
motor1=(a*-1)+r
motor2=(b*-1)+r
motor3=b+r
motor4=a+r
motors=[motor1, motor2, motor3, motor4]
largest=max(motors)
for motor in range(0,4):
if motors[motor] == 0:
powers["motor"+str(motor+1)]=90
if motors[motor] > 0:
powers["motor"+str(motor+1)]=round((motors[motor]/largest)*90)+90
if (motors[motor] < 0):
powers["motor"+str(motor+1)]=90+round((motors[motor]/largest)*90)
return powers
#vectoradd(0,0,2,1)
def run(x1,y1,x2,y2,r,runtime):
powers=vectoradd(x1,y1,x2,y2,r)
current=0
time.sleep(1)
start=time.time()
while ((current-start)<=runtime):
move(1,int(powers["motor1"]))
move(2,int(powers["motor2"]))
move(3,int(powers["motor3"]))
move(4,int(powers["motor4"]))
current=time.time()
#print(current-start)
start=time.time()
while (current-start <= 1):
move(2,90)
move(4,90)
move(3,90)
move(1,90)
current=time.time()
run(0,0,1,0,0,1)