迹APP的视觉跟随小车端实现【向pz_cloud致敬】

沧海笑1122 · 发表于 2017-2-27 23:46

本帖最后由沧海笑1122 于 2017-2-27 23:48 编辑

项目：迹APP的视觉跟随小车端实现【向pz_cloud致敬】

基本想法：本届蓝宙赛的获奖作品，迹APP及视觉跟随小车（http://www.arduino.cn/thread-23020-1-1.html）给我留下了很深的印象，用旧安卓手机做视觉传感器，控制小车实现目标跟随。尤其是pz_cloud师兄提到的小车算法：

kittenblock中小学创客名师推荐的图形化编程软件

   左轮速度v_left=v_run+v_turn;
   右轮速度v_right=v_run-v_turn;

让我不由得想试试。正好近期在测试摩图的MU视觉传感器，也想将这两种视觉传感器实现方式做一个体验上的比对。所以就做了这个小车端的程序和尝试。

硬件：

1、安卓手机一部，刷了原生安卓CM11

2、履带式小车平台：全金属减速电机，是擦窗机器人的拆旧品，配有一块亚克力面板，真的是DIY佳品。电池是7.4V航模电池

3、主控板（UNO）+电机驱动板（L9110S）+传感器扩展板，来自为蓝宙altar测试时的硬件。

4、蓝牙透传模块（蓝宙）

5、可变电阻一只（4.7K）

6、乐高积木少许，用于做了一个临时手机固定架

软件想法：

1、我参考了极客工坊“小学徒”师兄的思路，他的思路是对屏幕区域进行判断，对目标出现的位置分别作出机动策略。我也做了尝试，感觉也不错，有特点。

但我还是希望严格按照pz_cloud师兄提供的小车算法：即对两只电机分别控制，用屏幕中心坐标作为参考，分别得到小车的左右偏移量以及前后偏移量，控制左右两只电机。

简单地说，两只电机分别控制，各自遵循各自的控制策略。大大简化了编程。

2、关于电机调速系数的设定，设置了一只可变电阻，然后在行进中观察系数的变化给小车行为带来的不同，选择一个相对稳定、精确的系数。

当然，这个系数，仅仅适合你这部手机以及你所使用的小车底盘。如果更换了上述配件，那就必须重新标定。

我的体会，系数大了，动力会猛、响应迅速，但缺点是小车会扭动，也就是调整中都可能出现过调，然后再回调，就使得小车走之字形前进。

如果系数过小，动力不足，响应慢，而且对于电机而言，实际上是有调速盲区的，比如我的电机，在PWM=70以下，基本上动不了。

分享代码，注释得比较多，就不一一解释了。

kittenblock中小学创客名师推荐的图形化编程软件

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

一个个人小项目，配套APP下载以及相关教程见
http://pengzhihui.xyz/2016/05/05/trace/
来自蓝宙赛的参赛项目，“迹”APP，旧手机做视觉传感器，颜色识别、跟随
http://www.arduino.cn/thread-23020-1-1.html
（1）2017-02-15
测试，带蓝牙成功
（2）2017-02-25 
可变电阻作为系数的输入，开始调试差速履带小车跟随
严格按照pz_cloud师兄的建议：
      左轮速度v_left=v_run+v_turn;
      右轮速度v_right=v_run-v_turn;
把v_left，v_right乘以一个系数之后当作pwm赋值给两个电机

判断条件：
v_left<-20  正转             v_left>20 反转      除此之外（-20<v_left<20） 停止
v_right<-20 反转             v_right>20 正转     除此之外（-20<v_right<20） 停止
构造 car_go()函数

关于系数的获取，如果系数过大，造成在追踪过程中，小车发生左右摇摆，如果系数过小，则动力不足。
因此设置了一只可变电阻，获取一个可以在运行中调整的系数，经过比对，选择1.82~1.85作为我这部小车（对应这部手机）的调速系数。
一旦手机、小车固定，就可以在运行中，去除可变电阻。将系数写入程序即可
*******************************************/

#include <TraceApp.h>
#define analogInPin 4    //用A4作为可变电阻输入
float sensorValue = 0;    //存储调整系数
float kc=1.82;//系数定1.82，在调试后确定
int v_left,v_right,v_run,v_turn; //左右电机以及前进、转向等四个参数
int x,y;//目标的座标

//小车部分设置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  

TraceApp obj(Serial, 9600);   //实例化检测对象

void setup()
{
  obj.begin();      //初始化,注意:后面不需要再用Serial.begin()!
}

void loop()
{
  obj.routine();    //尽可能让这一句频繁运行
  follow();//调用跟随子程序
    
}


void follow(){  //跟随子程序
  obj.routine();//物体识别；尽可能让这一句频繁运行
  if (obj.valid())
  {
     x=obj.getX();//x坐标
     y=obj.getY();//y坐标
    
    v_run=y-170;//根据我的手机情况，计算y偏移差值，也就是前进后退量，此参数需要根据各自手机情况标定，下同
          v_turn=x-135;//根据我的手机情况，计算x偏移差值，也就是转向量
    v_left=  v_run+  v_turn;//合成左电机参数
          v_right= v_run-  v_turn;//合成右电机参数
   // Serial.println(v_right,v_left);//右、左，为了匹配履带电机的右A，左B
    car_go(v_right,v_left);//调用小车控制函数
              
   }
 }

float getc(){
//读取可变电阻值，作为差速系数，在确定后，此段可以注释掉
   sensorValue = analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   sensorValue += analogRead(analogInPin);
   //show on the Serial Monitor
   Serial.print("kc = ");
   Serial.println(1023/(sensorValue/8));
   return 1023/(sensorValue/8);//返回1023/平均值,则在1023~1之间

}


  

//小车控制函数
void  car_go(int speedA_value,int speedB_value){ //小车的双轮控制程序
//  以俯视小车，HUB为正向，A为右电机，B为左电机
//判断B（左）电机行为
   if (speedB_value<-20)
   {
     speedB_value=constrain(int(abs(speedB_value)*getc()),0,255);
     analogWrite(BIA ,   speedB_value); //B电机（左）正转
     analogWrite(BIB ,  0 )  ; 
     Serial.print("speedB_value:+");
     Serial.println(speedB_value);
   }
   else 
   {
     if (speedB_value>20)
         {
     analogWrite(BIA , 0 ); //B电机（左）反转
     speedB_value=constrain(int(speedB_value*getc()),0,255);
     analogWrite(BIB ,  speedB_value); 
     Serial.print("speedB_value:-");
     Serial.println(speedB_value);
     

         }
         else 
         {//停转
           analogWrite(BIA ,  0); 
     analogWrite(BIB ,  0); 
         }
   }
//判断A（右）电机行为
   if (speedA_value<-20)
   {
     analogWrite(AIA , 0 ); //A电机（右）反转
     speedA_value=constrain(int(abs(speedA_value)*getc()),0,255);
     analogWrite(AIB , speedA_value); 
     Serial.print("speedA_value:+");
     Serial.println(speedA_value);

   }
   else 
   {
     if (speedA_value>20)
         {
     speedA_value=constrain(int(speedA_value*getc()),0,255);
     analogWrite(AIA ,   speedA_value); //A电机（右）正转
     analogWrite(AIB ,  0 )  ; 
     Serial.print("speedA_value:-");
     Serial.println(speedA_value);
 

         }
         else 
         {//停转
           analogWrite(AIA ,  0); 
     analogWrite(AIB ,  0);            
         }
   }
}

在进行小车跟随行为分析时，对各种目标位置进行了初步分析。屏幕右上角的坐标是0,0。