[kenrobot_code]
/*******************************************
Adapt to visionsensor command 1.1
morpx .inc
*******************************************/
/*
* ---------------------------
* | |
* | ----------- |
* | | | |
* | | * | |
* | | center | |
* | ----------- |
* | object |
* | |
* ---------------------------
* Field of view
* 2017-2-12：kalman滤波，小车跟随以及TFT屏显示测试成功
* 履带小车底盘，蓝宙电机驱动板、蓝宙传感器HUB、arduino UNO
*
L9110 motor driver controlling 2 small DC motors
原型源代码来自：
http://www.electrodragon.com/w/L ... two_small_DC_motors
根据蓝宙altar电机驱动模块对应pin进行定义
关于转向：因为是差速转向，所以不再编写左右转向程序，而是用差速表达。
卡尔曼滤波;
https://raw.github.com/nut-code-monkey/KalmanFilter-for-Arduino
*/
#include "KalmanFilter.h"
//----------卡尔曼滤波部分定义
KalmanFilter kalmanFilterx;//实例化kalmanFilterx--x坐标
KalmanFilter kalmanFiltery;//实例化kalmanFiltery--y坐标
KalmanFilter kalmanFilterw;//实例化kalmanFilterw--宽度
KalmanFilter kalmanFilterh;//实例化kalmanFilterh--高度
//视觉识别模块MU部分定义
#include "VisionSensor.h"
VisionSensor MU(Serial, 115200);
//TFT 1.8寸彩屏，美科(microduino),采用Adafruit ST7735驱动库
#define sclk 13
#define mosi 12
#define cs 7
#define dc 8
#define rst -1 // you can also connect this to the Arduino reset
#include <Adafruit_GFX.h> // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library
#include <SPI.h>
//----------履带小车部分，采用蓝宙电子L9110S电机驱动板
const int AIA = 3 ; // (pwm) pin 3 connected to pin A-IA
const int AIB = 5 ; // (pwm) pin 5 connected to pin A-IB
const int BIA = 6 ; // (pwm) pin 6 connected to pin B-IA
const int BIB = 9 ; // (pwm) pin 9 connected to pin B-IB
byte speed = 200 ; // change this (0-255) to control the speed of the motors
#if defined(__SAM3X8E__)
#undef __FlashStringHelper::F(string_literal)
#define F(string_literal) string_literal
#endif
int objx,objy,objw,objh,objscore,objdist; //定义全局变量
Adafruit_ST7735 tft = Adafruit_ST7735(cs, dc, mosi, sclk, rst);
void setup() {
//串口监视
Serial.begin(115200);
//打开MU传感器，uart上送端口
MU.begin();
MU.search(); //开始视觉识别目标搜索
//小车部分
pinMode(AIA ,OUTPUT) ; // set pins to output
pinMode(AIB ,OUTPUT) ;
pinMode(BIA ,OUTPUT) ;
pinMode(BIB , OUTPUT) ;
//卡尔曼部分定义setState
kalmanFilterx.setState( MU.getX());
kalmanFiltery.setState( MU.getY());
kalmanFilterw.setState( MU.getWidth());
kalmanFilterh.setState( MU.getHeight());
//TFT屏绘制背景
tft.initR(INITR_BLACKTAB); // initialize a ST7735S chip, black tab
tft.fillScreen(ST7735_BLACK);//清屏
//画十字线
tft.drawFastHLine(0, 80, 128, ST7735_RED);
tft.drawFastVLine(64, 0, 160, ST7735_RED);
//画距离的标题：Dist(cm):
tft.setCursor(5, 5);
tft.setTextColor(ST7735_WHITE);
tft.setTextSize(0);
tft.println("Dist(cm):");
}
void loop() {
MU.search(); //搜索目标并识别
//卡尔曼滤波处理部分
double valueX = MU.getX();
double valueY = MU.getY();
double valueW = MU.getWidth();
double valueH = MU.getHeight();
kalmanFilterx.correct(valueX);
kalmanFiltery.correct(valueY);
kalmanFilterw.correct(valueW);
kalmanFilterh.correct(valueH);
double correctedValuex = kalmanFilterx.getState();
double correctedValuey = kalmanFiltery.getState();
double correctedValuew = kalmanFilterw.getState();
double correctedValueh = kalmanFilterh.getState();
objx=correctedValuex;
objy=correctedValuey;
objw=correctedValuew;
objh=correctedValueh;
//Serial.print(value); Serial.print(" | "); Serial.println(correctedValue);
if (MU.valid()>0)
{
if(MU.detected() == true) //如果目标搜索正确
{
/*
Serial.print("detected score is:");
Serial.println(MU.detectScore());
Serial.print("Object detected at:");
Serial.print(MU.getX()); // getX() means X (horizontal) coordinate of the center of the detection rectangle //
Serial.print(",");
Serial.print(MU.getY()); // getY() means Y (vertical) coordinate of the center of the detection rectangle //
Serial.print(" with wdith of:");
Serial.print(MU.getWidth());
Serial.print(" and height of:");
Serial.println(MU.getHeight());
*/
objdist=600/(objw+objh);
//这部分为测试观察，实际运行可以去除
Serial.print(MU.getX());
Serial.print(",");
Serial.println(objdist);
//Serial.print(MU.getY());
//Serial.print(",");
//Serial.println(objy);
objscore=MU.detectScore();
////画出目标识别框、显示距离以及识别评分棒
drawobject();
//小车跟随部分控制
//if (objw>32){
if (objdist<6){
goBackward(230,230); //正后
// delay(50);
}
//else if (objw<28){
else if (objdist>8){
goForward(230,230); //正前
//delay(50);
}
if (objx<40){
goForward(0,230); //前左
// delay(50);
}
else if (objx>60){
goForward(230,0); //前 右
//delay(50);
}
delay(50);
car_stop();
}
else
{
//Serial.println("No object detected");
delay(10);
}
}
else
{
//Serial.println("No data or too many detected");
delay(10);
}
}
void drawobject(){
//画十字线
tft.drawFastHLine(0, 80, 128, ST7735_RED);
tft.drawFastVLine(64, 0, 160, ST7735_RED);
tft.drawRect(objx,objy,objw,objh, ST7735_YELLOW); //画一个黄色框
tft.fillRect(5, 153,int(5+objscore*0.04) , 154, ST7735_BLUE);
//计算距离,常数为30*10=300
//int length=600/(objw+objh);
tft.setTextSize(0);
tft.setCursor(64, 5);
tft.setTextColor(ST7735_WHITE);
tft.println(objdist);//打印距离，先占位
delay(10);
tft.drawRect(objx,objy,objw,objh, ST7735_BLACK); //画一个黑框,遮盖黄框
tft.fillRect(5, 153, int(5+objscore*0.04), 154, ST7735_BLACK);
// delay(100);
tft.setCursor(64, 5);
tft.setTextColor(ST7735_BLACK);
tft.println(objdist);
}
void goForward(int speedA_value,int speedB_value){ //小车向前（包含正前，前左及前右）
analogWrite(AIA , 0 ); //A电机（左）反转
analogWrite(AIB , speedA_value);
analogWrite(BIA , speedB_value) ; //B电机（右）正转
analogWrite(BIB , 0) ;
}
void goBackward(int speedA_value,int speedB_value){ //小车向后，包括正后，后左及后右
analogWrite(AIA , speedA_value); //A电机（左）正转
analogWrite(AIB , 0 ) ;
analogWrite(BIA , 0) ; //B电机（右）反转
analogWrite(BIB , speedB_value) ;
}
void car_stop(){ //全停
analogWrite(AIA , 0);
analogWrite( AIB , 0);
analogWrite(BIA , 0) ;
analogWrite(BIB , 0) ;
}
[/kenrobot_code]
发表于 2017-3-13 00:59
发表于 2017-3-13 01:05
楼主
发表于 2017-3-13 08:24
