def drive(speed = 0.0, angle = 0.0):
if speed == 0.0 and angle == 0.0:
free()
return
if angle == 0 or angle ==360:
d1 = speed
d2 = 0.0
elif angle <= 90:
d1 = speed
d2 = (angle/90.0)
elif angle <= 180:
d1 = -speed
d2 = 1.0-(angle-90)/90.0
elif angle < 270:
d1 = -speed
d2 = -(1.0-(angle-180)/90.0)
elif angle < 360:
d1 = speed
d2 = -(1.0-(angle-270)/90.0)
d1 = -d1
d2 = -d2
motor1.set(d1)
motor2.set(d2)
def trn_left(duty):
motor2.set(duty)
motor3.set(-duty+0.1)
def trn_right(duty):
motor2.set(duty+0.1)
motor3.set(-duty)
def right(duty):
motor2.set(duty)
motor3.set(duty)
def left(duty):
motor2.set(-duty)
motor3.set(-duty)
def reverse(duty):
motor2.set(-duty)
motor3.set(duty)
def stop():
motor2.set(0)
motor3.set(0)
def forward(duty):
motor2.set(duty)
motor3.set(-duty)
def drive(leftSpeed,rightSpeed):
motor1.set(leftSpeed)
motor2.set(rightSpeed)
Display_Block (Index, Blocks[Index], Pan_PID_Controller.Command)
Pan_Offset = (pixy.get_frame_width () / 2) - Blocks[Index].m_x;
Tilt_Offset = Blocks[Index].m_y - (pixy.get_frame_height () / 2)
Pan_PID_Controller.Update (Pan_Offset)
Tilt_PID_Controller.Update (Tilt_Offset)
pixy.set_servos(int(round(Pan_PID_Controller.Command)), int(round(Tilt_PID_Controller.Command)))
# Now we add the motors
leftSpeed = speed + drivePID
rightSpeed = speed - drivePID
motor1.set(leftSpeed)
motor2.set(rightSpeed)
""" if Pan_PID_Controller.Command >= 511:
drive(0, .5)
elif 480 <= Pan_PID_Controller.Command <= 510:
drive(.5, 0)
else:
drive(.5, .5)"""
else:
print(('Frame %3d:' % (Frame)))
# Display all the blocks in the frame #
for Index in range (0, Count):
Display_Block (Index, Blocks[Index],0)
# Find an acceptable block to lock on to #
if Blocks[0].m_age > MINIMUM_BLOCK_AGE_TO_LOCK: