导轮角可变的越野履带机器人-机器谱-robotway

对传统的履带固定式机器人进行结构方面的改进设计，使前导轮可以进行角度调整并张紧调整后的履带，然后对该履带机器人的越障能力进行了理论计算分析，运用探索者模块，进行导轮角不同的越野实践，对导轮角进行优化，优化结果表明，通过调节前导轮的角度可以提高固定机器人的越障高度、跨沟宽度和爬坡角度，验证了该改进机器人的可行性。与传统的固定式履带机器人相比，其越障能力有一定改善和提高；与四摆臂履带机器人相比，其拥有传统固定式履带机器人的优点。

关键词：前导轮、小型固定式履带机器人、结构改进、越野能力

1. 市场调研及前景说明

履带式机器人与轮式、足式移动机器人相比，其越障能力和地面适应能力明显增强，因此在工程机械、军事等环境复杂领域得到广泛应用。从20世纪80年代起，国外就对小型履带式机器人展开了系统的研究，比较有影响的是美国的Packbot机器人、UR-BOT,NUGV和Talon机器人[1]。目前履带机器人可分为固定式履带机器人和摆臂式履带机器人两类，固定式履带机器人具有结构简单、控制容易和成本低的优点，但越障能力的不足，使其在环境复杂的领域并不能得到广泛的应用。摆臂式履带机器人虽有较好的越障能力和运动灵活性的优点，在国内也得到了更加广泛的研究与应用，但也有结构复杂和成本高的缺点。

本项目中设计的越野履带机器人是针对以上两种机器人的改进，这种机器人保持了固定式履带机器人的结构，所以相比于摆臂式履带机器人成本较低。同时由于前导轮的角度可以调节，所以相比于固定式履带机器人越野能力增强。

作者：周全白金宝向召华张超智

单位：辽宁科技大学

指导老师：李玲玲

作品说明

导轮角可变的越野履带机器人

2. 作品的先进性说明

因为固定式履带机器人的整体结构大同小异，原理相同，所以本文中设计的履带机器人以任一款常见的固定式履带机器为基础进行改进。原有的固定式履带机器人与改进后的履带机器人整体结构所改进后履带机器人的张紧机构和前导轮的安装方式有了明显改变，两个部分的设计就是提高固定式履带机器人越障力的关键。

一般的固定式履带机器人的前导轮轴是固联在机器人车体的两侧，而履带机器人的越障能力又和前导轮的仰角有关。所以当前导轮安装完成，在质心、履带张紧力等其他与越障能力有关因素一定的情况下，履带机器人的越障能力也随之确定。前导轮调节机构的设计，就可以满足在其他条件一定的情况下，通过螺丝位置调节前导轮的仰角而到改变越障能力的目的。

履带式移动机器人具有良好的地形适应性能，容易跨越楼梯、台阶等障碍物。在爬楼梯的过程中，与楼梯的接触点可能有一个、两个或者多个，除考虑动作规划外，还必须考虑接触作用时的稳定性，防止机器人突然倾翻。稳定性分为静态和动态稳定性，静态稳定性已有不少研究，判断方法较为成熟。动态稳定性用于机器人在运动过程中的稳定性判断，判断方法也有许多，但多用于足式移动机器人稳定性的判断。近年来，关节式履带机器人的动态稳定性研究越来越受到重视，但未同时考虑机器人质心惯性力、驱动轮和摆臂轮惯性力矩的影响。在越障过程中摆臂旋转角度的变化，将引起机器人质心位置的变化，对机器人的动力学方程和动态稳定性有很大影响。

在关节式履带机器人爬楼梯过程中，履带棱与楼梯台阶相互接触，可能引起机器人的滑移与滑转现象，严重时导致机器人发生倾翻。为避免此种情况，必须考虑机器人与楼梯接触的不滑移充分必要条件，结合机器人爬楼梯过程中正确的动作规划、质心位置变化引起的惯性力和惯性力矩等因素[10]r推导出机器人不发生倾翻的动态稳定性判断准则，利用此准则判定关节式履带机器人爬楼梯的动态稳定性，使机器人能够安全、稳定、成功爬上楼梯。关节式履带机器人不同的结构尺寸使得机器人与楼梯台阶接触点的数目可能不同，但分析方法类似，以此推导机器人与楼梯台阶多点接触的动态稳定性问题。

