本智能小车体感控制系统如图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);
}