基于Kinect体感遥控多功能小车_基于kinect的智能体感小车-优快云博客

本文链接：https://blog.youkuaiyun.com/rong563/article/details/77772513

本文介绍了一种基于Kinect传感器的体感控制智能小车系统，通过人体动作识别控制小车移动，并利用超声波传感器实现避障功能。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

本智能小车体感控制系统如图1，主要包括操控部分和智能小车部分。操控部分包括Kinect传感器和计算机，实现对智能小车的控制。Kinect获取操作者的体势动作，通过计算机端的软件识别人体动作并生成对应的智能小车控制指令，通过蓝牙数传模块，发给智能小车的控制模块处理并做出相应的动作。

图 1 系统结构示意图

图 2 小车控制框图

2.2体势识别

当人体做出特定动作，全身的骨骼节点位置坐标满足一定条件，即被识别为一种控制指令。对人体的体势识别主要是对骨骼节点之间的角度与欧氏距离进行判别，当达到一定的角度与距离事就被系统识别并处理。

具体体势设计如表1，智能小车启动后，除了这六种控制动作，智能小车都保持停止状态。体势动作设计思想：

表格1 体势动作设计思想

智能小车指令	手势动作设计
前进	向上挥手
后退	向下挥手
左移	向左挥手
右移	向右挥手
停止	顺/逆时针挥手

2.3超声波避障

超声波传感器测距是通过先发射超声波然后检测传回的超声波所耗费的时间从而计算距离。在小车前方和两侧设置超声波模块，避障危险距离为20厘米。当小车在某一方向测得的距离小于20厘米时，小车将自动停车且不能继续往这一方向前进，但在其他方向可继续控制前进。图 3 是其流程图。

图 3 超声波测距图

2.4智能小车控制

本系统以Arduino单片机为核心控制器，主要由电源模块、电机驱动模块等构成。采用前桥转向，后轮驱动的布置方式，两轮各用一个直流电机配合齿轮减速机构实现。

3系统硬件设计

3.1 Kinect体感传感器

Kinect是微软在2009年6月2日的E3大展上正式公布的XBOX360体感周边外设，当时的代号为“Project Natal”，意味着初生，也是遵循了微软一向以城市为开发代号的传统，因为Natal是巴西城市的名称。Natal在英语中的意思是初生，也是微软希望Kinect能对XOBOX360带来新生的期望，但从目前的Kinect的发展来看，已经远远超出了它的设计初衷。2010年6月14日“Project Natal”被正式命名为Kinect。

Kinect的推出颠覆了单一的游戏操作，即玩家不再需要键盘，鼠标，手柄和遥感等控制机器，Kinect的理念是人本身就是最理想的传感器，它是一种体感的3D投影机，捕捉玩家的身体动作来控制游戏。

Microsoft Kinect是一种复合传感器。Kinect包含3个镜头，中间的是RGB彩色图像传感器，图像分辨率为640*480，最大帧率为30Fps；左侧为散斑发射器，发射近红外波段的散斑图案;右侧为三维传感器，图像分辨率为640*480，最大帧率为30Fps，水平视场角为58°，垂直视场角为45°，散斑发射器和三维传感器构成三维深度传感器系统[27]。相比于普通的彩色摄像机，Kinect在彩色视频流(Color Image Stream)之外，还可以提供用于表现视野中物体到Kinect距离的深度数据流(Depth Image Stream)，以及由麦克风阵列确定的包含声源位置信息的音频流。除此之外，Kinect通过对深度数据流进行处理，得到了骨骼数据流(Skeleton Stream)，将Kinect视野中的人体用主要的20个骨骼节点进行描述。每个节点数据包括该节点在摄像机视野之中的位置，包含长度、宽度与深度3个分量[28]。

Kinect传感器主要由以下几个部分组成：

麦克风阵列：声音从4个麦克风采集，采用有效的噪声消除和回声抑制（Acoustic Echo Cancellation，AEC）算法，能消除20db的环境噪音，同时采用波束成型（Beam Forming）技术，可以通过每个独立设备的响应时间确认音源位置。