3. 设计过程

3.1 结构设计

3.1.1 结构设计概述

履带式移动机器人具有良好的地形适应性能，容易跨越楼梯、台阶等障碍物。在爬楼梯的过程中，与楼梯的接触点可能有一个、两个或者多个，除考虑动作规划外，还必须考虑接触作用时的稳定性，防止机器人突然倾翻。稳定性分为静态和动态稳定性，静态稳定性已有不少研究，判断方法较为成熟。动态稳定性用于机器人在运动过程中的稳定性判断，判断方法也有许多，但多用于足式移动机器人稳定性的判断。近年来，关节式履带机器人的动态稳定性研究越来越受到重视，但未同时考虑机器人质心惯性力、驱动轮和摆臂轮惯性力矩的影响。在越障过程中摆臂旋转角度的变化，将引起机器人质心位置的变化，对机器人的动力学方程和动态稳定性有很大影响。

在关节式履带机器人爬楼梯过程中，履带棱与楼梯台阶相互接触，可能引起机器人的滑移与滑转现象，严重时导致机器人发生倾翻。为避免此种情况，必须考虑机器人与楼梯接触的不滑移充分必要条件，结合机器人爬楼梯过程中正确的动作规划、质心位置变化引起的惯性力和惯性力矩等因素推导出机器人不发生倾翻的动态稳定性判断准则，利用此准则判定关节式履带机器人爬楼梯的动态稳定性，使机器人能够安全、稳定、成功爬上楼梯。关节式履带机器人不同的结构尺寸使得机器人与楼梯台阶接触点的数目可能不同，但分析方法类似，以此推导机器人与楼梯台阶多点接触的动态稳定性问题。

3.1.2 越障方案分析与设计

自行研制的关节式履带机器人由两前摆臂和机器人本体组成，如下图所示。机器人本体和摆臂的橡胶履带均包含有内履带齿和外履带棱，内履带齿与履带轮啮合传动，外履带棱增大与地面接触的附着力。机器人本体采用后轮驱动，两后轮由两台直流伺服电动机分别驱动，以实现机器人的直线行走、转弯功能。两前摆臂由一台直流伺服电动机驱动，保证两摆臂的同步转动。

3.1.3 攀爬斜坡计算分析

机器人爬坡如下图所示：机器人在进行爬坡运动时，需要考虑移动过程中的静稳定性。

机器人不发生颠覆的条件是重心作用线保持在后轮与地面的接触点之前设机器人能攀爬的最大坡度为a，则不发生颠覆需要满足的条件为

由上式分析可得，当质心Z的距离增大时，爬坡角度也增大，减小前导轮角度可以增加质心，即Z的值，所以改进后的固定履带机器人可以增大最大爬坡角度。

3.1.4 跨越沟道分析

固定式履带机器人跨越沟道需要满足的条件为：其质心在越过近侧沟道边缘时，机器人履带前端支撑在沟道的远侧边缘线上，当质心越过远侧边缘线时，机器人履带后端不得脱离近侧边缘线。所以当固定履带机器人长度越长则跨沟能力越强。而改进后的机器人只须把前导轮角度调节为0使其长度最大化则明显可以提高跨沟长度。

3.2 硬件设计

3.2.1 Basra主控板

（1）简介

Basra是一款基于Arduino开源方案设计的一款开发板，通过各种各样的传感器来感知环境，通过控制灯光、马达和其他的装置来反馈、影响环境。板子上的微控制器可以在Arduino、eclipse、Visual Studio等IDE中通过c/c++语言来编写程序，编译成二进制文件，烧录进微控制器。Basra的处理器核心是ATmega328，同时具有14路数字输入/输出口（其中6路可作为PWM输出），6路模拟输入，一个16MHz晶体振荡器，一个USB口，一个电源插座，一个ICSP header 和一个复位按钮。

