我的多能智能小车

萧芸凤 · 发表于 2015-5-31 17:22:12

本帖最后由萧芸凤于 2015-6-5 17:38 编辑

5月份开始做四驱小车，一开始就选定了是四轮驱动，Arduino 做主控，然后才是电机的选择，驱动的选择。
关于电机的驱动，现在用的最多的是L298和L293（特别是淘宝上这类模块扩展板很多）。使用过298的模块，不是很好用，5V的电机基本上需要7.2V的电压来驱动而且效率不是很高。查阅了资料才发现298的压降太厉害了，上下桥加起来2A时近5V，就是1A也有3V左右的压降，这样算下来几乎一半的电能都用在芯片上了。293没有用过，不过600mA的驱动电流很多的电机被排除了。

还有一个问题就是很多的驱动板都需要3个端口来控制，两个是电机的方向选择，一个是PWM控制转速。如此一来四个电机需要12个接口，Arduino没有这么多的资源来使用。然后很偶然的一个机会，在逛淘宝时发现了一个I2C的电机驱动板，而且还是扩展板的形式，连接比较方便。最巧的是居然是四电机的驱动板，使用东芝的TB6612FNG H桥电机驱动芯片。这个是MOSFET的结构，不是L298的晶体管，整体压降很少，每路1.2A的驱动能力，3.2A的峰值电流超过了293两倍。

驱动板确定了然后就是电机的选择。做小车最通常的是黄色的香蕉电机（130电机），这个在淘宝上一抓一大把。以前也是用过，不是很好减速比还行吧，好像是1:40，具体的忘记了，有一点就是驱动电流要求大，但是驱动力矩不够。还有一点体积比较大，何其配套的车轮一比较大。电机在运行时抖动比较厉害，之前做的小车只能全速运行，PWM使用时走起来一顿一顿的。
小型化电机时看到了N20，这个非常满足要求。自带减速机构，而且还是金属的齿轮减速系统。各项参数也很不错，驱动电压3-12V，转速3-1000rpm，驱动力矩7kgcm，外形12*10*25很少小巧，噪声也比香蕉电机小了很多。
这个电机其实以前使用过，公司的一个电动钟表螺丝批用的就是这个，噪声和震动控制的都很不错。还有在淘宝上发现了它的固定支架，DFROOBT，虽然他家的东西是出了名的贵，性价比严重的不好，可是也只有他们在买啊，而且质量倒是不错的，之前买过的开发板模块什么的就数他们家的做工精良。
选择了两对支架，逛店铺的时候发现了配套车轮，42mm直径，也很小巧。同时还带有12齿编码盘，虽然暂时不需要速度带闭环控制，但是车轮有这个也算是不错吧，以后也许可以用上。
如此一来一个小车基本上可以做了，驱动、电机、车轮、主控都好了。车架选择上没有采用现有的，主要是自己做车，如果买来现成车架还是自己做吗，那仅仅是组装了。在亚克力和PCB中选择了PCB，亚克力切割现在做不到。
电机点安装，驱动什么的都是很简单。既然要与众不同，那就要增加一些别人没有的，于是想到了灯光系统。
第一是方向灯，左右转向用，然后是刹车灯，还有前大灯。直接端口控制又要使用有限灯IO了，于是用595来扩展，使用4个接口其他灯四个也可以方便以后增加功能。
方向灯需要闪烁，如果用595来控制闪烁那就需要不停的写入数据，总觉得有些奇怪。既然这样闪光控制就需要独立出来来，RC电路虽然简单但是不好控制，555的暂稳态电路简单实用。
闪光频率选在在1秒左右，测试几种组合，还有是电容的体积，闪光效率等待，使用了现在的这个组合，R1，R2为20K，C1为22uF，周期924ms，占空比67%。
LED的控制使用与门，555的输出接一端，595的输出接另一个，然后与门输出接LED。74系列着了74HC08，很典型的四与门芯片。
四个闪光用了两个控制方向灯，另两个备用，595直接使用2两个控制前后灯，如此还有两个闪光和两个常态可以备用。
控制方面预留巡线，避障。巡线最少要三个输入，避障也需要两个。然后用了165扩展了8个输入，这样巡线避障等可以同时连接，需要什么功能可以自行选择。
为了体现PWM控制的特点需要实时的控制电机的加速与减速，这个就需要知道前方的距离了。在现有的模块中也只有超声波可以得到距离数值，红外什么的只能得到一个数值量，设定的数字。
连接线路，编写程序，一切都以为结束了。等到都准备好了才发现，电机一运行就会对灯光系统，对超声波系统产生干扰。一开始以为是辐射干扰，使用外置的595模块确实会好许多，然后对比发现了ST需要接下拉电阻。之后在测试还是不行。几经反复发现不只是辐射干扰，最主要的干扰来自马达的输入线路。
然后开始考虑如果来过滤这个干扰。首先确认干扰来源，这个网上讲的很是清楚了，换相时造成的电压波动。这个是周期性的，应该属于交流信号了吧，考虑各种滤波措施了。RC、LC、RL等各种手段都需要尝试。
在不了解各类滤波技术的时候只能先凭经验了。和电机串联的只能是电感和电阻，电容串联相当于断路，毕竟驱动是用的直流电源。电阻也不在选择之列，主要是会影响马达。
如此一来只有LC滤波了，串联了L，画出电路图一开居然是LC低频率波。
不知道该选择怎样的频率，把手头现有的电感和电容进行了组合，计算一下可能的组合，然后看一下各自的频率进行测试。

	220uH	100uH	68uH	47uH	33uH	22uH	10uH