红外投影机：主动投射近红外光谱，照射到粗糙物体、或者穿射毛玻璃后，光谱发生扭曲，会形成随机的反射斑点（成为散斑），进而能被红外摄像头读取。

红外摄像头：分析红外光谱，创建可视范围内的人体、物体的深度图像。彩色摄像头：用于拍摄视角范围内的彩色视频图像。

仰角控制马达：可编程控制仰角的马达，用于获取最佳视角。 Kinect传感器如图2所示，主要规格参数如表2所示。

图 4 Kinect传感器

表格2 Kinect传感器规格参数

传感器特性	规格参数
可视角度	43°垂直方向和57°水平方向有效视野范围
机械化倾斜范围（垂直方向）	±28°
帧率（深度和彩色图像数据流）	30帧每秒（Fps）
深度图像（默认）	QVGA（320*240）
彩色图像（默认）	VGA（640*480）
音频格式	16kHz，16bit单声道（PCM）
音频输入特性	4个带有24bit的ADC和消除处理噪声的麦克风阵列

通过固件升级，Kinect for Windows支持“近景模式”（Near Mode）。在该模式下，Kinect可视范围为0.4~3米，与默认模式的差别如图3所示。

图 5 Kinect的默认模式和近景模式

3.2超声波测距模块

对于超声波测距功能，本次设计中使用HC-SR04超声波传感器，主要用于测量前方及两侧物体距离小车的距离以及通过测量距离避免小车因向前行驶速度过快撞击前方障碍物毁坏车体。

HC-SR04超声波传感器是最常见的超声波传感器之一，其价格便宜、易用。

可提供 2cm-400cm 的非接触式距离感测功能，测距精度可达高到 3mm；模块包括超声波发射器、接收器与控制电路。

基本工作原理如图6：

1．采用 IO 口 TRIG 触发测距，给至少 10us 的高电平信号；

2．模块自动发送 8 个 40khz 的方波，自动检测是否有信号返回；

3．有信号返回，通过 IO 口 ECHO 输出一个高电平，高电平持续的时间就是超声；

4．两次测距时间间隔最少在60ms以上，以防止发射信号对回响信号的影响；

波从发射到返回的时间。测试距离=(高电平时间*声速(340M/S))/2；

图 6 超声波时序图

HC-SR04电气参数如表3所示：

表格3 HC-SRO4 电气参数

电气参数	HC-SR04超声波模块
工作电压	DC5V
工作电流	15mA
工作频率	40HZ
最远射程	400cm
最近射程	2cm
测量角度	15度
输入出发信号	10uS 的 TTL 脉冲
输出回应信号	输出 TTL 电平信号，与射程成比例

3.3基于Arduino的智能小车

Arduino是一个能够用来感应和控制现实物理世界的一套工具。它由一个基于单片机并且开放源码的硬件平台，和一套为Arduino板编写程序的开发环境组成。准确的说Arduino是一块简单、方便的以AVR单片机为核心的GPIO接口板，并可以通过USB接口和电脑通信。它基于开放的源代码编程，具有使用类似Java， C语言的IDE集成开发环境，提供丰富的库函数。

Arduino Uno是Arduino开源平台中最典型的参考标准，其处理器为ATmega328p，工作电压为5v，输入电压（推荐7-12v）范围6-20v，数字IO脚14路（其中6路作为PWM输出），模拟输入脚6路，IO脚直流电流40mA，3.3V脚直流电流50mA，FlashMemory 32Kb（其中0.5Kb用于bootloader），静态随机存储器（SRAM）为2Kb，电可擦除只读存储器（EEPROM）为1Kb，工作时钟为16MHz，产品样式如图7，电路图如图8

图 7 Arduino Uno R3

图 8 Arduino Uno R3电路图

4系统软件设计

软件设计采用模块化处理方法，体感操控部分程序主要由主程序、Kinect初始化、图像辨识、串口通信等模块组成，采用processing编写，编译环境为processing，使用部分环境库函数。智能小车部分程序主要有主程序、串口通信、电机驱动、舵机驱动、动作函数等模块组成。采用C++语言编写，编译环境为Avrtudio4，使用部分环境库函数。系统程序流程如图9所示。

图 9 系统程序流程

4.1 Kinect体感操控部分程序

