Created
October 6, 2022 05:48
-
-
Save idriszmy/2be9acfa0c9f57f43cad7e59d4996e84 to your computer and use it in GitHub Desktop.
ZOOM:BIT line following robot and detect junctions.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import time | |
from microbit import * | |
import music | |
import rekabit | |
rekabit.init() | |
def get_voltage(): | |
return (pin1.read_analog() * 3.3) / 1023 | |
def motor_speed(left, right): | |
rekabit.run_motor(rekabit.M1, rekabit.Forward, int(left)) | |
rekabit.run_motor(rekabit.M2, rekabit.Forward, int(right)) | |
last_proportional = 0 | |
kp = 0.9 | |
kd = 5 | |
def line_follow_pd(data, setpoint): | |
global last_proportional | |
global kp | |
global kd | |
speed_left = 1 | |
speed_right = 1 | |
proportional = data - setpoint | |
derivative = proportional - last_proportional | |
last_proportional = proportional | |
power_different = (proportional * kp) + (derivative * kd) | |
if power_different < -1: | |
power_different = -1 | |
if power_different > 1: | |
power_different = 1 | |
if power_different < 0: | |
speed_left = speed_left + power_different | |
else: | |
speed_right = speed_right - power_different | |
motor_speed(speed_left*150, speed_right*150) | |
def servo_move(init_pos, target_pos): | |
change = 0 | |
if init_pos < target_pos: | |
change = 1 | |
else: | |
change = -1 | |
for pos in range(init_pos, target_pos, change): | |
rekabit.sets_servo_position(rekabit.S1, pos) | |
time.sleep(0.005) | |
rekabit.sets_servo_position(rekabit.S1, 76) | |
motor_speed(0, 0) | |
print("Robot ready!") | |
display.show(Image.HAPPY) | |
follow_line = False | |
left_count = 0 | |
right_count = 0 | |
cross_count = 0 | |
while True: | |
if button_a.was_pressed(): | |
follow_line = not follow_line | |
if follow_line: | |
print("Move!") | |
display.show(Image.ARROW_S) | |
music.play(music.POWER_UP) | |
else: | |
print("Stop!") | |
motor_speed(0, 0) | |
maker_line = get_voltage() | |
ir_left = pin0.read_analog() | |
ir_right = pin2.read_analog() | |
#print("Maker Line: {:.2f}".format(maker_line)) | |
#print("IR Left: {:.2f}".format(ir_left)) | |
#print("IR Right: {:.2f}".format(ir_right)) | |
#sleep(1000) | |
if follow_line == True: | |
if ir_left > 700 and ir_right < 700: # Left junction | |
right_count = 0 | |
cross_count = 0 | |
if left_count < 100: | |
left_count += 1 | |
if left_count > 50: | |
motor_speed(0, 0) | |
display.show(Image.ARROW_E) | |
servo_move(76, 170) | |
music.play(music.BA_DING) | |
time.sleep(1) | |
motor_speed(100, 100) | |
servo_move(170, 76) | |
display.show(Image.ARROW_S) | |
elif ir_left < 700 and ir_right > 700: # Right junction | |
left_count = 0 | |
cross_count = 0 | |
if right_count < 100: | |
right_count += 1 | |
if right_count > 50: | |
motor_speed(0, 0) | |
display.show(Image.ARROW_W) | |
servo_move(76, 0) | |
music.play(music.BA_DING) | |
time.sleep(1) | |
motor_speed(100, 100) | |
servo_move(0, 76) | |
display.show(Image.ARROW_S) | |
elif ir_left > 700 and ir_right > 700: # Cross junction | |
left_count = 0 | |
right_count = 0 | |
if cross_count < 100: | |
cross_count += 1 | |
if cross_count > 50: | |
motor_speed(0, 0) | |
display.show(Image('00900:' | |
'00900:' | |
'99999:' | |
'00900:' | |
'00900')) | |
music.play(music.BA_DING) | |
time.sleep(1) | |
motor_speed(100, 100) | |
time.sleep(0.5) | |
display.show(Image.ARROW_S) | |
elif maker_line > 3: | |
motor_speed(100, 100) | |
else: | |
line_follow_pd(maker_line, 1.6) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment