麦卡姆轮运动原理

import cv2
import numpy as np
import time
 
 
import time
import RPi.GPIO as GPIO
 
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
 
# 由于硬件引脚链接电机驱动正反，位置不一样，实际需要逐个测试，输入对应正确引脚
pinLQ1=26     # 左前电机引脚1
pinLQ2=19     # 左前电机引脚2
pinRQ1=20     # 右前电机引脚1
pinRQ2=21     # 右前电机引脚2

pinLH1=6     # 左后电机引脚1
pinLH2=5     # 左后电机引脚2
pinRH1=12    # 右后电机引脚1
pinRH2=16    # 右后电机引脚2


 
GPIO.setup(pinLQ1, GPIO.OUT) #设置脚为输出模式
GPIO.setup(pinLQ2, GPIO.OUT) #设置脚为输出模式
GPIO.setup(pinRQ1, GPIO.OUT) #设置脚为输出模式
GPIO.setup(pinRQ2, GPIO.OUT) #设置脚为输出模式

GPIO.setup(pinLH1, GPIO.OUT) #设置脚为输出模式
GPIO.setup(pinLH2, GPIO.OUT) #设置脚为输出模式
GPIO.setup(pinRH1, GPIO.OUT) #设置脚为输出模式
GPIO.setup(pinRH2, GPIO.OUT) #设置脚为输出模式


 


#停止
def stop(time1):
    print ("stop")
    GPIO.output(pinLQ1, GPIO.LOW)  #  A停止
    GPIO.output(pinLQ2, GPIO.LOW)  #灭
    GPIO.output(pinRQ1, GPIO.LOW)  # B停止
    GPIO.output(pinRQ2, GPIO.LOW)  #灭
    GPIO.output(pinLH1, GPIO.LOW)  #  A停止
    GPIO.output(pinLH2, GPIO.LOW)  #灭
    GPIO.output(pinRH1, GPIO.LOW)  # B停止
    GPIO.output(pinRH2, GPIO.LOW)  #灭
    time.sleep(time1)   #延时 秒
    
    

#左测前轮前进
def LQGO(move):
    if move==1:#前进
        GPIO.output(pinLQ1, 1)  #  A往前转
        GPIO.output(pinLQ2, 0)  #灭
    else:#后退
        GPIO.output(pinLQ1, 0)  #  A往前转
        GPIO.output(pinLQ2, 1)  #灭
    #time.sleep( time)   #延时 秒
    
#左侧后轮前进   
def LHGO(move):
    if move==1:#前进
        GPIO.output(pinLH1, 1)  #  A往前转
        GPIO.output(pinLH2, 0)  #灭
    else:
        GPIO.output(pinLH1, 0)  #  A往前转
        GPIO.output(pinLH2, 1)  #灭
    #time.sleep( time)   #延时 秒    
    
#右侧前轮前进   
def RQGO(move):
    if move==1:#前进
        GPIO.output(pinRQ1, 1)  #  A往前转
        GPIO.output(pinRQ2, 0)  #灭
    else:
        GPIO.output(pinRQ1, 0)  #  A往前转
        GPIO.output(pinRQ2, 1)  #灭        
        
#右侧后轮前进   
def RHGO(move):
    if move==1:#前进
        GPIO.output(pinRH1, 1)  #  A往前转
        GPIO.output(pinRH2, 0)  #灭
    else:
        GPIO.output(pinRH1, 0)  #  A往前转
        GPIO.output(pinRH2, 1)  #灭
        


#原地旋转左

def turn_right(Gtime):
    print ("turn_right")    
    LQGO(1)
    LHGO(0)
    RQGO(1)
    RHGO(0)
    time.sleep(Gtime)   #延时 秒
    
#水平向右    
def right_go(Gtime):
    print ("turn_right")
    LQGO(1)
    LHGO(0)
    RQGO(0)
    RHGO(1)
    
    time.sleep(Gtime)   #延时 秒
    
    
#原地左转
def tuen_left(Gtime):
    print ("tuen_left")
    LQGO(0)
    LHGO(1)
    RQGO(0)
    RHGO(1)

    time.sleep(Gtime)   #延时 秒
     
    
#水平左移动
def left_go(Gtime):
    print ("tuen_left")
    
    LQGO(0)
    LHGO(1)
    RQGO(1)
    RHGO(0)
    time.sleep(Gtime)   #延时 秒     

#前进      
def go(Gtime):
    print ("go_qian")
    LQGO(1)
    LHGO(1)
    RQGO(1)
    RHGO(1)
    time.sleep(Gtime)   #延时 秒
 
#后退      
def back(Gtime):
    print ("turn_back")
    LQGO(0)
    LHGO(0)
    RQGO(0)
    RHGO(0)
    time.sleep(Gtime)   #延时 秒
    


#运动测试
def move_test():
    stop(1) 
    #go(0.5)
    #back(0.5)
    #turn_right(0.5)
    #tuen_left(0.5)
    #left_go(0.5)
    #right_go(0.5)
    #time.sleep( 3)   #延时 秒
    stop(1)
   
   
move_test()