import SimpleOpenNI.*;

import processing.opengl.*;

import processing.serial.*;

import java.util.Iterator;

SimpleOpenNI kinect;

Serial myPort;

// NITE

XnVSessionManager sessionManager;

XnVPointControl pointControl;

XnVCircleDetector CircleDetector;

// Font for text on screen

PFont font;

// Variable to define different modes

int mode = 0;

// Variables for Hand Detection

boolean handsTrackFlag = true;

PVector screenHandVec = new PVector();

PVector handVec = new PVector();

ArrayList<PVector> handVecList = newArrayList<PVector>();

int handVecListSize = 30;

PVector centerVec = new PVector();

PVector screenCenterVec = new PVector();

int colorlamp;

int Time;

float rad;

float rot;

float prevRot;

float angle;

void setup()

{

//Simple-openni object

kinect = new SimpleOpenNI(this);

kinect.setMirror(true);

//enable depthMap generation, hands + gestures

kinect.enableDepth();

kinect.enableGesture();

kinect.enableHands();

kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);

//setup NITE

sessionManager = kinect.createSessionManager("Wave","RaiseHand");

//Setup NITE.s Hand Point Control

pointControl = new XnVPointControl();

pointControl.RegisterPointCreate(this);

pointControl.RegisterPointDestroy(this);

pointControl.RegisterPointUpdate(this);

CircleDetector = new XnVCircleDetector();

CircleDetector.RegisterCircle(this);

CircleDetector.RegisterNoCircle(this);

//Add the Detector to the session

sessionManager.AddListener(pointControl);

sessionManager.AddListener(CircleDetector);

//Set the sketch size to match the depth map

//size(kinect.depthWidth(), kinect.depthHeight());

if(kinect.enableRGB() == false)

{

println("Can't open the rgbMap, maybe the camera is not connectedor there is no rgbSensor!");

exit();

return;

}

size(kinect.depthWidth() + kinect.rgbWidth() + 10, 480);

smooth();

//Initialize Font

font = loadFont("SansSerif-48.vlw");

//This gets the port on your computer

myPort = new Serial(this, "COM7", 9600);

}

void draw()

{

background(0);

//Update Kinect data

kinect.update();

//update NITE

kinect.update(sessionManager);

//draw depthImageMap

//image(kinect.depthImage(), 0, 0);

image(kinect.rgbImage(),kinect.depthWidth() + 10,0);

if(kinect.isTrackingSkeleton(1)) {

drawSkeleton(1);

//Switch between modes

switch(mode) {

case 0: // Waiting Mode

checkSpeed(); // Check the speed of the hand

if (handsTrackFlag) drawHand(); // Draw the hand if it's beeninitialized

break;

case 1:

volumeControl();

break;

case 2:

lampcolor(colorlamp);// draw the change channel simbol

// Add one to the timer

Time++;

// If the timer gets to 10, reset the counter and go back to waitingmode (0)

if (Time>10) {

Time = 0;

mode = 0;

}

break;

}

void drawSkeleton(int userId) {

pushStyle();

stroke(255,0,0);

strokeWeight(3);

kinect.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);kinect.drawLimb(userId, SimpleOpenNI.SKEL_NECK,SimpleOpenNI.SKEL_LEFT_SHOULDER); kinect.drawLimb(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW,SimpleOpenNI.SKEL_LEFT_HAND);

kinect.drawLimb(userId, SimpleOpenNI.SKEL_NECK,SimpleOpenNI.SKEL_RIGHT_SHOULDER); kinect.drawLimb(userId,SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW,SimpleOpenNI.SKEL_RIGHT_HAND); kinect.drawLimb(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER,SimpleOpenNI.SKEL_TORSO); kinect.drawLimb(userId, SimpleOpenNI.SKEL_TORSO,SimpleOpenNI.SKEL_LEFT_HIP); kinect.drawLimb(userId,SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);kinect.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);kinect.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP,SimpleOpenNI.SKEL_RIGHT_KNEE); kinect.drawLimb(userId,SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT); popStyle();

}

void onNewUser(int userId) {

println("onNewUser - userId: " + userId);

if(kinect.isTrackingSkeleton(1)) return;

println(" start pose detection");

kinect.startPoseDetection("Psi", userId);

}

