هوشمند سازی ربات شاسی دو چرخ- بخش دو

				
					# 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

######################################################################################################