220uF		1.07
100uF	1.07	1.59	1.93	2.32	2.77	3.39	5.03
68uF	1.3	1.93	2.34	2.82	3.36	4.11	6.1
47uF	1.57	2.32	2.82	3.39	4.04	4.95	7.34
33uF	1.87	2.77	3.36	4.04	4.82	5.91	8.76
22uF	2.29	3.39	4.11	4.95	5.91	7.23	10.73
10uF	3.39	5.03	6.1	7.34	8.76	10.73	15.92

有不少的组合是重复的，虽然不知道相同的频率不同的电感和电容组合会有什么影响，但是现在就需要测试一下。
电机的PWM频率是1-1.6KHz的，就在这个附近开始测试。选择了220uH／47uF、100uH／100uF这两个组合。

220uH／47uF灯光还是会受到干扰，100uH／100uF干扰小了许多，有时候没有有时候又有了，但是电机速度受影响比较大，速度下降明显。

继续测试另一个组合，这次选择小于1.6KHz的，由于手头没有68uF的电容，只能先测试220uH／100uF、100uH／220uF这两个组合。
结果不是很好，干扰现不是是否接触，电机基本上是动了。分析原因可能是低通滤波将1K以上的过滤了，而PWM是用的1.6KHz的。
知道了原因就测试另一个组合，高于1.6的，先测试100uH／47uF、100uH／33uF、68uH／47uF、68uH／33uF四个组合。
100uH／47uF干扰基本解除了，但是马达速度还是受到了影响；100uH／33uF马达速度倒是还好，干扰不是滤除的很好，灯光可以不受干扰，但是超声波还是受到了干扰；68uH／47uF干扰滤除的还可以，偶尔能看到灯光到闪烁；68uH／33uF灯光不受干扰，超神波的影响也没有那么大，但是总觉得没有之前的那些好。
分析一下，68uH／33uF的频率是3.36KHz的，效果没有前面的好可能的原因是频率高了，一些低于这个频率的信号还是进入了系统。100uH／47uF的最好，100uH／33uF还是会受到影响说明了干扰频率来源在2.32KHz附近。所以其他的两个组合滤除的是高于2.8KHz附近的，一些干扰还是存在的。
本来还应该测试一下220uH／33uF、68uH／100uF的，这个是低于2K的组合，但是手头的100uF电容和33uF电容数量不足，没办法满足4个电机所需要的8个要求所以没在做测试。虽然100uH／47uF这个组合会影响一下马达速度，但是实际测试发现马达的加速和减速效果最好。之前的加速和减速是突然进行的，现在明显的有个过程，显然是在滤波的时候也同时影响到了马达对PWM的响应。这个是什么原理，为什么这样怎样来避免这个不是很清楚，好在这不是个问题，反而是个意外的惊喜。所以说这一次就只是记录了下来，以后如果遇到了在继续研究。
遥控方面，通常来说是蓝牙、APC220、24L01几个，Xbee太贵了。蓝牙比较简单，串口透传，安卓有很多的免费软件，就是距离短；APC220号称可以传输1000米，没测试过，仅仅试过几米的，超过5米的时候就经常受到错误的数据，而且控制端要不是电脑要不是另一个控制板；24L01的速度是最快的，现在的库也能保证数据接收，但是要占用4个端口，距离也不咋地，同样需要电脑或另一个控制器。
闲逛的时候看到了DTMF模块（双频多音），电话拨号时用的数据传输方法，看了一下也挺简单的。芯片MT8870，四个输出Q1～Q4组合成16个数据，STD用来判断是否有数据到达，其他的端口可以默认不需要控制。这样一来只需要一个接口接STD来判断数据是否到达，Q接口接165的输入端，音频口直接接手机，如此可以远程控制了，理论上讲距离时无限的，只要电话能够接通。
现在所有的接口（除了01串口）都用上了，2、3、4用来控制595；5、6、12用来控制165；7用来控制LCD背光；8、9、10、11、13用来控制LCD；A0接五维按键；15、16（A1、A2）连接超声波；17（A3）接STD；A4、A5是I2C接口。至于串口，以后有时间加个蓝牙吧，可能会内存不足，现在已经占用了76%的内存（1516字节），程序大小19546字节（占用60％）。