主CPU采用AVR ATMEGA328型控制芯片，支持C语言编程方式；该系统的硬件电路包括：电源电路、串口通信电路、MCU 基本电路、烧写接口、显示模块、AD/DA 转换模块、输入模块、IIC 存储模块等其他电路模块电路。控制板尺寸不超过 60*60mm，便于安装。CPU 硬件软件全部开放，除能完成对小车控制外，还能使用本实验板完成单片机所有基础实验。供电范围宽泛，支持 5v~9v 的电压，干电池或锂电池都适用。编程器集成在控制板上，通过 USB 大小口的方式与电脑连接。下载程序。开放全部底层源代码。控制板含 3A6V的稳压芯片，可为舵机提供 6v 额定电压。板载 8*8led 模块采用 MAX7219 驱动芯片。板载一片直流电机驱动芯片 FAN8100MTC，可同时驱动两个直流电机。板载 USB 驱动芯片及自动复位电路，烧录程序时无需手动复位。2 个 2*5 的杜邦座扩展坞，方便无线模块、OLED、蓝牙等扩展模块直插连接，无需额外接线。

（2）特点

开放源代码的电路图设计，程序开发接口免费下载，也可依需求自己修改。

可以采用 USB 接口供电，不需外接电源，也可以使用外部 DC 输入。

支持 ISP 在线烧，可以将新的“bootloader”固件烧入芯片。有了 bootloader 之后，可以在线更新固件。

支持多种互动程序，如：Flash、Max/Msp、VVVV、PD、C、Processing 等。

具有宽泛的供电范围，电源电压可任选 3v~12v 的电源

采用堆叠设计，可任意扩展11. 电源端口，接入电池或适配器连接主控板尺寸不超过 60mm*60mm，便于给小型机电设备安装

板载 USB 驱动芯片及自动复位电路，烧录程序时无需手动复位

（3）参数

处理器 ATmega328

工作电压 5V

输入电压（推荐） 7-12V

输入电压（范围） 6-20V

数字 IO 脚 14 (其中 6 路作为 PWM 输出）

模拟输入脚 6

IO 脚直流电流 40 mA

3.3V 脚直流电流 50 mA

Flash Memory 32 KB （ATmega328，其中 0.5 KB 用于 bootloader）

SRAM 2 KB （ATmega328）

EEPROM 1 KB （ATmega328）

工作时钟 16 MHz

（4）电源

Basra 可以通过 3 种方式供电，而且能自动选择供电方式：

外部直流电源通过电源插座供电。

电池连接电源连接器的 GND 和 VIN 引脚。

USB 接口直接供电。

电源引脚说明：

VIN --- 当外部直流电源接入电源插座时，可以通过 VIN 向外部供电；也可以通过此引

脚向 UNO 直接供电；VIN 有电时将忽略从 USB 或者其他引脚接入的电源。

5V --- 通过稳压器或 USB 的 5V 电压，为 UNO 上的 5V 芯片供电。

3.3V --- 通过稳压器产生的 3.3V 电压，最大驱动电流 50mA。

GND --- 地脚。

（5）存储器

ATmega328 包括了片上 32KB Flash，其中 0.5KB 用于 Bootloader。同时还有 2KB

SRAM 和 1KB EEPROM。

（6）输入输出

14 路数字输入输出口：工作电压为 5V，每一路能输出和接入最大电流为 40mA。每一路配置了 20-50K 欧姆内部上拉电阻（默认不连接)。除此之外，有些引脚有特定的功能

串口信号 RX（0 号）、TX（1 号）: 与内部 ATmega8U2 USB-to-TTL 芯片相连，提供 TTL 电压水平的串口接收信号。

外部中断（2 号和 3 号）：触发中断引脚，可设成上升沿、下降沿或同时触发。

脉冲宽度调制 PWM（3、5、6、9、10 、11）：提供 6 路 8 位 PWM 输出。

SPI（10(SS)，11(MOSI)，12(MISO)，13(SCK)）：SPI 通信接口。

LED（13 号）：Arduino 专门用于测试 LED 的保留接口，输出为高时点亮 LED，反之输出为低时 LED 熄灭。

6 路模拟输入 A0 到 A5：每一路具有 10 位的分辨率（即输入有 1024 个不同值），默认输入信号范围为 0 到 5V，可以通过 AREF 调整输入上限。除此之外，有些引脚有特定功能。

TWI 接口（SDA A4 和 SCL A5）：支持通信接口（兼容 I2C 总线）。

AREF：模拟输入信号的参考电压。

Reset：信号为低时复位单片机芯片。

（7）通信

串口：ATmega328 内置的 UART 可以通过数字口 0（RX）和 1（TX）与外部实现串口通信；ATmega16U2 可以访问数字口实现 USB 上的虚拟串口。