void onLostUser(int userId) {

println("onLostUser - userId: " + userId);

}

void onStartPose(String pose, int userId) {

println("onStartPose - userId: " + userId + ", pose:" + pose);

println(" stop pose detection");

kinect.stopPoseDetection(userId);

kinect.requestCalibrationSkeleton(userId, true);

}

void onEndPose(String pose, int userId) {

println("onEndPose - userId: " + userId + ", pose: "+ pose);

}

void onStartCalibration(int userId) {

println("onStartCalibration - userId: " + userId); }

public void onEndCalibration(int userId, boolean successfull) {println("onEndCalibration - userId: " + userId + ", successfull:" + successfull); if (successfull) {

println(" User calibrated !!!");

kinect.startTrackingSkeleton(userId); }

else {

println(" Failed to calibrate user !!!"); println(" Startpose detection"); kinect.startPoseDetection("Psi", userId);

}

// XnVPointControl callbacks

void onPointCreate(XnVHandPointContextpContext)

{

println("onPointCreate:");

handsTrackFlag = true;

handVec.set(pContext.getPtPosition().getX(),pContext.getPtPosition().getY(), pContext.getPtPosition().getZ());

handVecList.clear();

handVecList.add(handVec.get());

}

void onPointDestroy(int nID)

{

println("PointDestroy: " + nID);

handsTrackFlag = false;

}

void onPointUpdate(XnVHandPointContextpContext)

{

handVec.set(pContext.getPtPosition().getX(),pContext.getPtPosition().getY(), pContext.getPtPosition().getZ());

handVecList.add(0, handVec.get());

if(handVecList.size() >= handVecListSize)

{// remove the last point

handVecList.remove(handVecList.size()-1);

}

void onCircle(float fTimes, booleanbConfident, XnVCircle circle) {

println("onCircle: " + fTimes +" , bConfident=" + bConfident);

rot = fTimes;

angle = (fTimes % 1.0f) * 2 * PI - PI/2;

centerVec.set(circle.getPtCenter().getX(),circle.getPtCenter().getY(), handVec.z);kinect.convertRealWorldToProjective(centerVec, screenCenterVec);

rad = circle.getFRadius();

mode = 1; }

void onNoCircle(float fTimes, int reason) {

println("onNoCircle: " + fTimes +" , reason= " + reason);

mode = 0;

}

// Check if the hand movement matches whatwe want

void checkSpeed() {

//Checkl only if we have two positions, so we can calculate the speed

if(handVecList.size()>1) {

// Check the distance between the two last hand positions

PVector vel = PVector.sub(handVecList.get(0), handVecList.get(1));

// If the distance is greater than 50 on the x-axis

if (vel.x>100) {

mode = 2;

colorlamp = 1;

}

// If the distance is lesser than -50 on the x-axis

if (vel.x<-80) {

colorlamp = -1;

mode = 2;

}

if (vel.y>60) {

colorlamp = 0;

mode = 2;

}

if(vel.y<-100) {

colorlamp = 2;

mode = 2;

}

//send the change channel signal to Arduino

void lampcolor(int sign) {

String lampcolor;

pushStyle();

//If we are changing to the next channel

if(sign==1) {

stroke(255, 0, 0);

fill(255, 0, 0);

// Send the signal only if it's the first loop

if (Time == 0)myPort.write('r');

textAlign(LEFT);

//lampcolor = "RED";

println(sign);

strokeWeight(10);

pushMatrix();

translate(width/2,height/2);

line(-320,0,sign*200-320,0);

triangle(-320+sign*200,20,-320+sign*200,-20,-320+sign*250,0);

textFont(font,20);

//text(lampcolor,0,40);

popMatrix();

popStyle();

}

//Else, we are changing to the previous channel

else if (sign==0) {

stroke(0, 0, 255);

fill(0, 0, 255);

println(sign);

// Send the signal only if it's the first loop

if (Time == 0)myPort.write('f');

textAlign(CENTER, BOTTOM);

//lampcolor = "BLUE";

println(sign);

strokeWeight(10);

pushMatrix();

translate(width/2,height/2);

line(-320,0,-320,-200);

triangle(-340,-200,-300,-200,-320,-240);

fill(0,0,255);

textFont(font,20);

//text(lampcolor,0,40);

popMatrix();

popStyle();

}

else if (sign==2) {

stroke(0, 255, 255);

fill(0, 255, 255);

println(sign);

// Send the signal only if it's the first loop

if (Time == 0)myPort.write('b');

textAlign(CENTER, BOTTOM);

//lampcolor = "XBLUE";

println(sign);

strokeWeight(10);

pushMatrix();

translate(width/2,height/2);

line(-320,0,-320,200);

triangle(-340,200,-300,200,-320,240);

//fill(0,0,255);

textFont(font,20);

//text(lampcolor,0,40);

popMatrix();

popStyle();

}

else if(sign==-1) {

stroke(0, 255, 0);

fill(0, 255, 0);

println(sign);

// Send the signal only if it's the first loop

if (Time == 0)myPort.write('l');

textAlign(RIGHT);

//lampcolor = "GREEN";

strokeWeight(10);

pushMatrix();

translate(width/2,height/2);

line(-320,0,-320+sign*200,0);

triangle(-320+sign*200,20,-320+sign*200,-20,-320+sign*250,0);

textFont(font,20);

//text(lampcolor,0,40);

popMatrix();

popStyle();

}

//Draw an arrow on screen

/*strokeWeight(10);

pushMatrix();

translate(width/2,height/2);

line(0,0,sign*200,0);

triangle(sign*200,20,sign*200,-20,sign*250,0);

textFont(font,20);

text(lampcolor,0,40);

popMatrix();

popStyle();*/

}

void volumeControl(){

String volumeText = "You Can Now Change the Volume";

if(rot>prevRot) {

noFill();stroke(0,255,0);strokeWeight(4);

ellipse(screenCenterVec.x, screenCenterVec.y, 2*rad, 2*rad); fill(255);

fill(0, 255, 0);volumeText = "Forward"; myPort.write('s');

}

/*if (rot=prevRot) {

noFill();stroke(0,255,0);strokeWeight(4);

//ellipse(screenCenterVec.x, screenCenterVec.y, 2*rad, 2*rad);fill(255);

fill(255, 0, 0);volumeText = "Stop"; myPort.write(3);

}*/

else {

noFill();stroke(0,0,255);strokeWeight(4);ellipse(screenCenterVec.x,screenCenterVec.y, 2*rad, 2*rad); fill(255);

fill(0, 0, 255);volumeText = "Backword"; myPort.write('s');

}

prevRot = rot;text(volumeText, screenCenterVec.x, screenCenterVec.y);line(screenCenterVec.x, screenCenterVec.y, screenCenterVec.x+rad*cos(angle),

screenCenterVec.y+rad*sin(angle));

}

void drawHand() {

stroke(255, 0, 0);

pushStyle();

strokeWeight(6);

kinect.convertRealWorldToProjective(handVec, screenHandVec);

point(screenHandVec.x, screenHandVec.y);

popStyle();

noFill();

Iterator itr = handVecList.iterator();

beginShape();while ( itr.hasNext ()) {

PVector p = (PVector) itr.next();PVector sp = new PVector();kinect.convertRealWorldToProjective(p, sp); vertex(sp.x, sp.y);

}

endShape();

}

4.2 Arduino的智能小车程序

#include"SR04.h"

#defineTRIG_PIN1 9 //设置超声波接收引脚

#defineECHO_PIN1 10

#defineTRIG_PIN2 13

#defineECHO_PIN2 14

#defineTRIG_PIN3 15

#defineECHO_PIN3 16

SR04sr04 = SR04(ECHO_PIN1,TRIG_PIN1);

SR04sr05 = SR04(ECHO_PIN2,TRIG_PIN2);

SR04sr06 = SR04(ECHO_PIN3,TRIG_PIN3);

longH_distance = 100,R_distance = 100,L_distance = 100; //存储距离变量

intcar_speed = 50; //设定车速50

booleanH_state = true,R_state = true,L_state = true; //前、左、右三个方向的状态

booleanh_state = true,r_state = true,l_state = true;

constint E1 = 3; ///<Motor1 Speed //设置电机引脚

constint E2 = 11;///<Motor2 Speed

constint E3 = 5; ///<Motor3 Speed

constint E4 = 6; ///<Motor4 Speed

constint M1 = 4; ///<Motor1 Direction

constint M2 = 12;///<Motor2 Direction

constint M3 = 8; ///<Motor3 Direction

constint M4 = 7; ///<Motor4 Direction

charopt;

voidM1_advance(char Speed) ///<Motor1 Advance //电机1前进函数

{

digitalWrite(M1,LOW);

analogWrite(E1,Speed);

}

voidM2_advance(char Speed) ///<Motor2 Advance

{

digitalWrite(M2,HIGH);

analogWrite(E2,Speed);

}

voidM3_advance(char Speed) ///<Motor3 Advance

{

digitalWrite(M3,LOW);

analogWrite(E3,Speed);

}

voidM4_advance(char Speed) ///<Motor4 Advance

{

digitalWrite(M4,HIGH);

analogWrite(E4,Speed);

}

voidM1_back(char Speed) ///<Motor1 Back off //电机1后退函数

{

digitalWrite(M1,HIGH);

analogWrite(E1,Speed);

}

voidM2_back(char Speed) ///<Motor2 Back off

{

digitalWrite(M2,LOW);

analogWrite(E2,Speed);

}

voidM3_back(char Speed) ///<Motor3 Back off

{

digitalWrite(M3,HIGH);

analogWrite(E3,Speed);

}

voidM4_back(char Speed) ///<Motor4 Back off

{

digitalWrite(M4,LOW);

analogWrite(E4,Speed);

}

voidsetup() {

for(int i=3;i<9;i++) //初始化电机引脚为OPTPUT

pinMode(i,OUTPUT);

for(int i=11;i<13;i++)

pinMode(i,OUTPUT);

Serial.begin(9600); //设置波特率

}

voidloop() {

H_distance = sr04.Distance(); //接受超声波数据

// Serial.write( H_distance);

R_distance = sr05.Distance();

// Serial.write(R_distance);

L_distance = sr06.Distance();

// Serial.write(L_distance);

if(H_distance<30) //根据距离判定前、左、右三个方向的状态，如果小于30CM则为false

H_state = false;

else{

H_state = true;

h_state = true;

}

if(L_distance<30)

L_state = false;

else{

L_state = true;

l_state = true;

}

if(R_distance<30)

R_state = false;

else{

R_state = true;

r_state = true;

}

opt = Serial.read(); //接受Kinect发送的字符信息

if(H_state || h_state){ //f为前进

if(H_state){

if(opt == 'f')

qian();

}

else{

ting();

h_state = false;

}

if(R_state || r_state){ //r为向右平移

if(R_state){

if(opt == 'r')

you();

}

else{

ting();

r_state = false;

}

if(L_state || l_state){ //l为向左平移

if(L_state){

if(opt == 'l')

zuo();

}

else{

ting();

l_state = false;

}

if(opt == 'b') //b后退

hou();

if(opt == 's') //s停止

ting();

}

voidqian(){ //前进函数

M1_advance(car_speed);

M2_back(car_speed);

M3_advance(car_speed);

M4_back(car_speed);

}

voidhou(){ //后退函数

M1_back(car_speed);

M2_advance(car_speed);

M3_back(car_speed);

M4_advance(car_speed);

}

voidyou(){ //向右平移函数

M1_advance(car_speed);

M2_advance(car_speed);

M3_advance(car_speed);

M4_advance(car_speed);

}

voidzuo(){ //向左平移函数

M1_back(car_speed);

M2_back(car_speed);

M3_back(car_speed);

M4_back(car_speed);

}

voidting(){ //停止函数

M1_back(0);

M2_back(0);

M3_back(0);

M4_back(0);

}

void zuozhuan(){ //右转函数

M1_back(car_speed);

M2_advance(car_speed);

M3_advance(car_speed);

M4_back(car_speed);

}

void youzhuan(){ //左转函数

M1_advance(car_speed);

M2_back(car_speed);

M3_back(car_speed);

M4_advance(car_speed);

}