#include <Wire.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include "U8glib.h"
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
//
#define remotepin 17
//超声波模块端口
#define trpin 16
#define ecpin 15
//74HC165 端口
#define q7pin 12
#define plpin 6
#define cppin 5
//74HC595 端口
#define dspin 4
#define stpin 3
#define shpin 2
//前置巡线数据－> 74HC165 数据位
#define IRL 7
#define IRR 6
#define IRM 4
//前置避障数据－> 74HC165 数据位
#define BZL 4
#define BZR 5
//motor number 马达编号
#define LF 1
#define RF 4
#define LB 2
#define RB 3
//RGB PWM编号
#define GPIN 0
#define RPIN 1
#define GPIN 14
#define BPIN 15
#define LEFT 0x40 // 2 -> D6 -> 0x40
#define RIGHT 0x02 // 4 -> D1 -> 0x02
#define BRIGHT 0x10 // D4 -> 0x10
#define BRAKEL 0x01 // D0 -> 0x01
#define STOP 0x00 //
#define WARNING 0x43 //2+4 -> D6+D1+D0 -> 0x43
#define DEBUG 0x53 //2+4 -> D6+D1+D0+D4 -> 0x53
#define NORMAL 0x00
#define KEYIN A0 //5-key 五维导航按键读取
#define KEY_NONE 0
#define KEY_PREV 1
#define KEY_NEXT 2
#define KEY_SELECT 3
#define KEY_BACK 4
//
Adafruit_MotorShield Motor = Adafruit_MotorShield();
Adafruit_DCMotor *Motor_LF = Motor.getMotor(LF);
Adafruit_DCMotor *Motor_RF = Motor.getMotor(RF);
Adafruit_DCMotor *Motor_LB = Motor.getMotor(LB);
Adafruit_DCMotor *Motor_RB = Motor.getMotor(RB);
/////////////////////////////////////////////////////////////////////////////////////////////////////
U8GLIB_NHD_C12864 u8g(13, 11, 10, 9, 8); // SPI Com: SCK = 13, MOSI = 11, CS = 10, A0 = 9, RST = 8
static char *menu_strings[5] = //模式选择菜单［避障模式、巡线模式、遥控模式、系统测试、设置］
{
"Avoida mode", "Patrol mode", "Remote mode", "System Mode" ,"Setup Mode"
};
static char *linght_testing[7] = //灯光测试显示字符串
{
"Front light testing","Brake light testing","Left light testing",
"Right light testing","Waring signal testing","Debug signal testing ",
" Normal Status"
};
static char *motor_testing[7] = //马达测试显示字符串
{
"Forward testing","Backward testing",
" Left testing"," Right testing",
"Turn Left testing ","Turn Right testing ",
" Stop ! "
};
static char *GRB_testing[7] = // RGB测试显示字符串
{
"Red testing","Green testing","Blue testing",
"Yellow testing"," Pink testing","Cyan testing",
"Whihe testing"
};
static char *runstauts[9] = //小车运行状态显示字符串
{
" Forward "," Backward ",
" Left "," Right ",
" Turn Left ","Turn Right",
" Stop "," Accel ",
" Brake "
};
static char light[7] = {'F','B','L','R','W','D','N'};
static char motor[7] = {'F','B','L','R','l','r','S'};
static int rgb[7][3] = //RGB 测试时的显示序列，红绿蓝黄粉青白
{
{4096,0,0},{0,4096,0},{0,0,4096},
{4096,4096,0},{4096,0,4096},
{0,4096,4096},{4096,4096,4096}
};
uint8_t uiKeyCodeFirst = KEY_NONE; //按键控制
uint8_t uiKeyCodeSecond = KEY_NONE;
uint8_t uiKeyCode = KEY_NONE;
uint8_t menu_current = 0;
uint8_t menu_redraw_required = 0;
uint8_t last_key_code = KEY_NONE;
int keyvalue ;
int key = -1 ;
int oldkey = -1;
/////////////////////////////////////////////////////////////////////////////////////////
int ms = 5 ; //
int distance,motorspeed;
int current_speed;
char dir_flag,ctrl;
int runstaut;
byte dd;
void setup()
{
u8g.setRot180(); // 屏幕旋转180度
uiStep();
menu_redraw_required = 2 ;
init_595(); //初始化
init_ultra();
Motor.begin(); //马达初始化
motorspeed = 0;
speedCtrl( 100 );
motorCtrl('S');
sysInitialize();
distance = distanceRead();
current_speed = 0;
randomSeed(analogRead(A2)*analogRead(A1));
Serial.begin(9600);
}
void loop()
{
//GRBTest();
sysStar();
if(uiKeyCodeFirst == KEY_SELECT)
{
switch (menu_current)
{
case 0 : avoidanceMode();break;
case 1 : patrolMode();break;
case 2 : remoteMode();break;
case 3 : systemMode();break;
case 4 : setupMode();break;
}
}
}
///////////////////////////////////////////////////////////////////////////////
void sysStar() //模式菜单显示
{
uiStep(); // 按键检查
if ( menu_redraw_required != 0 )
{
u8g.firstPage();
do
{
drawMenu();
}
while( u8g.nextPage() );
menu_redraw_required = 0;
}
updateMenu(); // 更新菜单
}
int keyGetvalue( int value) //按键值
{
if(value < 100 ) return 0 ; //左键
else if (value < 300 ) return 1 ; //中键
else if (value < 500 ) return 2 ; //下键
else if (value < 700 ) return 3 ; //右键
else if (value < 900 ) return 4 ; //上键
else return -1 ;
}
void uiStep(void) //按键设置
{
keyvalue = analogRead(KEYIN);
key = keyGetvalue(keyvalue);
if (key != oldkey)
{
delay(10);
keyvalue = analogRead(KEYIN);
key = keyGetvalue(keyvalue);
if(key != oldkey)
{
oldkey = key;
switch (key)
{
case 0 : uiKeyCodeFirst = KEY_BACK; break ;
case 1 : uiKeyCodeFirst = KEY_SELECT; break ;
case 2 : uiKeyCodeFirst = KEY_NEXT; break ;
case 3 : uiKeyCodeFirst = KEY_NONE; break ;
case 4 : uiKeyCodeFirst = KEY_PREV; break ;
default : uiKeyCode = uiKeyCodeFirst; break ;
}
}
}
delay(20);
}
void drawMenu(void) //绘制菜单
{
uint8_t i, h;
u8g_uint_t w, d;
u8g.setFont(u8g_font_6x13);
u8g.setFontRefHeightText();
u8g.setFontPosTop();
h = u8g.getFontAscent()-u8g.getFontDescent();
w = u8g.getWidth();
for( i = 0; i < 5; i++ )
{
d = (w-u8g.getStrWidth(menu_strings[i]))/2;
u8g.setDefaultForegroundColor();
if ( i == menu_current )
{
u8g.drawBox(0, i*h+1, w, h);
u8g.setDefaultBackgroundColor();
}
u8g.drawStr(d, i*h, menu_strings[i]);
}
}
void updateMenu(void) //更新菜单显示
{
switch ( uiKeyCode )
{
case KEY_NEXT :
menu_current++;
if ( menu_current >= 5 )
menu_current = 0;
menu_redraw_required = 1;
break;
case KEY_PREV :
if ( menu_current == 0 )
menu_current = 5;
menu_current--;
menu_redraw_required = 1;
break;
}
uiKeyCode = KEY_NONE ;
}
void menuLight() //选择条高亮（加黑）
{
uint8_t i, h;
u8g_uint_t w, d;
u8g.setFont(u8g_font_6x13);
u8g.setFontRefHeightText();
u8g.setFontPosTop();
h = u8g.getFontAscent()-u8g.getFontDescent();
w = u8g.getWidth();
for( i = 0; i < 5; i++ )
{
d = (w-u8g.getStrWidth(menu_strings[i]))/2;
u8g.setDefaultForegroundColor();
if ( i == menu_current )
{
u8g.drawBox(0, i*h+1, w, h);
u8g.setDefaultBackgroundColor();
}
u8g.drawStr(d, i*h, menu_strings[i]);
}
}
void screenSet() //屏幕模式获取
{
if ( u8g.getMode() == U8G_MODE_R3G3B2 )
{
u8g.setColorIndex(255); // white
}
else if ( u8g.getMode() == U8G_MODE_GRAY2BIT )
{
u8g.setColorIndex(3); // max intensity
}
else if ( u8g.getMode() == U8G_MODE_BW )
{
u8g.setColorIndex(1); // pixel on
}
else if ( u8g.getMode() == U8G_MODE_HICOLOR )
{
u8g.setHiColorByRGB(255,255,255);
}
}
////////////////////////////////////////////////////////////////////////////
void avoidanceMode() //避障模式
{
distance = distanceRead();
speedCtrl(motorspeed);
car_speed_dirCtrl(distance);
///////////////////////////////////////////////////////////
screenSet();
u8g.firstPage();
do
{
u8g.setFont(u8g_font_6x13);
u8g.drawStr( 10, 15, "*** Run Stauts ***");
u8g.drawStr( 35, 35, runstauts[runstaut]);
u8g.drawStr( 10, 60, "*******************");
} while(u8g.nextPage());
//////////////////////////////////////////////////////////
}
void patrolMode() //巡线模式
{
car_dirCtrl();
//////////////////////////////////////////////////////////
screenSet();
u8g.firstPage();
do
{
u8g.setFont(u8g_font_6x13);
u8g.drawStr( 10, 15, "*** Run Stauts ***");
u8g.drawStr( 35, 35, runstauts[runstaut]);
u8g.drawStr( 10, 60, "*******************");
} while(u8g.nextPage());
///////////////////////////////////////////////////////////
}
void remoteMode() //遥控模式
{
byte value;
if ( digitalRead(remotepin) )
{
value = read_165();
value = value & 0x0f;
switch (value)
{
case 1 : ctrl = 'l'; runstaut = 4; break; // 1
case 2 : ctrl = 'F'; runstaut = 0; break;
case 3 : ctrl = 'r'; runstaut = 5; break;
case 4 : ctrl = 'L'; runstaut = 2; break;
case 5 : ctrl = 'S'; runstaut = 6; break;
case 6 : ctrl = 'R'; runstaut = 3; break;
//case 7 : ctrl = 'F'; runstaut = 4; break;
case 8 : ctrl = 'B'; runstaut = 4; break;
//case 9 : ctrl = 'F'; runstaut = 4; break;
//case 10 : ctrl = 'F' runstaut = 4; ;break; // 0
//case 11 : ctrl = 'F'; runstaut = 4; break; // *
//case 12 : ctrl = 'F'; runstaut = 4; break; // #
//case 13 : ctrl = 'F'; runstaut = 4; break; // A
//case 14 : ctrl = 'F'; runstaut = 4; break; // B
//case 15 : ctrl = 'F'; runstaut = 4; break; // C
//case 0 : ctrl = 'D'; runstaut = 4; break; // D
}
}
screenSet();
u8g.firstPage();
do
{
u8g.setFont(u8g_font_6x13);
u8g.drawStr( 10, 15, "*** Run Stauts ***");
u8g.drawStr( 35, 35, runstauts[runstaut]);
u8g.drawStr( 10, 60, "*******************");
} while(u8g.nextPage());
lightCtrl(ctrl);
motorCtrl(ctrl);
Serial.println(value,HEX);
}
void systemMode()
{
}
void setupMode()
{
}
//////////////////////////////////////////////////////////////////////////////////////
void sysInitialize() //系统初始化显示信息，包括功能测试
{
screenSet();
u8g.setFont(u8g_font_6x13);
u8g.firstPage();
do
{
u8g.drawStr( 0, 35, "System is initializing ...");
delay(100);
} while(u8g.nextPage());
u8g.firstPage();
do
{
u8g.drawStr( 0, 35, "Light system testing");
delay(200);
} while(u8g.nextPage());
for ( int i = 0 ; i < 7; i ++)
{
u8g.firstPage();
do
{
u8g.drawStr( 5, 35, linght_testing[i]);
delay(10);
} while(u8g.nextPage());
lightCtrl(light[i]);
delay(3000);
}
u8g.firstPage();
do
{
u8g.drawStr( 0, 35, "Motor system testing");
delay(200);
} while(u8g.nextPage());
for ( int i = 0 ; i < 7; i ++)
{
u8g.firstPage();
do
{
u8g.drawStr( 20, 35, motor_testing[i]);
delay(10);
} while(u8g.nextPage());
motorCtrl(motor[i]);
delay(1000);
}
u8g.firstPage();
do
{
u8g.drawStr( 10, 35, "RGB LED testing");
delay(100);
} while(u8g.nextPage());
for ( int i = 0 ; i < 7; i ++)
{
u8g.firstPage();
do
{
u8g.drawStr( 20, 35, GRB_testing[i]);
delay(10);
} while(u8g.nextPage());
RGBctrl(rgb[i][0],rgb[i][1],rgb[i][2]);
delay(1000);
}
RGBctrl(0,0,0);
u8g.firstPage();
do
{
u8g.drawStr( 0, 35, "Initializtion Finished");
delay(1000);
} while(u8g.nextPage());
}
////////////////////
void RGBctrl(int r,int g,int b) ///
{
Motor.setPWM(RPIN,r);
Motor.setPWM(GPIN,g);
Motor.setPWM(BPIN,b);
}
void GRBTest()
{
int i ;
i = random();
if ( !(i % 17) ) RGBctrl(random(1024, 4095),0,0 );
if ( !(i % 37) ) RGBctrl(0,random(1024, 4095),0 );
if ( !(i % 47) ) RGBctrl(0,0,random(1024, 4095) );
if ( !(i % 57) ) RGBctrl(0,random(1024, 4095),random(1024, 4095) );
if ( !(i % 67) ) RGBctrl(random(1024, 4095),random(1024, 4095),0 );
if ( !(i % 87) ) RGBctrl(random(1024, 4095),0,random(1024, 4095) );
if ( !(i % 97) ) RGBctrl(random(1024, 4095),random(1024, 4095),random(1024, 4095) );
}
void lightTest()
{
int value;
value = analogRead(KEYIN);
if(value < 100 ) lightCtrl('L') ;
else if (value < 300 ) lightCtrl('W') ;
else if (value < 500 ) lightCtrl('B') ;
else if (value < 700 ) lightCtrl('R') ;
else if (value < 900 ) lightCtrl('F') ;
}
void MotorTest()
{
int value;
value = analogRead(KEYIN);
if(value < 100 ) motorCtrl('L') ;
else if (value < 300 ) motorCtrl('S') ;
else if (value < 500 ) motorCtrl('B') ;
else if (value < 700 ) motorCtrl('R') ;
else if (value < 900 ) motorCtrl('F') ;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void speedCtrl(byte speed)
{
Motor_LF->setSpeed(speed);
Motor_RF->setSpeed(speed);
Motor_LB->setSpeed(speed);
Motor_RB->setSpeed(speed);
}
//
void motorCtrl(byte status)
{
switch(status)
{
case 'F' :
Motor_LF->run(FORWARD);
Motor_RF->run(FORWARD);
Motor_LB->run(FORWARD);
Motor_RB->run(FORWARD);
break;
case 'B' :
Motor_LF->run(BACKWARD);
Motor_RF->run(BACKWARD);
Motor_LB->run(BACKWARD);
Motor_RB->run(BACKWARD);
break;
case 'L' :
Motor_LF->run(RELEASE);
Motor_RF->run(FORWARD);
Motor_LB->run(RELEASE);
Motor_RB->run(FORWARD);
break;
case 'R' :
Motor_LF->run(FORWARD);
Motor_RF->run(RELEASE);
Motor_LB->run(FORWARD);
Motor_RB->run(RELEASE);
break;
case 'S' :
Motor_LF->run(RELEASE);
Motor_RF->run(RELEASE);
Motor_LB->run(RELEASE);
Motor_RB->run(RELEASE);
break;
case 'l' :
Motor_LF->run(BACKWARD);
Motor_RF->run(FORWARD);
Motor_LB->run(BACKWARD);
Motor_RB->run(FORWARD);
break;
case 'r' :
Motor_LF->run(FORWARD);
Motor_RF->run(BACKWARD);
Motor_LB->run(FORWARD);
Motor_RB->run(BACKWARD);
break;
}
}
void init_ultra()
{
pinMode(trpin,OUTPUT);
pinMode(ecpin,INPUT);
}
int distanceRead()
{
float distance;
digitalWrite(trpin, LOW); // 发射低电压2μs
delayMicroseconds(2);
digitalWrite(trpin, HIGH); // 发射高电压并维持10μs
delayMicroseconds(10);
digitalWrite(trpin, LOW); // 恢复低压发射
distance = pulseIn(ecpin, HIGH); // 读取返回时间差
distance = distance/5.8/10;
return (int) distance;
}
char dirSet(char mode) //方向确定，a为避障模式，红外避障传感器作用，p为巡线模式，3个巡线传感器作用
{
char flag;
byte d = read_165();
if ( mode == 'a' ) /////需要确认有效电平是高还是低
{
if ( !(bitRead(d,BZR)) && (bitRead(d,BZR)) ) flag = 'L' ;
if ( (bitRead(d,BZR)) && !(bitRead(d,BZL)) ) flag = 'R' ;
if ( !(bitRead(d,BZR)) && !(bitRead(d,BZR)) ) flag = 'S' ;
//if ( !(bitRead(d,BZR)) && !(bitRead(d,BZL ) ) flag = 'B' ;
}
if (mode == 'p') /////需要确认有效电平是高还是低
{
if ( !(bitRead(d,IRL)) && !(bitRead(d,IRM)) && !(bitRead(d,IRR)) ) flag = 'F' ;
if ( (bitRead(d,IRL)) && !(bitRead(d,IRM)) && !(bitRead(d,IRR)) ) flag = 'R' ;
if ( !(bitRead(d,IRL)) && !(bitRead(d,IRM)) && (bitRead(d,IRR)) ) flag = 'L' ;
if ( (bitRead(d,IRL)) && (bitRead(d,IRM)) && (bitRead(d,IRR)) ) flag = 'S' ;
}
return flag;
}
void car_speed_dirCtrl(int distance) //避障模式方向与速度控制，需要根据前方距离确定运行状态
{
if (distance >= 1000)
{
if(current_speed < 255 )
{
motorspeed += 30 ;
if (motorspeed > 255 ) motorspeed = 255;
current_speed = motorspeed;
}
lightCtrl('F');
motorCtrl('F');
runstaut = 7 ;
}
else if (distance < 1000 && distance >= 500)
{
if(current_speed > 200)
{
current_speed = motorspeed;
}
else if (current_speed < 50 )
{
motorspeed += 30 ;
if (motorspeed > 200 ) motorspeed = 200;
current_speed = motorspeed;
}
else
{
motorspeed += 20 ;
if (motorspeed > 200 ) motorspeed = 200;
current_speed = motorspeed;
}
lightCtrl('F');
motorCtrl('F');
runstaut = 7 ;
}
else if (distance < 500 && distance >= 100)
{
if(current_speed > 180)
{
current_speed = motorspeed;
}
else if (current_speed < 50 )
{
motorspeed += 20 ;
if (motorspeed > 180 ) motorspeed = 180;
current_speed = motorspeed;
}
else
{
motorspeed += 5 ;
if (motorspeed > 180 ) motorspeed = 180;
current_speed = motorspeed;
}
lightCtrl('F');
motorCtrl('F');
runstaut = 7 ;
}
else if (distance < 100 && distance >= 80)
{
if(current_speed > 200)
{
motorspeed -= 20 ;
if (motorspeed < 100 ) motorspeed = 100;
current_speed = motorspeed;
}
lightCtrl('B');
motorCtrl('F');
runstaut = 8 ;
}
else if (distance < 80 && distance >= 50)
{
if(current_speed > 200)
{
motorspeed -= 20 ;
if (motorspeed < 80 ) motorspeed = 80;
current_speed = motorspeed;
}
else if (current_speed > 150)
{
motorspeed -= 10 ;
if (motorspeed < 80 ) motorspeed = 80;
current_speed = motorspeed;
}
motorCtrl('F');
lightCtrl('B');
runstaut = 8 ;
}
else if (distance < 50 && distance >= 30)
{
if(current_speed > 200)
{
motorspeed -= 50 ;
if (motorspeed < 50 ) motorspeed = 50;
current_speed = motorspeed;
}
else if (current_speed > 150)
{
motorspeed -= 40 ;
if (motorspeed < 50 ) motorspeed = 50;
current_speed = motorspeed;
}
else if (current_speed > 100)
{
motorspeed -= 40 ;
if (motorspeed < 10 ) motorspeed = 10;
current_speed = motorspeed;
}
dir_flag = dirSet('a');
if (dir_flag == 'L') runstaut = 4 ;
if (dir_flag == 'R') runstaut = 5 ;
if (dir_flag == 'S') runstaut = 6 ;
if (dir_flag == 'B') runstaut = 1 ;
lightCtrl('B');
lightCtrl(dir_flag);
motorCtrl(dir_flag);
}
else if (distance < 30 && distance >= 15)
{
if(current_speed > 200)
{
motorspeed -= 80 ;
if (motorspeed < 0 ) motorspeed = 0;
current_speed = motorspeed;
}
else if (current_speed > 150)
{
motorspeed -= 60 ;
if (motorspeed < 0 ) motorspeed = 0;
current_speed = motorspeed;
}
else if (current_speed > 100)
{
motorspeed -= 40 ;
if (motorspeed < 0 ) motorspeed = 0;
current_speed = motorspeed;
}
lightCtrl('B');
dir_flag = dirSet('a');
if (dir_flag == 'L') runstaut = 4 ;
if (dir_flag == 'R') runstaut = 5 ;
if (dir_flag == 'S') runstaut = 6 ;
if (dir_flag == 'B') runstaut = 1 ;
dir_flag += 32;
lightCtrl(dir_flag);
motorCtrl(dir_flag);
}
else if (distance < 15 && distance > 10)
{
motorspeed = 0 ;
motorCtrl('S');
lightCtrl('W');
motorspeed = 0 ;
runstaut = 6 ;
}
}
void car_dirCtrl() // 巡线模式方向控制，小车速度恒定，方向确认
{
dir_flag = dirSet('p');
if (dir_flag == 'F') runstaut = 0 ;
if (dir_flag == 'L') runstaut = 2 ;
if (dir_flag == 'R') runstaut = 3 ;
if (dir_flag == 'S') runstaut = 6 ;
lightCtrl(dir_flag);
motorCtrl(dir_flag);
}
//////////////////////////////////////////////////////////////////////////////////
void init_595()
{
pinMode(stpin,OUTPUT);
pinMode(shpin,OUTPUT);
pinMode(dspin,OUTPUT);
}
void write_595(byte data)
{
digitalWrite(stpin,LOW); //将ST_CP口上面加低电平让芯片准备好接收数据
shiftOut(dspin,shpin,MSBFIRST,data);
digitalWrite(stpin,HIGH); //将ST_CP这个针脚恢复到高电平
}
void lightCtrl(byte status)
{
switch (status)
{
case 'L' : write_595(LEFT ); break;
case 'R' : write_595(RIGHT ); break;
case 'F' : write_595(BRIGHT ); break;
case 'B' : write_595(BRAKEL ); break;
case 'S' : write_595(STOP ); break;
case 'W' : write_595(WARNING); break;
case 'D' : write_595(DEBUG ); break;
default : write_595(NORMAL ); break;
}
}
////////////////////////////////////////////////////////////////////////////
void init_165()
{
pinMode(cppin,OUTPUT);
pinMode(plpin,OUTPUT);
pinMode(q7pin,INPUT);
}
byte read_165()
{
byte temp,data;
digitalWrite(plpin, LOW);
delayMicroseconds(ms);
digitalWrite(plpin, HIGH);
delayMicroseconds(ms);
data = 0 ;
for(int i = 0 ; i < 8 ; i++)
{
data = data << 1;
digitalWrite(cppin, LOW);
delayMicroseconds(ms);
temp = digitalRead(q7pin);
data |= temp;
digitalWrite(cppin, HIGH);
delayMicroseconds(ms);
}
return data ;
}

复制代码

zoologist · 发表于 2015-5-31 19:50:28

放个照片吧~

whtech · 发表于 2015-5-31 19:51:50

不错,赞一个

heiketiguo · 发表于 2015-5-31 21:39:20

看着程序够牛，赞个{:soso_e179:}

萧芸凤 · 发表于 2015-6-5 17:39:20

zoologist 发表于 2015-5-31 19:50
放个照片吧~

不知道为什么，没办法上传图片，JPEG 320K也不行

帐号		自动登录	找回密码
密码			注册