TWI（兼容 I2C）接口

SPI 接口

（8）下载程序

Basra 上的 ATmega328 已经预置了 bootloader 程序，因此可以通过 Arduino 软件直接下载程序到主控板中。

可以直接通过主控板上 ICSP header 直接下载程序到 ATmega328。

（9）注意事项

USB口附近有一个可重置的保险丝，对电路起到保护作用。当电流超过500mA时，会断开USB连接。

主控板提供了自动复位设计，可以通过主机复位。这样通过Arduino软件下载程序到主控板中时，软件可以自动复位，不需要在复位按钮。

3.2.2 直流电机

减速比1:87

额定电压4.5v

额定电流180mA

扭力5kgf.cm

3.2.3 黑标传感器

黑标/白标传感器可以帮助进行黑线/白线的跟踪，可以识别白色/黑色背景中的黑色/白色区域，或悬崖边缘。寻线信号可以提供稳定的输出信号，使寻线更准确更稳定，有效距离在0.7cm~3cm之间。

工作电压：4.7～5.5V，工作电流：1.2mA

① 固定孔，便于用螺丝将模块固定于机器人上

② 四芯输入线接口，连接四芯输入线

③ 黑标/白标传感器元件，用于检测黑线/白线信号

注意事项：黑标/白标传感器的安装应当贴近地面且与地面平行，这样才能更加灵敏并且有效的检测到信号。

3.2.4 触须传感器

触须传感器可以检测到物体对弹簧触须的有效触动。安装时通常是将弹簧与地面平行，有效触动角度45度。

① 固定孔，便于用螺丝将模块固定于机器人上

② 四芯输入线接口，连接四芯输入线

③ 弹簧触须：与障碍物接触后发生弹性形变，触发传感器

注意事项：触须感应器需要安装在机器人前端容易被触碰到的位置，需要弹簧触须被物体折弯至接触金属卡桥才会被触发。

3.3 软件设计

介绍作品各环节的控制策略或算法分析，介绍作品实现的主要功能，并给出主程序框架的流程图和核心程序的代码。

3.3.1 控制策略

主要有5个黑标、2个触碰开关和1个触须开关；开关1程序起停开关，开关2为路线选择；5个黑标用于循迹；触须开关用于检测障碍物。

3.3.2算法分析

开启电源后，开关2选择路线；放好小车，触碰开关1，启动小车。

小车用5个黑标循迹，具体为：只有中间的黑标为1时，前行，左边黑标为1时，右转；右边黑标为1时，左转；当左右边黑标都为1时，直行。

当触须开关为1时，检测到有障碍物，关闭循迹；延时一定时间，待通过障碍物后，重新打开循迹。

左转控制：左电机慢速反转，右电机正转；右转控制：右电机慢速反转，左电机正转。

3.3.3 主要功能

循迹小车越障功能。

3.3.4 主程序流程图

3.3.5 核心程序源代码

#include <LedControl.h>

LedControl lc = LedControl(12,11,13,1);

char YN = 0;

int k = 0;

int pwm = 250;

int pw = 220;

int pattern = 0;

int sensor = 0;

int ii=0;

int frequency = 0;

//***********************************************************************

void speed_1(char a, char b);

int testing();

void right();

void left();

void go();

//***********************************************************************

void setup() {

Serial.begin(9600);

lc.shutdown(0,false);

lc.setIntensity(0,8);

lc.clearDisplay(0);

pinMode(5,OUTPUT);

pinMode(6,OUTPUT);

pinMode(9,OUTPUT);

pinMode(10,OUTPUT);

pinMode(4,INPUT);

pinMode(7,INPUT);

pinMode(A2,INPUT);

pinMode(A4,INPUT);

pinMode(A3,INPUT);

pinMode(2,INPUT);

pinMode(8,INPUT);

pinMode(3,INPUT);

}

//**** sensor == 0 0 0 x x x x x ******

//**************************************************************************

