Car following a changing AprilTag - the camera and pico are connected via bluetooth. Camera tracks AprilTag movement and is translated to motor motion, at the end of the video the car stops when it reaches a threshold distance from the tag.
#Initialize Variables and Classes
k_p = 0.1
k_d = 0.05
error = 0
prev = 0
f_x = (2.8 / 3.984) * 160
f_y = (2.8 / 2.952) * 120
c_x = 160 * 0.5
c_y = 120 * 0.5
while True:
clock.tick()
img = sensor.snapshot()
currentTag = ''
while True:
clock.tick()
img = sensor.snapshot()
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y):
img.draw_rectangle(tag.rect, color=(255, 0, 0))
img.draw_cross(tag.cx, tag.cy, color=(0, 255, 0))
prev = error
error = tag.cx - (img.width() // 2)
direction = int(k_p * error + k_d * (error - prev))
distance = abs(int(tag.z_translation))
motor_ble.notify(direction, distance)
This is the main PID logic that the camera conducts. The program begins by initializing constants like k_p (proportional gain) and k_d (derivative gain), which are used in a proportional-derivative (PD) controller to calculate the motor's steering direction. It also defines the focal lengths f_x and f_y and the principal points c_x and c_y for the camera. In the main loop, an image is captured, and AprilTags are detected. For each tag, the code calculates the error, which is the difference between the tag's x-coordinate (tag.cx) and the center of the image (img.width() // 2). The direction of the motor is computed using a PD equation: direction = int(k_p * error + k_d * (error - prev)), where error is the current error and prev is the previous error. This equation adjusts the motor direction based on both the current deviation from the center and the rate of change of that deviation. The distance to the tag is calculated as the absolute value of the z-axis translation (int(tag.z_translation)), representing the tag's distance from the camera. The calculated direction and distance are then sent to a motor controller via Bluetooth (motor_ble.notify).
def motor_speed(direction,distance):
if direction == 0:
print("Forward")
if distance > 5:
left_motor.duty_u16(45000 + (distance * 12))
right_motor.duty_u16(45000 + (distance * 12))
elif distance <= 2:
left_motor.duty_u16(0)
right_motor.duty_u16(0)
else:
left_motor.duty_u16(45000 + distance)
right_motor.duty_u16(45000 + distance)
elif direction > 0:
print("Right")
direction = abs(direction)
if distance > 5:
left_motor.duty_u16((direction * 12) + 45000 + (distance * 12))
right_motor.duty_u16((45000 // direction) + (distance * 12))
elif distance <= 2:
left_motor.duty_u16(0)
right_motor.duty_u16(0)
else:
left_motor.duty_u16((direction * 12) + 45000 + distance)
right_motor.duty_u16((45000 // direction) + distance)
elif direction < 0:
print("Left")
direction = abs(direction)
if distance > 5:
left_motor.duty_u16((45000 // direction) + (distance * 12))
right_motor.duty_u16((direction * 12) + 45000 + (distance * 12))
elif distance <= 2:
left_motor.duty_u16(0)
right_motor.duty_u16(0)
else:
left_motor.duty_u16((45000 // direction) + distance)
right_motor.duty_u16((direction * 12) + 45000 + distance)
if __name__ == "__main__":
central = BLECentral()
central.start_scan()
while True:
if central._last_notification:
direction, distance = central._last_notification.split()
print(direction)
print(distance)
motor_speed(int(direction),int(distance))
direction = 0
distance = 0
This code controls motor speeds based on direction and distance data received via Bluetooth. The motor_speed() function adjusts the motor duty cycles for forward movement, right turns, or left turns, depending on the direction value. For forward movement (when direction == 0), the motors speed up or stop based on the distance, while for turns, the motor on the opposite side of the turn receives more power to facilitate the movement. The BLECentral object scans for notifications containing direction and distance, which are then passed to the motor_speed() function to dynamically control the motors. The system resets the values after each command to ensure smooth operation.
Find the full code below under ../Making Car Smart