منو
در بخش اول آماده‌سازی مکانیکی ربات و نحوه سیم بندی و مونتاژ ربات دو چرخ ارائه شد که در این لینک قابل مشاهده است. در ادامه کار نحوه راهاندازی و بخش نرم‌افزار میکرو پایتون ربات بررسی و ارائه می شود.
فهرست
در این پروژه از ماژول رزبری پیکو استفاده شده است که عملاٌ ماژول پردازشگر و کنترل GPIO است. یکی از مهمترین زبانهای پیشنهادی برای رزبری پیکو، میکروپایتون است که توزیعی از پایتون3 می باشد. برای کار با میکروپایتون نرم افزارهای مختلفی قابل ارائه است اما Thonny IDE هم ساده و هم به‌طور رسمی معرفی شده است. لذا ما هم در این نوشته از این IDE استفاده می کنیم. براحتی می توانید آنرا از لینک رسمی دانلود نمائید. مجموعه برنامه های استفاده شده برای ربات در این مسیر قرار دارد و برای استفاده از آنها همگی در مسیر روت ماژول پیکو دانلود نمائید.
حال به بررسی ساختار کدهای برنامه ربات دو چرخ می پردازیم. ساختار کد این ربات شامل 3 لایه برنامه و درایور می باشد که در لایه اول یک برنامه اصلی قرار دارد که از همانطور که از اسمش مشخص است وظیفه کنترل و هدایت ربات را برعهده دارد و برای این کار از توابع و درایورهای لایه 2 و 3 بهره می گیرد. در لایه دوم 5 زیر برنامه با شکل تابع با نامهای Distance، MDrive, OLED، ULS و RGB_LED قرار دارند که در برنامه اصلی به فراخور نیاز فراخوانی می شوند. در لایه سوم برنامه های درایور قرار دارند و ماژولهای RGB LED, OLED, Ultrasonic Sensor را کنترل می کنند.
همانطور که اشاره شده برنامه اصلی main را در لایه اول قرار گرفته است و توابع لایه 2 و گاهی 3 را برای هدایت ربات فراخوانی می کند. در ادامه به بررسی خطوط مهم این برنامه می پردازیم.
# Main Function
######################################################################################################
# This Program is written by Giga Pardaz PARS
# PICO ROBOT with Mohandesika Two Wheel Robot CHasis
# CRR-PICO VER 1.5
# Universal Interface VER1.0
# MDRV DC Motor Drive VER1.1
# Esfand 1401
######################################################################################################
from machine import Pin, I2C, PWM, Timer , UART
import time
from utime import sleep
import ssd1306
from ssd1306 import SSD1306_I2C
import MDrive
import RGB_LED
import Distance
import OLED
######################################################################################################
uart = UART(0, baudrate=9600, tx=Pin(0), rx=Pin(1))
KEY = machine.Pin(15, machine.Pin.IN)
Interrupt=Pin(15,Pin.IN) # setting GPIO15 KEY_Interrupt as input
Buzzer = PWM(Pin(11))
Buzzer.freq(2000)
dist = 0
run = False
Obstacle = True
Move_State = 'Stop'
Control = 'Auto'
Obstacle_Counnter = 0
speed = 50
######################################################################################################
def handle_interrupt(Pin):
global run
if Control == 'Auto':
if run == False:
run = True
else:
run = False
return
Interrupt.irq(trigger=Pin.IRQ_RISING, handler=handle_interrupt)
######################################################################################################
def State_Info():
global speed, Move_State
if Control == 'Auto':
if Move_State != 'stop_motor':
OLED.Display(0,'PICO ROBO: Auto',str(Move_State) + ' SP:'+ str(speed) )
print('PICO ROBO: Auto',Move_State, 'Speed:',speed)
else:
OLED.Display(0,'PICO ROBO: Auto',str(Move_State) + ' SP:'+'0')
print('PICO ROBO: Auto',Move_State, 'Speed:',' 0')
if Control == 'Manual':
if Move_State != 'stop_motor':
OLED.Display(0,'PICO ROBO: Manual',str(Move_State) + ' SP:'+ str(speed))
print('PICO ROBO: Manual',Move_State, 'Speed:',speed)
else:
OLED.Display(0,'PICO ROBO: Auto',str(Move_State) + ' SP:'+'0')
print('PICO ROBO: Auto',Move_State, 'Speed:',' 0')
return
######################################################################################################
timer1=Timer(-1)
timer1.init(period=500, mode=Timer.PERIODIC,callback=lambda t: State_Info())
######################################################################################################
MDrive.setup()
sleep(.5)
RGB_LED.fill('Stop')
[dist, Obstacle]= Distance.dist_cm()
print('setup done.','Distance:', dist, 'cm')
while(True):
if uart.any() > 0:
data = uart.read()
print(data)
if "Auto" in data:
Control ="Auto"
print("Auto")
if "Manual" in data:
Control ="Manual"
print("Manual")
if Control == 'Manual':
try:
if (int(data)29):
speed = int(data)
except:
pass
if "forward" in data:
Move_State = MDrive.forward(speed)
if "backward" in data:
Move_State = MDrive.backward(speed)
if "left" in data:
Move_State = MDrive.turn_left()
if "right" in data:
Move_State = MDrive.turn_right()
if "sstop" in data:
Move_State = MDrive.stop_motor()
if Control == 'Auto':
if run == True:
[dist,Obstacle] = Distance.dist_cm()
if (Obstacle == False):
Move_State = MDrive.forward(speed)
else:
[Obstacle, Obstacle_Counnter] = MDrive.OBST(Obstacle, Obstacle_Counnter)
else:
Move_State = MDrive.stop_motor()
این برنامه با هدف تعیین فاصله ربات تا مانع نوشته شده است و با فراخوانی درایور سنسور التراسونیک HC SR04 فاصله را به سانتیمتر محاسبه و به لایه بالاتر در برنامه اصلی و MDrive برمی گرداند.
# Distance Function
######################################################################################################
# This Program is written by Giga Pardaz PARS
# PICO ROBOT with Mohandesika Two Wheel Robot CHasis
# CRR-PICO VER 1.5
# Universal Interface VER1.0
# MDRV DC Motor Drive VER1.1
# Esfand 1401
######################################################################################################
from machine import Pin
from hcsr04 import HCSR04
import time
from utime import sleep
sensor = HCSR04(trigger_pin=26, echo_pin=2, echo_timeout_us=10000)
LED = Pin(25,Pin.OUT)
######################################################################################################
def dist_cm():
try:
global dist, Obstacle
dist = sensor.distance_cm()
time.sleep_us(50)
if dist > 15 :
Obstacle = False
LED.value(1)
else:
Obstacle = True
LED.value(0)
except OSError as ex:
print('ERROR getting distance:', ex)
return dist, Obstacle
######################################################################################################
زیر تابع مهم ربات راه انداز موتورها هست که حرکتهای جلو، عقب، گردش به راست، گردش به چپ و توقف و البته تابع عبور از مانع در آن قرار دارد. در ادامه به بررسی دقیقتر خطوط مهم می پردازیم.
# MDrive Function
######################################################################################################
# This Program is written by Giga Pardaz PARS
# PICO ROBOT with Mohandesika Two Wheel Robot CHasis
# CRR-PICO VER 1.5
# Universal Interface VER1.0
# MDRV DC Motor Drive VER1.1
# Esfand 1401
######################################################################################################
from machine import Pin, I2C, PWM, Timer
import time
from utime import sleep
import ULS
import Distance
import OLED
import RGB_LED
i2c = I2C(0, sda=Pin(16), scl=Pin(17),freq = 300000)
Buzzer = PWM(Pin(11))
Buzzer.freq(2000)
######################################################################################################
PCA9538_I2C_ADDRESS = 0x73
PCA9538_CMD_Output = b'\x00'
Stop_All_Motor = b'\x00'
Forward_All_Motor = b'\x15'
Backwrad_All_Motor = b'\x1A'
Turn_Left = b'\x16'
Turn_Right = b'\x19'
pca9538_IN_REG = const(0x00) #Input register
pca9538_OUT_REG = const(0x01) #Output register
pca9538_POL_REG = const(0x02) #Polarity inversion register (1=data inverted)
pca9538_DIR_REG = const(0x03) #Config register (0=output, 1=input)
pwma = PWM(Pin(20))
pwmb = PWM(Pin(21))
pwma.freq(30000)
pwmb.freq(30000)
pwm_servo = PWM(Pin(5))
pwm_servo.freq(50)
ultrasonic_dir_forward = 5000
ultrasonic_dir_right = 1700
ultrasonic_dir_left = 8000
last_dir = ultrasonic_dir_forward
######################################################################################################
def forward(speed):
global Move_State
Move_State = 'Forward'
i2c.writeto_mem(PCA9538_I2C_ADDRESS, pca9538_OUT_REG , Forward_All_Motor)
duty_16 = int((speed*60000)/100)
pwmb.duty_u16(duty_16)
pwma.duty_u16(duty_16)
RGB_LED.fill('Forward')
return Move_State
######################################################################################################
def backward(speed):
global Move_State
Move_State = 'Backward'
i2c.writeto_mem(PCA9538_I2C_ADDRESS, pca9538_OUT_REG , Backwrad_All_Motor )
duty_16 = int((speed*65536)/100)
pwmb.duty_u16(duty_16)
pwma.duty_u16(duty_16)
RGB_LED.fill('Backward')
return Move_State
######################################################################################################
def turn_right():
speed = 30
global Move_State
Move_State = 'Turn_Right'
i2c.writeto_mem(PCA9538_I2C_ADDRESS, pca9538_OUT_REG , Turn_Right )
duty_16 = int((speed*65536)/100)
pwmb.duty_u16(duty_16)
RGB_LED.fill('Right')
sleep(.8)
return Move_State
######################################################################################################
def turn_left():
speed = 30
global Move_State
Move_State = 'Turn_Left'
i2c.writeto_mem(PCA9538_I2C_ADDRESS, pca9538_OUT_REG , Turn_Left )
duty_16 = int((speed*65536)/100)
pwmb.duty_u16(duty_16)
RGB_LED.fill('Left')
sleep(.8)
return Move_State
######################################################################################################
def turn_back():
speed = 30
global Move_State
Move_State = 'Turn_Back'
i2c.writeto_mem(PCA9538_I2C_ADDRESS, pca9538_OUT_REG , Turn_Left )
duty_16 = int((speed*65536)/100)
pwmb.duty_u16(duty_16)
RGB_LED.fill('turn_back')
sleep(1.6)
return Move_State
######################################################################################################
def stop_motor():
global Move_State
Move_State = 'Stop'
i2c.writeto_mem(PCA9538_I2C_ADDRESS, pca9538_OUT_REG , Stop_All_Motor )
RGB_LED.fill('Normal')
return Move_State
######################################################################################################
def setup():
i2c.writeto_mem(PCA9538_I2C_ADDRESS , pca9538_DIR_REG , PCA9538_CMD_Output)
i2c.writeto_mem(PCA9538_I2C_ADDRESS , pca9538_OUT_REG , Stop_All_Motor)
ULS.direct(ultrasonic_dir_forward)
RGB_LED.fill('Normal'); print('Setup Done')
Buzzer.duty_u16(60000)
sleep(.2)
Buzzer.duty_u16(0)
time.sleep_us(5)
return
######################################################################################################
def OBST(a,b):
global Obstacle,Obstacle_Counnter
Obstacle = a ; Obstacle_Counnter = b
if Obstacle_Counnter < 200:
Obstacle_Counnter = Obstacle_Counnter + 1
print('Obstacle_Counnter:',Obstacle_Counnter)
RGB_LED.fill('Stop')
OLED.Display(0,'Obstacle','Off')
sleep(.1)
stop_motor()
ULS.direct(ultrasonic_dir_right)
[dist, Obstacle] = Distance.dist_cm()
if (Obstacle == False):
Move_State = turn_right()
ULS.direct(ultrasonic_dir_forward)
else:
ULS.direct(ultrasonic_dir_left)
sleep(.1)
[dist, Obstacle] = Distance.dist_cm()
if(Obstacle== False):
Move_State = turn_left()
ULS.direct(ultrasonic_dir_forward)
else:
Move_State = turn_back()
ULS.direct(ultrasonic_dir_forward)
sleep(.2)
[dist, Obstacle] = Distance.dist_cm()
else:
Move_State = motor_off()
return Obstacle, Obstacle_Counnter
######################################################################################################
با توجه به این تابع سروو موتور SG-90 سنسور التراسونیک HC SR04 را بترتیب به سمت چپ، راست و حالت رو به جلو می چرخاند. ترتیب کار بسیار ساده است و متغیر مربوطه از تایع MDrive به این تابع ارسال می شود. در نهایت آخرین وضعیت زاویه چرخش سنسور التراسونیک به تابع وضعیت و MDrive ارسال می شود.
# ULS Function
######################################################################################################
# This Program is written by Giga Pardaz PARS
# PICO ROBOT with Mohandesika Two Wheel Robot CHasis
# CRR-PICO VER 1.5
# Universal Interface VER1.0
# MDRV DC Motor Drive VER1.1
# Esfand 1401
######################################################################################################
from machine import Pin,PWM
from utime import sleep
######################################################################################################
def direct (position):
pwm_servo = PWM(Pin(5))
pwm_servo.freq(50)
global last_dir
pwm_servo.duty_u16(position)
sleep(0.1)
last_dir = position
return last_dir
######################################################################################################
# OLED Display Function
######################################################################################################
# This Program is written by Giga Pardaz PARS
# PICO ROBOT with Mohandesika Two Wheel Robot CHasis
# CRR-PICO VER 1.5
# Universal Interface VER1.0
# MDRV DC Motor Drive VER1.1
# Esfand 1401
######################################################################################################
from machine import Pin, I2C, Timer
import ssd1306
from ssd1306 import SSD1306_I2C
i2c = I2C(0, sda=Pin(16), scl=Pin(17),freq = 300000)
display = ssd1306.SSD1306_I2C(128, 32, i2c)
######################################################################################################
# OLED 32x128 Display Setting
pix_res_x = 128 # SSD1306 horizontal resolution
pix_res_y = 32 # SSD1306 vertical resolution
oled = SSD1306_I2C(pix_res_x, pix_res_y, i2c) # oled controller
oled.fill(0) # clear the OLED
start_x = 0 # start point for text in x-dir
start_y = 2 # start point for text in y-dir
lineskip = 20 # space between text in y-dir
######################################################################################################
def Display(a,b,c):
global Control
if a == 0:
display.fill(0)
if b :
display.text(str(b),0,10,1)
if c:
display.text(str(c),0,24,1)
display.show()
return
این تابع با کمک درایور RGB LED های سری WS28X حالتهای مختلف Forward, Backward, Turn_Right, Turn_Left، Stop،Off, Normal و Error تعریف می شود و RGB LED ها متناظر با وضعیت حرکت ربات روشن و تغییر می کنند.
# RGB_LED Function
######################################################################################################
# This Program is written by Giga Pardaz PARS
# PICO ROBOT with Mohandesika Two Wheel Robot CHasis
# CRR-PICO VER 1.5
# Universal Interface VER1.0
# MDRV DC Motor Drive VER1.1
# Esfand 1401
######################################################################################################
from neopixel2 import Neopixel
from utime import sleep
numpix = 2
pixels = Neopixel(numpix, 0, 14, "GRB")
yellow = (255, 100, 0)
orange = (255, 50, 0)
green = (0, 255, 0)
blue = (0, 0, 255)
red = (255, 0, 0)
black = (0,0,0)
white = (255,255,255)
COLORS = (white,red, yellow, green, blue, white,black)
color0 = red
pixels.brightness(25)
######################################################################################################
def fill(color):
pixels.brightness(25)
if color == 'Forward':
pixels.set_pixel(0, green)
pixels.set_pixel(1, green)
pixels.show()
if color == 'Right':
pixels.set_pixel(0, green)
pixels.set_pixel(1, black)
pixels.show()
if color == 'Left':
pixels.set_pixel(0, black)
pixels.set_pixel(1, green)
pixels.show()
if color == 'turn_back':
pixels.set_pixel(0, blue)
pixels.set_pixel(1, blue)
pixels.show()
if color == 'Backward':
pixels.set_pixel(0, yellow)
pixels.set_pixel(1, yellow)
pixels.show()
if color == 'Stop':
pixels.set_pixel(0, red)
pixels.set_pixel(1, red)
pixels.show()
if color == 'Off':
pixels.set_pixel(0, black)
pixels.set_pixel(1, black)
pixels.show()
if color == 'Error':
pixels.brightness(100)
pixels.set_pixel(0, red)
pixels.set_pixel(1, red)
pixels.show()
if color == 'Normal':
pixels.set_pixel(0, white)
pixels.set_pixel(1, white)
pixels.show()
return
برنامه اندرویدی برای حالت کنترل دستی ربات نوشته شده است و می توان با استفاده از موبایل و ارتباط بلوتوث ربات را کنترل کرد. روش طراحی این نرم‌افزار به‌صورت جداگانه در یک بلاگ دیگر معرفی خواهد شد. از این لینک می تواند اپلیکیشن اندرویدی را دانلود نمائید.
در نظر داشته باشید که در حالت اتوماتیک ربات نیازی به این اپلیکشن ندارد و خودکار مانع را تشخیص و از آن عبور می کند.
برای شروع کار ابتدا تمام برنامه ها در مسیر روت ماژول پیکو دانلود نمائید. بعد از ریست ماژول برنامه main بطور پیش فرض طبق توصیف بالا اجرا می شود و با استفاده از نرم‌افزار اندرویدی و کلید KEY روی کریر بورد پرومیک ربات قابل کنترل در دو وضعیت دستی و اتوماتیک است.
در دو ویدئوی زیر حالتهای دستی ربات و اتوماتیک ربات تست شده اند. برای تست حالت اتوماتیک بهتر است که از دیوارهای به اندازه ارتفاع سنسور التراسونیک استفاده کنید. پیشنهاد می شود که ارتفاع دیوارها بیشتر از 15 سانتیمتر باشد.
با تکمیل ربات و بهبودهای اشاره شده بنظر داشتن رباتهای با ابعاد مناسب و گاها کوچکتر و حذف دردسرهای سیم بندی و سوراخ کاری های اضافه می توان تمرکز را بر کدنویسی و بهینه‌سازی الگوریتم های حرکتی و مسیریابی گذاشت و لذا نتایج بهتری بدست آورد و امکان آزمون و مسابقه را با یک ربات حرفه ای داشت.
یک راه حرفه ای و مقرون به صرفه استفاده از رباتهای آماده سخت افزاری هست که نیاز به صرفا مونتاژ ماژولها دارد و اصل موضوع برنامه نویسی و بهینه‌سازی الگوریتمهای مرتبط است. شرکت گیگا پرداز پارس با برند ProMake این محصولات را به‌صورت حرفه ای داخلی سازی کرده و در اختیار علاقه مندان قرار داده است. در نوشته های آتی به معرفی این محصولات جذاب و حرفه ای می پردازیم که با استفاده از دوربین و SBC های مشابه رزبری پای 3 یا 4 و یا برد Jetson Nano انواع سناریوهای پردازش تصویر و AI را در ربات هوشمند قابل استفاده می کنند.