void loop() {

if (frequency == 0 && (!digitalRead(3)==1)){

delay(30);

if((!digitalRead(3)) == 1){

pattern++;

delay(300);

}

if ( (!digitalRead(2) == 1) && YN == 0 ){

delay(10);

if((!digitalRead(2)) == 1){

YN = 1;

delay(250);

}

if ( (!digitalRead(2) == 1) && YN == 1){

delay(10);

if((!digitalRead(2)) == 1){

YN = 0;

speed_1(0,0);

delay(250);

}

// 1 ***************************************************************** 1

if(pattern == 1 && YN == 1) { speed_1(1,255); }

// 2 ***************************************************************** 2

if(pattern == 2 && YN == 1) { testing(); go(); }

// 3 ***************************************************************** 3

if(pattern == 3 && YN == 1) {

testing();

switch(frequency){

case 0: for (ii=0; ii<200; ii++){ testing();go();delay(10); }

lc.setRow(0,7,0xff); speed_1(1,255); delay(4000); frequency=1; lc.setRow(0,7,0x00);

break;

case 1: testing(); go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(5000); frequency=2; lc.setRow(0,7,0x00); }

break;

default : testing(); go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(5000); frequency=2; lc.setRow(0,7,0x00); }

}

// 4 ***************************************************************** 4

if (pattern == 4 && YN == 1){

testing();

switch(frequency)

{

case 0:

testing(); go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(6000); frequency=1; lc.setRow(0,7,0x00); }

break;

case 1:

testing(); go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(4300); lc.setRow(0,7,0x00); }

break;

case 2:

testing(); go();

if (sensor == 0xff) { frequency++; delay(500); }

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(4000); lc.setRow(0,7,0x00); }

break;

case 3:

testing(); go();

if (sensor == 0xff) {delay(10); testing(); if(sensor == 0xff) left(); frequency++;}

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(4000); lc.setRow(0,7,0x00); }

break;

default: testing(); go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,200); delay(500); speed_1(1,255); delay(4000); lc.setRow(0,7,0x00); }

}

// 5 ***************************************************************** 5

if (pattern == 5 && YN == 1){

testing();

switch(frequency)

{

case 0: switch(sensor){

case 0x02: delay(10); testing(); if(sensor == 0x02) speed_1(2,240); break; //** 0000 0010 ** right

case 0x06: delay(10); testing(); if(sensor == 0x06) speed_1(2,240); break; //** 0000 0110 **

case 0x08: delay(10); testing(); if(sensor == 0x08) speed_1(3,240); break; //** 0000 1000 ** left

case 0x0c: delay(10); testing(); if(sensor == 0x0c) speed_1(3,240); break; //** 0000 1100 **

default: speed_1(1,pwm);

}

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff);speed_1(1,pwm); delay(6500); frequency=1; lc.setRow(0,7,0x00); }

break;

case 1:

testing();

go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,255); delay(5000); delay(5000); delay(5000); speed_1(0,0); frequency=2; lc.setRow(0,7,0x00); }

break;

case 2:

testing();

go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff); speed_1(1,255); delay(5000); frequency=2; lc.setRow(0,7,0x00); }

break;

}

// 6 *****************************************************************6

if (pattern == 6 && YN == 1){

testing();

switch(frequency)

{

case 0: switch(sensor){

case 0x02: delay(10); testing(); if(sensor == 0x02) speed_1(2,240); break; //** 0000 0010 ** right

case 0x06: delay(10); testing(); if(sensor == 0x06) speed_1(2,240); break; //** 0000 0110 **

case 0x08: delay(10); testing(); if(sensor == 0x08) speed_1(3,240); break; //** 0000 1000 ** left

case 0x0c: delay(10); testing(); if(sensor == 0x0c) speed_1(3,240); break; //** 0000 1100 **

default: speed_1(1,pwm);

}

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff);speed_1(1,pwm); delay(3900); frequency=1; lc.setRow(0,7,0x00); }

break;

case 1:

testing();

go();

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff);speed_1(1,255); delay(3800); speed_1(2,255); delay(3200); speed_1(1,255); delay(5000); delay(5000); delay(5000); frequency=0; lc.setRow(0,7,0x00); }

break;

}

// 7 *****************************************************************7

if (pattern == 7 && YN == 1){

testing();

switch(frequency)

{

case 0: switch(sensor){

case 0x02: delay(10); testing(); if(sensor == 0x02) speed_1(2,240); break; //** 0000 0010 ** right

case 0x06: delay(10); testing(); if(sensor == 0x06) speed_1(2,240); break; //** 0000 0110 **

case 0x08: delay(10); testing(); if(sensor == 0x08) speed_1(3,240); break; //** 0000 1000 ** left

case 0x0c: delay(10); testing(); if(sensor == 0x0c) speed_1(3,240); break; //** 0000 1100 **

default: speed_1(1,pwm);

}

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff);speed_1(1,pwm); delay(5400); frequency=1; lc.setRow(0,7,0x00); }

