避障代码
i
cludeServoh
定义四种状态
defi
eSTOP
0
defi
eFORWARD1
defi
eBACKWARD2
defi
eTURNLEFT3
defi
eTURNRIGHT4
引脚初始化i
tleftMotor14i
tleftMotor25i
trightMotor16i
trightMotor27
舵机引脚初始化i
ti
putPi
8i
toutputPi
12
ServomyServo
voidsetupmyServoattach9测速引脚初始化pi
ModeleftMotor1OUTPUTpi
ModeleftMotor2OUTPUTpi
ModerightMotor1OUTPUTpi
ModerightMotor2OUTPUT
pi
Modei
putPi
INPUTpi
ModeoutputPi
OUTPUT
voidloopavoida
ce
voidmotorRu
i
tcmd
fswitchcmdcaseFORWARDa
alogWriteleftMotor150a
alogWriteleftMotor20a
alogWriterightMotor150a
alogWriterightMotor20breakcaseBACKWARDa
alogWriteleftMotor10a
alogWriteleftMotor250a
alogWriterightMotor10a
alogWriterightMotor250breakcaseTURNLEFTa
alogWriteleftMotor10a
alogWriteleftMotor2150a
alogWriterightMotor1150a
alogWriterightMotor20breakcaseTURNRIGHTa
alogWriteleftMotor1150a
alogWriteleftMotor20a
alogWriterightMotor10a
alogWriterightMotor2150breakdefaulta
alogWriteleftMotor10a
alogWriteleftMotor20a
alogWriterightMotor10a
alogWriterightMotor20break
voidavoida
ce
i
tposi
tdis3距离motorRu
FORWARDmyServowrite90dis1getDista
ce中间
ifdis120
motorRu
STOP
fforpos90pos170pos1

myServowritepos
将舵机转到“pos”的位置
delay15

dis2getDista
ce左边
forpos170pos10pos1

myServowritepos
将舵机转到“pos”的位置
delay15
ifpos90
dis1getDista
ce中间

dis0getDista
ce右边
forpos10pos90pos1

myServowritepos
将舵机转到“pos”的位置

ifdis0dis2右边距离障碍的距离比左边近

左转
motorRu
TURNLEFT
delay600

else右边距离障碍的距离比左边远

右转
motorRu
TURNRIGHT
delay600



i
tgetDista
ce
digitalWriteoutputPi
LOW使发出发出超声波信号接口低电平2μsdelayMicroseco
ds2digitalWriteoutputPi
HIGH使发出发出超声波信号接口高电平10μs，这里是至少10μsdelayMicroseco
ds10digitalWriteoutputPi
LOW保持发出超声波信号接口低电平i
tdista
cepulseI
i
putPi
HIGH读出脉冲时间dista
cedista
ce58将脉冲时间转化为距离（单位：厘米）
fretur
dista
ce
舵机模块连接方式
GND：接开发板GND引脚VCC：接开发板“5V”引脚Echo：接开发板8号引脚Trig：接开发板12号引脚
前进代码
引脚初始化i
tleftMotor14i
tleftMotor25i
trightMotor16i
trightMotor27
voidsetuppi
ModeleftMotor1OUTPUTpi
ModeleftMotor2OUTPUTpi
ModerightMotor1OUTPUTpi
Mr