_SKU_DFR0302_MiniQ_2WD_Plus - jimaobian/DFRobotWikiCn GitHub Wiki
miniQ 2WD Plus是一款基于Atmega32u4单片机设计的机器人小车控制板，完全兼容Arduino Leonardo以及各种sheild扩展板接口。
小伙伴们可以在上面搭建各种传感器模块，更难得的是，模块还留有很多焊接口，再也不用自己去买万能板了！
miniQ 2WD Plus即可以作为桌面机器人小车的主控制器，搭建2轮自平衡机器人，也可以做为miniQ 2WD的端口扩展板，通过板子之间的Gadgeteer接口进行通信，怎么样？神奇吧！！！
- 主控芯片: Atmega 32u4
- 加速度计芯片：ADXL345
- 陀螺仪芯片： ITG3205
- 数字IO口:23（其中D17是RX指示灯）
- 5V数字/模拟口最大允许电流:40 mA
- 兼容标准Arduino接口
- 支持USB程序下载
- 7路PWM通道
- 具有1个xbee接口（Serial1）
- 具有1个单总线RGB灯
- 具有1个9g舵机安装孔
- 具有万用板功能
- 可直接安装到MiniQ 2WD和MiniQ 桌面机器人底盘上
- 电源：使用USB供电或电源供电。
- RGB灯珠：内置驱动IC-WS2811，采用单线式数据线控制
- 模拟输入接口：完全兼容Leonardo
- 数字接口：完全兼容Leonardo
- 复位按键：采用微动独立按键，手感更舒适
- 2个Gadgteer接口：用于连接各种Gadgteer接口的传感器，也可以用于连接miniQ 2WD进行通信控制
- 加速度计芯片：采用ADXL345
- 陀螺仪芯片： 采用ITG3205
- Xbee接口： 用于无线数据传输
- 预留TowerPro SG90舵机安装孔
- 预留UART接口
首先我们先单独学习上层板的应用。
MiniQ 2WD Plus集成加速度计，陀螺仪功能。加速度计&陀螺仪数据的读取是通过IIC获取的，输出的数据是相对于X,Y,Z轴的，如果想要获取相对于坐标系的角度值，则要进行换算。当然，由于加速度计过于灵敏，相信很多小伙伴也遇到这样的困难，模块一点儿小抖动，数据的拨动幅度就很大，这里用一个简单的例子，用陀螺仪的数据进行角度修正，并采用软件滤波器滤波，得到相对稳定的角度数据。实验结果可以通过串口助手观察到。左侧数据代表通过加速度计计算得出的角度，右侧为结合加速度计和陀螺仪最终得出的小车倾斜角度，为了尽量避免负数，我们假定水平放置的MiniQ 2WD Plus上层板角度为90°，使得得到的数据范围在0~180°之间。当然，你也可以修改显示的数据，以及角度范围。
/***************************************************
MiniQ 2WD plus (With Stainless Steel Probe)
<http://www.dfrobot.com.cn/goods-1074.html>
***************************************************
This example show how to use ADXL345 and ITG3205 sensor.
Created 2016-1-15
By Andy zhou <[email protected]>
version:V1.0
GNU Lesser General Public License.
See <http://www.gnu.org/licenses/> for details.
All above must be included in any redistribution
****************************************************/
/***********Notice and Trouble shooting***************
1.Connection and Diagram can be found here
<http://wiki.dfrobot.com.cn/index.php?title=(SKU:DFR0302)MiniQ_2WD_Plus>
2.This code is tested on Arduino Uno, Leonardo, Mega boards.
****************************************************/
#include <Wire.h>
#include <math.h>
#define DEVICE (0x53) //ADXL345 device address
#define TO_READ (6) //num of bytes we are going to read each time (two bytes for each axis)
//ITG3205
#define ITGAddress 0x68 //ITG3205的I2C地址（AD0->gnd）
#define G_SMPLRT_DIV 0x15 //设置采样率的寄存器
#define G_DLPF_FS 0x16 //设置量程、低通滤波带宽、时钟频率的寄存器
#define G_INT_CFG 0x17 //设置中断的寄存器
#define G_PWR_MGM 0x3E //设置电源管理的寄存器
float xGyro, yGyro, zGyro; //存放角速度值,温度
int buff[6]; //存放寄存器高低位值，X、Y、Z轴共6个
byte buff1[6];
// 陀螺仪传感器误差修正的偏移量
int g_offx = -35;
int g_offy = -9;
int g_offz = -30;
//******卡尔曼参数************
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_gy; //由角速度计算的倾斜角度
float Accel_x; //X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
//uchar value; //角度正负极性标记
float Q_angle=0.001;
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.01; //dt为kalman滤波器采样时间;
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
void writeRegister(int deviceAddress, byte address, byte val){
Wire.beginTransmission(deviceAddress);
Wire.write(address);
Wire.write(val);
Wire.endTransmission();
}
void readRegister(int deviceAddress, byte address) {
Wire.beginTransmission(deviceAddress);
Wire.write(address);
Wire.endTransmission();
Wire.beginTransmission(deviceAddress);
Wire.requestFrom(deviceAddress, 6);
int i = 0;
while(Wire.available())
{ buff[i++] = Wire.read(); }
Wire.endTransmission();
}
/*****************************************
* ITG3205
* G_SMPLRT_DIV：采样率 = 125Hz
* G_DLPF_FS：+ - 2000度/秒、低通滤波器5HZ、内部采样率1kHz
* G_INT_CFG：没有中断
* G_PWR_MGM：电源管理设定：无复位、无睡眠模式、无待机模式、内部振荡器
******************************************/
void initGyro(){
writeRegister(ITGAddress, G_SMPLRT_DIV, 0x07); //设置采样率
writeRegister(ITGAddress, G_DLPF_FS, 0x1E); //设置量程、低通滤波带宽、内部采样率
writeRegister(ITGAddress, G_INT_CFG, 0x00); //设置中断（默认值）
writeRegister(ITGAddress, G_PWR_MGM, 0x00); //设置电源管理（默认值）
//Turning on the ADXL345
writeTo(DEVICE, 0x2D, 0);
writeTo(DEVICE, 0x2D, 16);
writeTo(DEVICE, 0x2D, 8);
}
float getGyroValues(){
readRegister(ITGAddress, 0x1D); //读取陀螺仪ITG3205的数据
xGyro = ((buff[0] << 8) | buff[1]) + g_offx;
yGyro = ((buff[2] << 8) | buff[3]) + g_offy;
zGyro = ((buff[4] << 8) | buff[5]) + g_offz;
return xGyro;
}
float getadxvalues(){
int regAddress = 0x32; //first axis-acceleration-data register on the ADXL345
int x, y, z;
readFrom(DEVICE, regAddress, TO_READ, buff1); //read the acceleration data from the ADXL345
//each axis reading comes in 10 bit resolution, ie 2 bytes. Least Significat Byte first!!
//thus we are converting both bytes in to one int
x = (((int)buff1[1]) << 8) | buff1[0];
y = (((int)buff1[3])<< 8) | buff1[2];
z = (((int)buff1[5]) << 8) | buff1[4];
float Rx = x * 0.0039 + 0.035;
float Ry = y * 0.0039 + 0.035;
float Rz = z * 0.0039 + 0.040;
// Rx = abs(Rx);
float R = sqrt( Rx*Rx + Ry*Ry + Rz*Rz );
float Axr = acos(Rx/R) * 57.2958;
return Axr;
}
float kalmanUpdate(float Accel,float Gyro){
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
void setup(){
Serial.begin(9600);
Wire.begin();
initGyro();
delay(50);
}
void loop(){
float Accel_x = getadxvalues();
float Gyro_x = getGyroValues()/14.375;
kalmanUpdate( Accel_x, Gyro_x );
Angle = Angle + (((Accel_x-Angle)*0.5 + Gyro_y)*0.001);
Serial.print(Accel_x);
Serial.print(" ");
Serial.println(Angle);
}
//---------------- Functions
//Writes val to address register on device
void writeTo(int device, byte address, byte val) {
Wire.beginTransmission(device); //start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); //end transmission
}
//reads num bytes starting from address register on device in to buff array
void readFrom(int device, byte address, int num, byte buff[]){
Wire.beginTransmission(device); //start transmission to device
Wire.write(address); //sends address to read from
Wire.endTransmission(); //end transmission
Wire.beginTransmission(device); //start transmission to device
Wire.requestFrom(device, num); // request 6 bytes from device
int i = 0;
while(Wire.available()){ //device may send less than requested (abnormal)
buff[i] = Wire.read(); // receive a byte
i++;
}
Wire.endTransmission(); //end transmission
}
MiniQ 2WD plus 内置一个RGB灯珠WS2812B-4，其内置驱动IC-WS2811，采用单线通信方式控制。
样例展示灯珠多种颜色的变换、控制灯珠闪烁。实际上每个像素点都可显示三基色（RGB)，每种颜色更可实现256级亮度显示，完成16777216种颜色的全真色彩显示，你可以在代码中修改三基色的数值来改变灯光色彩。
/***************************************************
MiniQ 2WD plus (With Stainless Steel Probe)
<http://www.dfrobot.com.cn/goods-1074.html>
***************************************************
This example show how to control RGB LED.
Created 2016-1-15
By Andy zhou <[email protected]>
version:V1.0
GNU Lesser General Public License.
See <http://www.gnu.org/licenses/> for details.
All above must be included in any redistribution
****************************************************/
/***********Notice and Trouble shooting***************
1.Connection and Diagram can be found here
<http://wiki.dfrobot.com.cn/index.php?title=(SKU:DFR0302)MiniQ_2WD_Plus>
2.This code is tested on Arduino Uno, Leonardo, Mega boards.
****************************************************/
#include <Adafruit_NeoPixel.h>
#define PIN 4
Adafruit_NeoPixel strip = Adafruit_NeoPixel(150, PIN, NEO_GRB + NEO_KHZ800);
void setup() {
strip.begin();
strip.show(); // Initialize all pixels to 'off'
}
void loop() {
// Some example procedures showing how to display to the pixels:
colorWipe(strip.Color(255, 0, 0), 50); // Red
colorWipe(strip.Color(0, 255, 0), 50); // Green
colorWipe(strip.Color(0, 0, 255), 50); // Blue
// Send a theater pixel chase in...
theaterChase(strip.Color(127, 127, 127), 50); // White
theaterChase(strip.Color(127, 0, 0), 50); // Red
theaterChase(strip.Color( 0, 0, 127), 50); // Blue
rainbow(20);
// rainbowCycle(20);
// theaterChaseRainbow(50);
}
// Fill the dots one after the other with a color
void colorWipe(uint32_t c, uint8_t wait) {
for(uint16_t i=0; i<strip.numPixels(); i++) {
strip.setPixelColor(i, c);
strip.show();
delay(wait);
}
}
void rainbow(uint8_t wait) {
uint16_t i, j;
for(j=0; j<256; j++) {
for(i=0; i<strip.numPixels(); i++){
strip.setPixelColor(i, Wheel((i+j) & 255));
}
strip.show();
delay(wait);
}
}
// Slightly different, this makes the rainbow equally distributed throughout
void rainbowCycle(uint8_t wait) {
uint16_t i, j;
for(j=0; j<256*5; j++) { // 5 cycles of all colors on wheel
for(i=0; i< strip.numPixels(); i++) {
strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255));
}
strip.show();
delay(wait);
}
}
//Theatre-style crawling lights.
void theaterChase(uint32_t c, uint8_t wait) {
for (int j=0; j<10; j++) { //do 10 cycles of chasing
for (int q=0; q < 3; q++) {
for (int i=0; i < strip.numPixels(); i=i+3) {
strip.setPixelColor(i+q, c); //turn every third pixel on
}
strip.show();
delay(wait);
for (int i=0; i < strip.numPixels(); i=i+3) {
strip.setPixelColor(i+q, 0); //turn every third pixel off
}
}
}
}
//Theatre-style crawling lights with rainbow effect
void theaterChaseRainbow(uint8_t wait) {
for (int j=0; j < 256; j++) { // cycle all 256 colors in the wheel
for (int q=0; q < 3; q++) {
for (int i=0; i < strip.numPixels(); i=i+3) {
strip.setPixelColor(i+q, Wheel( (i+j) % 255)); //turn every third pixel on
}
strip.show();
delay(wait);
for (int i=0; i < strip.numPixels(); i=i+3) {
strip.setPixelColor(i+q, 0); //turn every third pixel off
}
}
}
}
// Input a value 0 to 255 to get a color value.
// The colours are a transition r - g - b - back to r.
uint32_t Wheel(byte WheelPos) {
if(WheelPos < 85){
return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
} else if(WheelPos < 170) {
WheelPos -= 85;
return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
} else {
WheelPos -= 170;
return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
}
}
MiniQ 2WD与PLUS上层板是天生的一对。MiniQ 2WD PLUS 和 MiniQ 2WD采用IIC通信，通过Gadgteer连接上下板。MiniQ 2WD PLUS作为IIC主机，MiniQ 2WD作为IIC从机，MiniQ 2WD PLUS通过无线接收命令数据，然后解析，通过IIC发送命令MiniQ 2WD小车底板，作出相应的动作命令。
这里特别说明一下在“运动功能”中，有“巡线模式”、“寻光模式”、“壁障模式”、“遥控模式”，其中的巡线模式中的传感器参数是可以调节的，也就是说，小车可以根据每次使用的巡线场景不同而进行实地调节的。具体操作如下，在进入到巡线模式后，小车并没有直接进入巡线，而是要先进行调节，调节按钮采用底层小车前端的KEY1、KEY2、KEY3三个按钮控制，按下KEY1，对第一个巡线传感器调节（从小车的左侧往右数，第一个巡线传感器），然后KEY2、KEY3是对传感器的值进行调节，不断按下KEY2，直到RGB灯变为红色，（如果接着按KEY3，RGB灯又会变为绿色）按下KEY1确定继续调节第二个。注意：中间那个传感器变为红色后，按下KEY3让它变为绿色，因为我们中间要用来巡黑线的。就这样一次调节完五个传感器。**当然，我们在调节的时候，是将小车放在线上的。**当五个传感器调节完成后，继续按下KEY1键，小车退出调节模式，而进入正常的巡线模式，如果想再次调节，则再次按下KEY1键，依照前面说的进行调节。遥控模式下使用的遥控器是MiniQ 2WD附带的红外遥控器，使用时“开关”、“VOL+”、“VOL—”、“上一曲”，“下一曲”键分别代表“停止”、“前进”、“后退”、“左转”、“右转”。
下面是样例代码，整个例程还需要直插“LCD12864显示屏扩展板”作为显示器，并使用该显示器扩展版上的集成摇杆进行操作。该样例展示了MiniQ 2WD常用功能：蜂鸣、灯光、巡线、寻光、红外壁障，遥控功能。通过该样例可以更 直观的感受产品的强大以及学习产品的使用方法
注意：程序分两部分，须分别烧录在miniQ 2WD Plus上层板和下层小车中。
/***************************************************
MiniQ 2WD plus (With Stainless Steel Probe)
<http://www.dfrobot.com.cn/goods-1074.html>
***************************************************
This example show you how to use LCD and button to control miniq_2wd.
Created 2016-1-15
By Andy zhou <[email protected]>
version:V1.0
GNU Lesser General Public License.
See <http://www.gnu.org/licenses/> for details.
All above must be included in any redistribution
****************************************************/
/***********Notice and Trouble shooting***************
1.Connection and Diagram can be found here
<http://wiki.dfrobot.com.cn/index.php?title=(SKU:DFR0302)MiniQ_2WD_Plus>
2.This code is tested on miniQ plus.
****************************************************/
#include<Wire.h>
#include<JLX12864G.h>
JLX12864G lcd(8,9,10,11,13);
long TimeNum=0;
char SendCommandData[]={'S','M','L','A','E','O','R','#'};//
char xReadData=0;
int Key_Num=0;
int Key_Up=0,Key_Down=0,Key_Left=0,Key_Right=0,Key_Ok=0;
byte CheckData1[]={1,3,5};
byte CheckData2[]={1,3,5,7};
char StartData[]={
0x40,0x42,0x42,0x42,0x42,0xFE,0x42,0x42,0x42,0x42,0xFE,0x42,0x42,0x42,0x42,0x00,/*-- 文字: 开 --*/
0x00,0x40,0x20,0x10,0x0C,0x03,0x00,0x00,0x00,0x00,0x7F,0x00,0x00,0x00,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0x90,0x70,0x1F,0x12,0xF0,0x00,0x20,0x70,0x28,0x27,0x22,0x28,0x70,0x20,0x00,/*-- 文字: 始 --*/
0x40,0x21,0x12,0x0C,0x06,0x09,0x30,0x00,0x7F,0x21,0x21,0x21,0x21,0x7F,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char StopData[]={
0x20,0x30,0xAC,0x63,0x10,0x00,0x08,0x48,0x48,0x48,0x7F,0x48,0x48,0x48,0x08,0x00,/*-- 文字: 结 --*/
0x22,0x23,0x22,0x12,0x12,0x00,0x00,0x7E,0x22,0x22,0x22,0x22,0x22,0x7E,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x04,0x04,0xF4,0x94,0x94,0x94,0x94,0xFF,0x94,0x94,0x94,0x94,0xF4,0x04,0x04,0x00,/*-- 文字: 束 --*/
0x20,0x20,0x11,0x10,0x08,0x06,0x01,0xFF,0x02,0x04,0x08,0x08,0x11,0x30,0x10,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char dfrobotlogo[]={
0x08,0xF8,0xF8,0x08,0x18,0xF0,0xE0,0x00,0x08,0x0F,0x0F,0x08,0x0C,0x07,0x03,0x00,
0x08,0xF8,0xF8,0x88,0xC8,0xC8,0x18,0x10,0x08,0x0F,0x0F,0x08,0x01,0x01,0x00,0x00,
0x08,0xF8,0xF8,0xC8,0xC8,0x78,0x30,0x00,0x08,0x0F,0x0F,0x08,0x03,0x0F,0x0C,0x08,
0xE0,0xF0,0x18,0x08,0x18,0xF0,0xE0,0x00,0x03,0x07,0x0C,0x08,0x0C,0x07,0x03,0x00,
0x08,0xF8,0xF8,0x48,0x48,0xF8,0xB0,0x00,0x08,0x0F,0x0F,0x08,0x08,0x0F,0x07,0x00,
0xE0,0xF0,0x18,0x08,0x18,0xF0,0xE0,0x00,0x03,0x07,0x0C,0x08,0x0C,0x07,0x03,0x00,
0x18,0x18,0x08,0xF8,0xF8,0x08,0x18,0x18,0x00,0x00,0x08,0x0F,0x0F,0x08,0x00,0x00,
};
char check[]={0xFF,0xFE,0xFC,0xF8,0xF0,0xE0,0xC0,0x80,0xFF,0x7F,0x3F,0x1F,0x0F,0x07,0x03,0x01,}; //It is a function switch indicator
char Spacebar[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,};
char FullPower[]={0xFF,0x01,0xFD,0xFD,0xFD,0xFD,0x01,0xFF,0xFF,0x80,0xBF,0xBF,0xBF,0xBF,0x80,0xFF,};//It is a full power indicator
char HalfPower[]={0xFF,0x01,0x81,0x81,0x81,0x81,0x01,0xFF,0xFF,0x80,0xBF,0xBF,0xBF,0xBF,0x80,0xFF,};//
char LowPower[]={0xFF,0x01,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x80,0xB8,0xB8,0xB8,0xB8,0x80,0xFF,};
char ShortagePower[]={0xFF,0x01,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x80,0x80,0x80,0x80,0x80,0x80,0xFF,};
char PatrolLineModel[]={
0x40,0x42,0x4C,0xC0,0x40,0xA0,0x18,0x07,0x62,0x98,0x07,0x62,0x98,0x07,0x02,0x00,/*-- 文字: 巡 --*/
0x80,0x40,0x20,0x1F,0x20,0x40,0x43,0x4C,0x40,0x41,0x4E,0x44,0x41,0x4E,0x44,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x40,0x60,0x58,0xC7,0x62,0x00,0x90,0x90,0x90,0xFF,0x90,0x92,0x9C,0x94,0x80,0x00,/*-- 文字: 线 --*/
0x20,0x22,0x23,0x12,0x12,0x12,0x20,0x20,0x10,0x13,0x0C,0x14,0x22,0x40,0xF8,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xD0,0xFF,0x50,0x90,0x04,0xF4,0x54,0x5F,0x54,0x54,0x5F,0xF4,0x04,0x00,0x00,/*-- 文字: 模 --*/
0x03,0x00,0xFF,0x00,0x00,0x84,0x85,0x45,0x35,0x0F,0x15,0x25,0x65,0xC4,0x44,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x08,0x88,0x88,0x88,0x88,0x88,0x08,0xFF,0x08,0x09,0x0E,0x0A,0x08,0x00,0x00,/*-- 文字: 式 --*/
0x00,0x20,0x60,0x30,0x1F,0x10,0x08,0x08,0x00,0x07,0x18,0x20,0x40,0x80,0x70,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char SearchLightModel[]={
0x00,0x00,0x42,0x4A,0x4A,0x4A,0x4A,0x4A,0x4A,0x4A,0xCA,0x4A,0x7E,0x00,0x00,0x00,/*-- 文字: 寻 --*/
0x00,0x01,0x01,0x01,0x05,0x39,0x11,0x01,0x41,0x81,0x7F,0x01,0x01,0x01,0x01,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x40,0x42,0x44,0x5C,0xC8,0x40,0x7F,0x40,0xC0,0x50,0x4E,0x44,0x60,0x40,0x00,/*-- 文字: 光 --*/
0x00,0x80,0x40,0x20,0x18,0x07,0x00,0x00,0x00,0x3F,0x40,0x40,0x40,0x40,0x78,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xD0,0xFF,0x50,0x90,0x04,0xF4,0x54,0x5F,0x54,0x54,0x5F,0xF4,0x04,0x00,0x00,/*-- 文字: 模 --*/
0x03,0x00,0xFF,0x00,0x00,0x84,0x85,0x45,0x35,0x0F,0x15,0x25,0x65,0xC4,0x44,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x08,0x88,0x88,0x88,0x88,0x88,0x08,0xFF,0x08,0x09,0x0E,0x0A,0x08,0x00,0x00,/*-- 文字: 式 --*/
0x00,0x20,0x60,0x30,0x1F,0x10,0x08,0x08,0x00,0x07,0x18,0x20,0x40,0x80,0x70,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char ObstacleAvoidanceModel[]={
0x40,0x42,0xCC,0x00,0xFE,0x92,0x92,0x9E,0x40,0x4C,0x55,0xE6,0x5C,0x44,0x40,0x00,/*-- 文字: 避 --*/
0x40,0x20,0x1F,0x24,0x43,0x4F,0x48,0x4F,0x40,0x42,0x42,0x5F,0x42,0x42,0x40,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0xFE,0x22,0x5A,0x86,0x10,0xD2,0x56,0x5A,0x53,0x52,0x5A,0xD6,0x12,0x10,0x00,/*-- 文字: 障 --*/
0x00,0xFF,0x02,0x04,0x13,0x10,0x17,0x15,0x15,0xFD,0x15,0x15,0x17,0x10,0x10,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xD0,0xFF,0x50,0x90,0x04,0xF4,0x54,0x5F,0x54,0x54,0x5F,0xF4,0x04,0x00,0x00,/*-- 文字: 模 --*/
0x03,0x00,0xFF,0x00,0x00,0x84,0x85,0x45,0x35,0x0F,0x15,0x25,0x65,0xC4,0x44,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x08,0x88,0x88,0x88,0x88,0x88,0x08,0xFF,0x08,0x09,0x0E,0x0A,0x08,0x00,0x00,/*-- 文字: 式 --*/
0x00,0x20,0x60,0x30,0x1F,0x10,0x08,0x08,0x00,0x07,0x18,0x20,0x40,0x80,0x70,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char RemoteControlModel[]={
0x40,0x42,0xCC,0x00,0x00,0x4A,0x32,0x26,0x2A,0xE1,0x31,0x29,0x25,0x00,0x00,0x00,/*-- 文字: 遥 --*/
0x40,0x20,0x1F,0x20,0x40,0x41,0x5D,0x51,0x51,0x5F,0x51,0x51,0x5D,0x41,0x40,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x08,0x08,0x08,0xFF,0x88,0x48,0x00,0x98,0x48,0x28,0x0A,0x2C,0x48,0xD8,0x08,0x00,/*-- 文字: 控 --*/
0x02,0x42,0x81,0x7F,0x00,0x00,0x40,0x42,0x42,0x42,0x7E,0x42,0x42,0x42,0x40,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xD0,0xFF,0x50,0x90,0x04,0xF4,0x54,0x5F,0x54,0x54,0x5F,0xF4,0x04,0x00,0x00,/*-- 文字: 模 --*/
0x03,0x00,0xFF,0x00,0x00,0x84,0x85,0x45,0x35,0x0F,0x15,0x25,0x65,0xC4,0x44,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x08,0x88,0x88,0x88,0x88,0x88,0x08,0xFF,0x08,0x09,0x0E,0x0A,0x08,0x00,0x00,/*-- 文字: 式 --*/
0x00,0x20,0x60,0x30,0x1F,0x10,0x08,0x08,0x00,0x07,0x18,0x20,0x40,0x80,0x70,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char MusicFunction[]={
0x40,0x40,0x44,0x44,0x4C,0x74,0x44,0x45,0x46,0x64,0x5C,0x44,0x44,0x44,0x40,0x00,/*-- 文字: 音 --*/
0x00,0x00,0x00,0xFF,0x49,0x49,0x49,0x49,0x49,0x49,0x49,0xFF,0x00,0x00,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x00,0x40,0xFC,0x44,0x44,0x44,0x46,0xFA,0x42,0x43,0x43,0x42,0x40,0x00,0x00,/*-- 文字: 乐 --*/
0x00,0x20,0x18,0x0C,0x07,0x12,0x20,0x40,0x3F,0x00,0x00,0x02,0x0C,0x38,0x10,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x04,0x04,0x04,0xFC,0x04,0x14,0x14,0x10,0x90,0x7F,0x10,0x10,0xF0,0x00,0x00,/*-- 文字: 功 --*/
0x04,0x0C,0x04,0x04,0x03,0x42,0x22,0x11,0x0C,0x23,0x20,0x60,0x20,0x1F,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xB8,0x97,0x92,0x90,0x94,0xB8,0x10,0x00,0x7F,0x48,0x48,0x44,0x74,0x20,0x00,/*-- 文字: 能 --*/
0x00,0xFF,0x0A,0x0A,0x4A,0x8A,0x7F,0x00,0x00,0x3F,0x44,0x44,0x42,0x72,0x20,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char LightsFunction[]={
0x80,0x70,0x00,0xFF,0x40,0x30,0x00,0x04,0x04,0x04,0x04,0xFC,0x04,0x04,0x04,0x00,/*-- 文字: 灯 --*/
0x40,0x30,0x0C,0x03,0x02,0x04,0x08,0x00,0x00,0x20,0x40,0x3F,0x00,0x00,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x40,0x42,0x44,0x5C,0xC8,0x40,0x7F,0x40,0xC0,0x50,0x4E,0x44,0x60,0x40,0x00,/*-- 文字: 光 --*/
0x00,0x80,0x40,0x20,0x18,0x07,0x00,0x00,0x00,0x3F,0x40,0x40,0x40,0x40,0x78,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x04,0x04,0x04,0xFC,0x04,0x14,0x14,0x10,0x90,0x7F,0x10,0x10,0xF0,0x00,0x00,/*-- 文字: 功 --*/
0x04,0x0C,0x04,0x04,0x03,0x42,0x22,0x11,0x0C,0x23,0x20,0x60,0x20,0x1F,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xB8,0x97,0x92,0x90,0x94,0xB8,0x10,0x00,0x7F,0x48,0x48,0x44,0x74,0x20,0x00,/*-- 文字: 能 --*/
0x00,0xFF,0x0A,0x0A,0x4A,0x8A,0x7F,0x00,0x00,0x3F,0x44,0x44,0x42,0x72,0x20,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
char MovementFunction[]={
0x40,0x41,0xCE,0x04,0x00,0x20,0x22,0xA2,0x62,0x22,0xA2,0x22,0x22,0x22,0x20,0x00,/*-- 文字: 运 --*/
0x40,0x20,0x1F,0x20,0x28,0x4C,0x4A,0x49,0x48,0x4C,0x44,0x45,0x5E,0x4C,0x40,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x20,0x24,0x24,0xE4,0x24,0x24,0x24,0x20,0x10,0x10,0xFF,0x10,0x10,0xF0,0x00,0x00,/*-- 文字: 动 --*/
0x08,0x1C,0x0B,0x08,0x0C,0x05,0x4E,0x24,0x10,0x0C,0x03,0x20,0x40,0x3F,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x00,0x04,0x04,0x04,0xFC,0x04,0x14,0x14,0x10,0x90,0x7F,0x10,0x10,0xF0,0x00,0x00,/*-- 文字: 功 --*/
0x04,0x0C,0x04,0x04,0x03,0x42,0x22,0x11,0x0C,0x23,0x20,0x60,0x20,0x1F,0x00,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
0x10,0xB8,0x97,0x92,0x90,0x94,0xB8,0x10,0x00,0x7F,0x48,0x48,0x44,0x74,0x20,0x00,/*-- 文字: 能 --*/
0x00,0xFF,0x0A,0x0A,0x4A,0x8A,0x7F,0x00,0x00,0x3F,0x44,0x44,0x42,0x72,0x20,0x00,/*-- 宋体12; 此字体下对应的点阵为：宽x高=16x16 --*/
};
void setup(){
pinMode(7,OUTPUT);
lcd.init();
lcd.clear();
Wire.begin();
Serial1.begin(57600);
}
void loop(){
int n=0;
lcd.graphic_8x16(3,2,dfrobotlogo,7);
lcd.string_5x7(5,5,"miniQ 2WD V3");
lcd.string_5x7(6,15,"www.dfrobot.com");
lcd.string_5x7(8,1,"Please press any key!");
digitalWrite(7,HIGH);
while(Key_Num==0){
Key_Scan();
}
lcd.clear();
FunctionMenu();
while(1){//Enter the cycle does not return
Key_Scan();
Key_Deal();
if(Key_Down==4){
Key_Down=1;
}
ReadPowerData();
}
}
void FunctionMenu(void){
lcd.graphic_16x16(1,9,MusicFunction,4);
lcd.graphic_16x16(3,9,LightsFunction,4);
lcd.graphic_16x16(5,9,MovementFunction,4);
}
void MusicFunctionMode(void){
lcd.graphic_16x16(1,29,MusicFunction,4);
}
void LightFunctionMode(void){
lcd.graphic_16x16(1,29,LightsFunction,4);
}
void MovementFunctionMode(void){
lcd.graphic_16x16(1,9,PatrolLineModel,4);
lcd.graphic_16x16(3,9,SearchLightModel,4);
lcd.graphic_16x16(5,9,ObstacleAvoidanceModel,4);
lcd.graphic_16x16(7,9,RemoteControlModel,4);
}
void Key_Scan(void){
int analog = analogRead(A0);
if(analog>=950){
Key_Num=0;
return;
}else{
Key_Num=1;
if(analog>=750&&analog<950){//Press up and key_num=1
delay(2);
if(analog>=750&&analog<950){
while(analog<950)
analog = analogRead(A0);
Key_Up++;
}
}else if(analog>=550&&analog<750){//Press right and key_num=2
delay(2);
if(analog>=550&&analog<750){
while(analog<950)
analog = analogRead(A0);
Key_Right=1;
}
}else if(analog>=350&&analog<550){//Press down and key_num=3
delay(2);
if(analog>=350&&analog<550){
while(analog<950)
analog = analogRead(A0);
Key_Down++;
}
}else if(analog>=150&&analog<350){//Press ok and key_num=4
delay(2);
if(analog>=150&&analog<350){
while(analog<950)
analog = analogRead(A0);
Key_Ok=1;
}
}else if(analog<150){//Press left and key_num=5
delay(2);
if(analog<150){
while(analog<950)
analog = analogRead(A0);
Key_Left=1;
}
}
}
}
void Key_Deal(void){
if(Key_Down==1){
Lcd_Check_Graphic(1,0);
if(Key_Ok==1){//进入音乐功能界面
lcd.clear();
MusicFunctionMode();
IICWritData(SendCommandData[1]);
while(1){
Key_Scan();
Key_Ok=0;
if(Key_Left==1){
Key_Left=0;
Key_Down=1;
IICWritData(SendCommandData[0]);
goto Back_One_Level;//跳回功能界面
}
ReadPowerData();
}
}
}
if(Key_Down==2){
Lcd_Check_Graphic(3,0);
if(Key_Ok==1){//进入灯光功能界面
lcd.clear();
LightFunctionMode();
IICWritData(SendCommandData[2]);
while(1){
Key_Scan();
Key_Ok=0;
if(Key_Left==1){
Key_Left=0;
Key_Down=2;
IICWritData(SendCommandData[0]);
goto Back_One_Level;//跳回功能界面
}
ReadPowerData();
}
}
}
if(Key_Down==3){
Lcd_Check_Graphic(5,0);
if(Key_Ok==1){//进入运动功能界面
Key_Ok=0;
Key_Down=1;
lcd.clear();
MovementFunctionMode();
while(1){
Key_Scan();
if(Key_Left==1){
Key_Left=0;
Key_Down=3;
goto Back_One_Level;
}
if(Key_Down==1){
Lcd_Check_Graphic(1,1);
if(Key_Ok==1){
Key_Ok=0;
Key_Up=0;
lcd.clear();
lcd.graphic_16x16(1,29,PatrolLineModel,4);//巡线模式
// lcd.graphic_16x16(3,1,StartData,2);
IICWritData(SendCommandData[3]);
//send line data
while(1){
Key_Scan();
if(Key_Left==1){
Key_Ok=0;
Key_Left=0;
Key_Down=1;
IICWritData(SendCommandData[0]);
goto Back_Two_Level_Move;//跳回到运动功能界面
}
ReadPowerData();
}
Back_Two_Level_Move://运动功能界面
lcd.clear();
MovementFunctionMode();
}
}
if(Key_Down==2){
Lcd_Check_Graphic(3,1);
if(Key_Ok==1){
Key_Ok=0;
lcd.clear();
lcd.graphic_16x16(1,29,SearchLightModel,4);
IICWritData(SendCommandData[4]);
//send light data
while(1){
Key_Scan();
if(Key_Left==1){
Key_Ok=0;
Key_Left=0;
Key_Down=2;
IICWritData(SendCommandData[0]);
goto Back_Two_Level_Move;//跳回到运动功能界面
}
ReadPowerData();
}
}
}
if(Key_Down==3){
Lcd_Check_Graphic(5,1);
if(Key_Ok==1){
Key_Ok=0;
lcd.clear();
lcd.graphic_16x16(1,29,ObstacleAvoidanceModel,4);
IICWritData(SendCommandData[5]);
//send OBSTACLEAVOIDANCE data
while(1){
Key_Scan();
if(Key_Left==1){
Key_Ok=0;
Key_Left=0;
Key_Down=3;
IICWritData(SendCommandData[0]);
goto Back_Two_Level_Move;//跳回到运动功能界面
}
ReadPowerData();
}
}
}
if(Key_Down==4) {
Lcd_Check_Graphic(7,1);
if(Key_Ok==1){
Key_Ok=0;
lcd.clear();
lcd.graphic_16x16(1,29,RemoteControlModel,4);
IICWritData(SendCommandData[6]);
//send Remote data
while(1){
Key_Scan();
if(Key_Left==1){
Key_Ok=0;
Key_Left=0;
Key_Down=4;
IICWritData(SendCommandData[0]);
goto Back_Two_Level_Move;//跳回到运动功能界面
}
SerialReadWriteData();
ReadPowerData();
}
}
}
if(Key_Down==5)
Key_Down=1;
ReadPowerData();
}
Back_One_Level://功能界面,如果功能跳转函数放在if的外面，lcd显示会闪烁
lcd.clear();
FunctionMenu();
}
}
}
void SerialReadWriteData(void){
if(Serial1.available()){
char SerialData = Serial1.read();
switch(SerialData){
case 'w':
IICWritData('w');
break;
case 'a':
IICWritData('a');
break;
case 's':
IICWritData('s');
break;
case 'd':
IICWritData('d');
break;
default:
IICWritData('S');
break;
}
}
}
void Lcd_Check_Graphic(byte CheckNum,char RowsNum){//The check of indicator switch function
if(RowsNum==0){
for(int i=0;i<3;i++){
if(CheckData1[i]==CheckNum) lcd.graphic_8x16(CheckNum,1,check,1);
else lcd.graphic_8x16(CheckData1[i],1,Spacebar,1);
}
}else if(RowsNum==1){
for(int j=0;j<4;j++){
if(CheckData2[j]==CheckNum)
lcd.graphic_8x16(CheckNum,1,check,1);
else
lcd.graphic_8x16(CheckData2[j],1,Spacebar,1);
}
}
}
void IICWritData(char data){
Wire.beginTransmission(4); //发送数据到设备号为4的从机
Wire.write(data); // 发送变量x中的一个字节
Wire.endTransmission(); // 停止发送
}
void IICReadData(void){
Wire.requestFrom(4, 1); //通知4号从机上传1个字节
while(Wire.available()>0){ // 当主机接收到从机数据时
xReadData = Wire.read(); //接收一个字节赋值给c
}
}
//读取电池电量函数
void ReadPowerData(void){
TimeNum++;
if(TimeNum==10){
TimeNum=0;
IICReadData();
switch(xReadData){
case'3':{
lcd.graphic_8x16(1,121,FullPower,1);
digitalWrite(7,HIGH);
break;
}
case'2':{
lcd.graphic_8x16(1,121,HalfPower,1);
digitalWrite(7,HIGH);
break;
}
case'1':{
lcd.graphic_8x16(1,121,LowPower,1);
digitalWrite(7,HIGH);
break;
}
case'0':{
lcd.graphic_8x16(1,121,ShortagePower,1);
digitalWrite(7,LOW);
break;
}
}
}
}
/***************************************************
MiniQ 2WD plus (With Stainless Steel Probe)
<http://www.dfrobot.com.cn/goods-1074.html>
***************************************************
This example show you how to use LCD and button to control miniq_2wd.
Created 2016-1-15
By Andy zhou <[email protected]>
version:V1.0
GNU Lesser General Public License.
See <http://www.gnu.org/licenses/> for details.
All above must be included in any redistribution
****************************************************/
/***********Notice and Trouble shooting***************
1.Connection and Diagram can be found here
<http://wiki.dfrobot.com.cn/index.php?title=(SKU:DFR0302)MiniQ_2WD_Plus>
2.This code is tested on miniQ.
****************************************************/
#include <Wire.h>
#include <Adafruit_NeoPixel.h>
#include <IRremote.h>
#if defined(ARDUINO) && ARDUINO >= 100
#define printByte(args) write(args);
#else
#define printByte(args) print(args,BYTE);
#endif
uint8_t empty[8] = {0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x1f};
uint8_t heart[8] = {0x0,0xa,0x1f,0x1f,0xe,0x4,0x0,0x0};
#define address 0x1e
int length;
#define RGB_ws2812 10
Adafruit_NeoPixel strip = Adafruit_NeoPixel(60, RGB_ws2812, NEO_GRB + NEO_KHZ800);
#define EN1 6//右侧电机使能端
#define IN1 7//右侧电机方向端
#define EN2 5//左侧电机使能端
#define IN2 12//左侧电机方向端
#define FORW 0//前进
#define BACK 1//后退
#define IR_IN 17//红外接收
#define L_IR 13//左红外发送
#define R_IR 8//右红外发送
#define BUZZER 16//蜂鸣器
#define Vr 5//参考电压值
#define NTD0 -1
#define NTD1 294
#define NTD2 330
#define NTD3 350
#define NTD4 393
#define NTD5 441
#define NTD6 495
#define NTD7 556
#define NTDL1 147
#define NTDL2 165
#define NTDL3 175
#define NTDL4 196
#define NTDL5 221
#define NTDL6 248
#define NTDL7 278
#define NTDH1 589
#define NTDH2 661
#define NTDH3 700
#define NTDH4 786
#define NTDH5 882
#define NTDH6 990
#define NTDH7 112
//列出全部D调的频率
#define WHOLE 1
#define HALF 0.5
#define QUARTER 0.25
#define EIGHTH 0.25
#define SIXTEENTH 0.625
//列出所有节拍
int tune1[]= //根据简谱列出各频率
{
NTDH3,NTDH3,NTDH2,NTDH3,NTD6,NTDH3,NTD6,NTD5,
NTDH2,NTDH1,NTDH2,NTDH3,NTDH2,NTDH1,
NTD6,NTDH3,NTDH2,NTDH3,NTDH2,NTDH1,NTD7,
NTD6,NTD5,NTD6,NTD7,NTD6,NTD5
};
float durt1[]= //根据简谱列出各节拍
{
0.5,0.5,1,0.5,1,1.5,1,1,
0.5,0.5,1,0.5,1,2,
1,1,0.5,0.5,1,0.5,1,
0.5,0.5,1,0.5,1,2
};
int tonepin=16; //得用16号接口
char xReadWriteData='S';
int count;//对返回的脉冲进行计数
float data[7]={0X00,0X00,0X00,0X00,0x00,0xff,0x00};//存储8个通道ad转换的值
char value[5]={0x00,0x00,0x00,0x00,0x00};//五个寻线传感器的电压值
char key_1=0x00,key_2=0x00,key_3=0x00;//按键的计数值
//计数里程
int count_r=0,count_l=0;//对左右轮返回的脉冲进行计数
//遥控参数
IRrecv irrecv(IR_IN);
decode_results results;
int SpeedNumL=100;
int SpeedNumR=100;
//控制电机转动
void Motor_Control(int M1_DIR,int M1_EN,int M2_DIR,int M2_EN){
//////////M1////////////////////////
if(M1_DIR==FORW)//M1方向为FORW
digitalWrite(IN1,FORW); //向前
else
digitalWrite(IN1,BACK);//向后
if(M1_EN==0) //M1速度为0
analogWrite(EN1,LOW);//置低，停止
else
analogWrite(EN1,M1_EN);//设置给定的数值
///////////M2//////////////////////
if(M2_DIR==FORW) //M2方向为FORW
digitalWrite(IN2,FORW);//向前
else
digitalWrite(IN2,BACK);//向后
if(M2_EN==0) //M2速度为0
analogWrite(EN2,LOW);//置低，停止
else//否则
analogWrite(EN2,M2_EN);//设置给定的数值
}
//避障
void L_Send40KHZ(void){//左发射管发射频率为40kHZ脉冲
int i;
for(i=0;i<24;i++){
digitalWrite(L_IR,LOW);//设置左发射管为高电平
delayMicroseconds(8);//延时
digitalWrite(L_IR,HIGH);//设置左发射管为低电平
delayMicroseconds(8);//延时
}
}
void R_Send40KHZ(void){//右发射管发射频率为40kHZ脉冲
int i;
for(i=0;i<24;i++){
digitalWrite(R_IR,LOW);//设置右发射管为高电平
delayMicroseconds(8);//延时
digitalWrite(R_IR,HIGH);//设置右发射管为低电平
delayMicroseconds(8);//延时
}
}
//计数里程
void LEFT(void){
//左侧车轮返回脉冲计数
//if(++count_l=100)
count_l++;
}
void RIGHT(void){
//右侧车轮返回脉冲计数
// if(++count_r=100)
count_r++;
}
void pcint0_init(void){//引脚变化中断初始化
PCICR = 0X01;//使能第0组引脚变化中断
PCMSK0 = 0X01;//使能第0组的第0个引脚变化中断
}
ISR(PCINT0_vect){//PB0引脚变化中断
count++;//计数接收到的脉冲
}
void Obstacle_Avoidance(void){//避障子函数
char i;
count=0;
for(i=0;i<20;i++){//左发射管发射20个脉冲
L_Send40KHZ();
delayMicroseconds(600);
}
if(count>20){//接收超过10个脉冲，判断有障碍物
Motor_Control(BACK,SpeedNumR,BACK,SpeedNumL);//后退
delay(50);//延时
Motor_Control(BACK,SpeedNumR,FORW,SpeedNumL);//右转
delay(50);//延时
}else{
Motor_Control(FORW,SpeedNumR,FORW,SpeedNumL);//前进
}
count=0;
for(i=0;i<20;i++){//右发射管发射20个脉冲
R_Send40KHZ();
delayMicroseconds(600);
}
if(count>20){
Motor_Control(BACK,SpeedNumR,BACK,SpeedNumL);//后退
delay(50);//延时
Motor_Control(FORW,SpeedNumR,BACK,SpeedNumL);//左转
delay(50);//延时
}else{
Motor_Control(FORW,SpeedNumR,FORW,SpeedNumL);//前进
}
}
//读取模拟端口电压值
void Read_Value(void){
int i;
for(i=0;i<7;i++){
data[i]=analogRead(i);//读取模拟i口电压值
data[i]= ((data[i]*Vr)/1024); //转换成模拟值
}
}
//寻线
void value_adjust(unsigned char num){//调整寻线传感器的值
if(num==1){//调节第一个寻线传感器
if(data[0]>value[0]){
colorWipe(strip.Color(0, 255, 0), 5); // Red
}else{
colorWipe(strip.Color(255, 0, 0), 1); // Green
}
}
if(num==2){//调节第二个寻线传感器
if(data[1]>value[1]){
colorWipe(strip.Color(0, 255, 0), 1); // Red
}else{
colorWipe(strip.Color(255, 0, 0), 1); // Green
}
}
if(num==3){//调节第三个寻线传感器
if(data[2]>value[2]){
colorWipe(strip.Color(0, 255, 0), 1); // Red
}else{
colorWipe(strip.Color(255, 0, 0), 1); // Green
}
}
if(num==4){//调节第四个寻线传感器
if(data[3]>value[3]){
colorWipe(strip.Color(0, 255, 0), 1); // Red
}else{
colorWipe(strip.Color(255, 0, 0), 1); // Green
}
}
if(num==5){//调节第五个寻线传感器
if(data[4]>value[4]){
colorWipe(strip.Color(0, 255, 0), 1); // Red
}else{
colorWipe(strip.Color(255, 0, 0), 1); // Green
}
}
}
void huntline_deal(void){
if(data[0]>(value[0]-1)&&data[1]>(value[1]-1)&&data[2]<(value[2])&&data[3]>(value[3]-1)&&data[4]>(value[4]-1))//测一下实际值
Motor_Control(FORW,100,FORW,100);//前进
else if(data[0]>(value[0]-1)&&data[1]>(value[1]-1)&&data[2]<(value[2]-1)&&data[3]<(value[3]-1)&&data[4]>(value[4]-1))
Motor_Control(BACK,20,FORW,100);//右转
else if(data[0]>(value[0]-1)&&data[1]>(value[1]-1)&&data[2]>(value[2]-1)&&data[3]<(value[3]-1)&&data[4]>(value[4]-1))
Motor_Control(BACK,100,FORW,100);//快速右转
else if(data[0]>(value[0]-1)&&data[1]>(value[1]-1)&&data[2]>(value[2]-1)&&data[3]<(value[3]-1)&&data[4]<(value[4]-1))
Motor_Control(BACK,100,FORW,100);//快速右转
else if(data[0]>(value[0]-1)&&data[1]>(value[1]-1)&&data[2]>(value[2]-1)&&data[3]>(value[3]-1)&&data[4]<(value[4]-1))
Motor_Control(BACK,100,FORW,100);//快速右转
else if(data[0]>(value[0]-1)&&data[1]<(value[1]-1)&&data[2]<(value[2]-1)&&data[3]>(value[3]-1)&&data[4]>(value[4]-1))
Motor_Control(FORW,100,BACK,20);//左转
else if(data[0]>(value[0]-1)&&data[1]<(value[1]-1)&&data[2]>(value[2]-1)&&data[3]>(value[3]-1)&&data[4]>(value[4]-1))
Motor_Control(FORW,100,BACK,100);//快速左转
else if(data[0]<(value[0]-1)&&data[1]<(value[1]-1)&&data[2]>(value[2]-1)&&data[3]>(value[3]-1)&&data[4]>(value[4]-1))
Motor_Control(FORW,100,BACK,100);//快速左转
else if(data[0]<(value[0]-1)&&data[1]>(value[1]-1)&&data[2]>(value[2]-1)&&data[3]>(value[3]-1)&&data[4]>(value[4]-1))
Motor_Control(FORW,100,BACK,100);//快速左转
}
//寻光
void hunt_light(void){
if (data[5]>3.5) //根据实际的实验环境进行测量
Motor_Control(BACK,SpeedNumR,FORW,SpeedNumL);//右转
else if (data[5]< 1.5)
Motor_Control(FORW,SpeedNumR,BACK,SpeedNumL);//
else
Motor_Control(FORW,0,FORW,0);//停止
}
//遥控功能函数
void dump(decode_results *results){
if(results->value==0x00fd00ff){
Motor_Control(FORW,0,FORW,0);//停止
}
if(results->value==0x00fd807f){
Motor_Control(FORW,100,FORW,100);//前进
}
if(results->value==0x00fd906f){
Motor_Control(BACK,100,BACK,100);//后退
}
if(results->value==0x00fd20df){
Motor_Control(FORW,100,BACK,100);//左转
}
if(results->value==0x00fd609f){
Motor_Control(BACK,100,FORW,100);//右转
}
}
//蜂鸣器声音
void buzzer(void){//蜂鸣器发出一种声音
digitalWrite(BUZZER,HIGH);//置高，蜂鸣器响
delay(1);
digitalWrite(BUZZER,LOW);//置低，蜂鸣器不响
delay(10);
}
//按键扫描
void key_scan(void){
if(data[6]>4.50&&data[6]<6.00)//没有按键按下
return;//返回
else{
if(data[6]>=0.00&&data[6]<0.80){//按键1按下
delay(10);//延时消抖
if(data[6]>=0.00&&data[6]<0.80){//按键1确实按下
buzzer();//蜂鸣器响
while(data[6]>=0.00&&data[6]<0.80)
Read_Value();
key_1++;//按键1计数
if(key_1>=1&&key_1<=5)
value_adjust(key_1);//寻线传感器的值调整
}
}
else if(data[6]>=0.80&&data[6]<3){//按键2按下
delay(10);//延时消抖
if(data[6]>=0.80&&data[6]<3){
buzzer();//蜂鸣器响
while(data[6]>=0.50&&data[6]<3)
Read_Value();
if(key_1>=1&&key_1<=5){//按键1的值在1~5之间
value[key_1-1]++;//传感器的分辨轨迹的界限值加加（调整）
value_adjust(key_1);//跟实际值对比
}else{
key_2++;//key2计数
}
}
}else if(data[6]>=3&&data[6]<4){//按键3按下
delay(10);//延时消抖
if(data[6]>=3&&data[6]<4){
buzzer();//蜂鸣器响
while(data[6]>=3&&data[6]<4)
Read_Value();
if(key_1>=1&&key_1<=5){//按键1的值在1~5之间
value[key_1-1]--;//传感器的分辨轨迹的界限值减减（调整）
value_adjust(key_1);//跟实际值对比
}else{
key_3++;//key3计数
}
}
}
}
}
void key_deal(){
if(key_1==6){//寻线
huntline_deal();//调用寻线处理子函数
key_2=0x00;
key_3=0x00;
}else if(key_1 == 7){
key_1 = 0;
Motor_Control(FORW,0,FORW,0);//停止
}
}
//低电压检测
void low_voltage_check(void){
float voltage_num=analogRead(A9);
voltage_num=(15*voltage_num)/2048;
if(voltage_num<4.0||voltage_num>7.0){
while(1){
voltage_num=analogRead(A9);
voltage_num=(15*voltage_num)/2048;
if(voltage_num>=4.0&&voltage_num<=7.0)
break;
buzzer(); //蜂鸣器响
colorWipe(strip.Color(0, 255, 0), 1); // Red
colorWipe(strip.Color(0, 0, 0), 1);
}
}
}
void Velocity_function(void){//速度处理函数
if(count_l>count_r){
SpeedNumL=SpeedNumL-1;
SpeedNumR=SpeedNumR+1;
}else if(count_l<count_r){
SpeedNumL=SpeedNumL+1;
SpeedNumR=SpeedNumR-1;
}
count_l=0;
count_r=0;
}
void colorWipe(uint32_t c, uint8_t wait){
for(uint16_t i=0; i<strip.numPixels(); i++){
strip.setPixelColor(i, c);
strip.show();
delay(wait);
}
}
void setup(){
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(12,OUTPUT);
pinMode(8,OUTPUT);
pinMode(10,OUTPUT);
pinMode(13,OUTPUT);
pinMode(14,OUTPUT);
pinMode(16,OUTPUT);
pinMode(17,INPUT);
irrecv.enableIRIn();
strip.begin();
strip.show();
length=sizeof(tune1)/sizeof(tune1[0]); //计算长度
Wire.begin(4);// 加入 i2c 总线，设置从机地址为 #4
Wire.onReceive(receiveEvent);//注册接收到主机字符的事件
Wire.onRequest(requestEvent);// 注册主机通知从机上传数据的事件
attachInterrupt(2,RIGHT,FALLING);
attachInterrupt(3,LEFT,FALLING);
Motor_Control(FORW,0,FORW,0);
}
void loop(){
switch(xReadWriteData){
case'S':
Motor_Control(FORW,0,FORW,0);
break;
case'M':
MusicFunction();
break;
case'L':
RGBFunction();
break;
case'A':
LineFunction();
break;
case'E':
LightFuntion();
break;
case'O':
ObstacleFunction();
break;
case'R':
RemoteFunction();
break;
case'w':
Motor_Control(FORW,SpeedNumR,FORW,SpeedNumL);//前进
break;
case'a':
Motor_Control(FORW,SpeedNumR,BACK,SpeedNumL);//左转
break;
case's':
Motor_Control(BACK,SpeedNumR,BACK,SpeedNumL);//后退
break;
case'd':
Motor_Control(BACK,SpeedNumR,FORW,SpeedNumL);//右转
break;
}
Velocity_function();
// low_voltage_check();
colorWipe(strip.Color(0, 0, 0), 1);
}
// 当从机接收到主机字符，执行该事件
void receiveEvent(int howMany){
while( Wire.available()>1){ // 循环执行，直到数据包只剩下最后一个字符
char c = Wire.read(); // 作为字符接收字节
}
//接收主机发送的数据包中的最后一个字节
xReadWriteData = Wire.read(); // 作为整数接收字节
}
//当主机通知从机上传数据，执行该事件
void requestEvent(){// 响应主机的通知，向主机发送一个字节数据
float analog = analogRead(A9);
analog =(15*analog)/2048;
if(analog>4.7)
Wire.write( '3');
else if(analog>4.3)
Wire.write( '2');
else if(analog>3.9)
Wire.write( '1');
else
Wire.write( '0');
}
uint32_t Wheel(byte WheelPos){
if(WheelPos < 85){
return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
} else if(WheelPos < 170){
WheelPos -= 85;
return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
}else {
WheelPos -= 170;
return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
}
}
void rainbow(uint8_t wait){
uint16_t i, j;
for(j=0; j<256; j++){
for(i=0; i<strip.numPixels(); i++){
strip.setPixelColor(i, Wheel((i+j) & 255));
}
strip.show();
delay(wait);
}
}
// Slightly different, this makes the rainbow equally distributed throughout
void rainbowCycle(uint8_t wait) {
uint16_t i, j;
for(j=0; j<256*5; j++) { // 5 cycles of all colors on wheel
for(i=0; i< strip.numPixels(); i++) {
strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255));
}
strip.show();
delay(wait);
}
}
void MusicFunction(void){
for(int x=0;x<length;x++){
tone(tonepin,tune1[x]);
delay(350*durt1[x]); //这里用来根据节拍调节延时，500这个指数可以自己调整，在该音乐中，我发现用500比较合适。
noTone(tonepin);
}
}
void RGBFunction(void){
rainbow(5);
rainbowCycle(5);
}
void LineFunction(void){
//buzzer();//蜂鸣器响
Read_Value();
key_scan();
key_deal();
}
void LightFuntion(void){
Read_Value();
hunt_light();//调用寻光子函数
Velocity_function();//速度处理函数
}
void ObstacleFunction(void){
pcint0_init();//引脚变化中断初始化
sei(); //全局中断使能
Obstacle_Avoidance();
Velocity_function();
}
void RemoteFunction(void){
Motor_Control(FORW,0,FORW,0);//run motor
while(1){
if(irrecv.decode(&results)){
dump(&results);
irrecv.resume();
}
}
}
注：蓝牙4.0 无线手柄的数据传输格式，请参照蓝牙4.0 无线手柄的手柄BLE透传协议。
miniQ 2wd plus原理图
miniQ 2wd + miniQ 2wd plus程序