break;

case 1:

testing();

go();

if (!digitalRead(A2)==1) { delay(10); if(!digitalRead(A2)==1) { lc.setRow(0,7,0xf0); left(); lc.setRow(0,7,0x00); frequency = 2;}}

break;

case 2:

testing();

go();

break;

}

// 8 *****************************************************************8

if (pattern == 8 && YN == 1){

testing();

switch(frequency)

{

case 0:

lc.setRow(0,7,0xff); speed_1(1,255); delay(7000); frequency=1; lc.setRow(0,7,0x00); break;

case 1:

testing();

go();

if (!digitalRead(8)==1) { delay(10); if(!digitalRead(8)==1) { lc.setRow(0,7,0xf0); speed_1(1,255); delay(5000); lc.setRow(0,7,0x00); }}

break;

case 2:

testing();

go();

if (!digitalRead(8)==1) { delay(10); if(!digitalRead(8)==1) { lc.setRow(0,7,0xf0); left(); lc.setRow(0,7,0x00); frequency = 2;}}

break;

}

///9**************************************************************9

if (pattern == 9 && YN == 1){

testing();

switch(frequency)

{

case 0: switch(sensor){

case 0x02: delay(10); testing(); if(sensor == 0x02) speed_1(2,240); break; //** 0000 0010 ** right

case 0x06: delay(10); testing(); if(sensor == 0x06) speed_1(2,240); break; //** 0000 0110 **

case 0x08: delay(10); testing(); if(sensor == 0x08) speed_1(3,240); break; //** 0000 1000 ** left

case 0x0c: delay(10); testing(); if(sensor == 0x0c) speed_1(3,240); break; //** 0000 1100 **

default: speed_1(1,pwm);

}

if (!digitalRead(8)==1) { lc.setRow(0,7,0xff);speed_1(1,pwm); delay(5400); frequency=1; lc.setRow(0,7,0x00); }

break;

case 1:

testing();

go();

if (!digitalRead(A2)==1) { delay(10); if(!digitalRead(A2)==1) { lc.setRow(0,7,0xf0); right(); lc.setRow(0,7,0x00); frequency = 2;}}

break;

case 2:

testing();

go();

break;

}

lc.setRow(0,6,pattern);

lc.setRow(0,5,frequency);

}

//*************************************************************************

void speed_1(char a, char b)

{

switch (a)

{

case 0: digitalWrite(5,LOW); digitalWrite(6,LOW); digitalWrite(9,LOW); digitalWrite(10,LOW); break; //stop

case 1: digitalWrite(5,LOW); analogWrite(6,b); digitalWrite(9,LOW); analogWrite(10,b); break; //GO

case 2: analogWrite(5,b/2); digitalWrite(6,LOW); digitalWrite(9,LOW); analogWrite(10,b); break; //right

case 3: digitalWrite(5,LOW); analogWrite(6,b); analogWrite(9,200); digitalWrite(10,LOW); break; //left

case 4: analogWrite(5,b); digitalWrite(6,LOW); digitalWrite(9,b/2); analogWrite(10,LOW); break; // back

}

//***********************************************************************

int testing()

{

if ((!digitalRead(A3)) == 1) { sensor = sensor | 0X01; } else { sensor = sensor & 0Xfe;} //right_right

if ((!digitalRead(4)) == 1) { sensor = sensor | 0X02; } else { sensor = sensor & 0Xfd;} //right

if ((!digitalRead(A4)) == 1) { sensor = sensor | 0X04; } else { sensor = sensor & 0Xfb;} //in

if ((!digitalRead(7)) == 1) { sensor = sensor | 0X08; } else { sensor = sensor & 0Xf7;} //left

if ((!digitalRead(A2)) == 1) { sensor = sensor | 0X10; } else { sensor = sensor & 0Xef;} //left_left

lc.setRow(0,0,sensor);

return sensor;

}

//****************************************************************************************************************************

void right()

{

speed_1(2,240);

delay(1200);

speed_1(1,240);

delay(500);

speed_1(2,240);

delay(1200);

speed_1(4,240);

delay(500);

speed_1(2,240);

delay(500);

speed_1(1,240);

delay(1200);

}

//************************************************************************************************************************************************************

void left()

{

speed_1(3,240);

delay(1000);

speed_1(1,240);

delay(500);

speed_1(3,240);

delay(1000);

speed_1(4,240);

delay(500);

speed_1(3,240);

delay(200);

speed_1(1,240);

delay(1000);

}

void go(){

switch(sensor){

case 0x01: delay(10); testing(); if(sensor == 0x01) speed_1(2,pw); k=0; break; //** 0000 0001 ** right

case 0x02: delay(10); testing(); if(sensor == 0x02) speed_1(2,pw); k++; break; //** 0000 0010 ** right

case 0x03: delay(10); testing(); if(sensor == 0x03) speed_1(2,pw); k=0; break; //** 0000 0011 **

case 0x05: delay(10); testing(); if(sensor == 0x05) speed_1(2,pw); k=0; break; //** 0000 0101 **

case 0x06: delay(10); testing(); if(sensor == 0x06) speed_1(2,pw); k=0; break; //** 0000 0110 **

case 0x0d: delay(10); testing(); if(sensor == 0x07) speed_1(2,pw); k=0; break; //** 0000 1101 **

case 0x0b: delay(10); testing(); if(sensor == 0x0a) speed_1(2,pw); k=0; break; //** 0000 1011 **

case 0x0f: delay(10); testing(); if(sensor == 0x0f) speed_1(2,pw); k=0; break; //** 0000 1111 **

case 0x07: delay(10); testing(); if(sensor == 0x07) {

lc.setRow(0,7,0x0f);

for(ii=0; ii<150; ii++){

testing();

switch(sensor){

case 0x02: delay(10); testing(); if(sensor == 0x02) speed_1(2,pw); k++; break; //** 0000 0010 ** right

case 0x03: delay(10); testing(); if(sensor == 0x03) speed_1(2,pw); k=0;break; //** 0000 0011 **

case 0x05: delay(10); testing(); if(sensor == 0x05) speed_1(2,pw); k=0; break; //** 0000 0101 **

case 0x06: delay(10); testing(); if(sensor == 0x06) speed_1(2,pw); k=0; break; //** 0000 0110 **

case 0x0d: delay(10); testing(); if(sensor == 0x07) speed_1(2,pw); k=0; break; //** 0000 1101 **

case 0x0b: delay(10); testing(); if(sensor == 0x0a) speed_1(2,pw); k=0; break; //** 0000 1011 **

case 0x0f: delay(10); testing(); if(sensor == 0x0f) speed_1(2,pw); k=0; break; //** 0000 1111 **

case 0x07: delay(10); testing(); if(sensor == 0x07) speed_1(2,pw); k=0; break;

default: speed_1(1,pwm); k=0;

}delay(10);

} lc.setRow(0,7,0x00);

} break; //** 0000 0111 **

case 0x08: delay(10); testing(); if(sensor == 0x08) speed_1(3,pw); k++; break; //** 0000 1000 ** left

case 0x10: delay(10); testing(); if(sensor == 0x10) speed_1(3,pw); k=0; break; //** 0001 0000 **

case 0x0c: delay(10); testing(); if(sensor == 0x0c) speed_1(3,pw); k=0; break; //** 0000 1100 **

case 0x14: delay(10); testing(); if(sensor == 0x14) speed_1(3,pw); k=0; break; //** 0001 0100 **

case 0x18: delay(10); testing(); if(sensor == 0x18) speed_1(3,pw); k=0; break; //** 0001 1000 **

case 0x16: delay(10); testing(); if(sensor == 0x16) speed_1(3,pw); k=0; break; //** 0001 0110 **

case 0x1a: delay(10); testing(); if(sensor == 0x1a) speed_1(3,pw); k=0; break; //** 0001 1010 **

case 0x1e: delay(10); testing(); if(sensor == 0x1e) speed_1(3,pw); k=0; break; //** 0001 1110 **

case 0x0a: delay(10); testing(); if(sensor == 0x0a) speed_1(3,pw); k=0; break;

case 0x1c: delay(10); testing(); if(sensor == 0x1c) {

lc.setRow(0,7,0xf0);