乐高指南针车(NXC)

发表于 9/29/2013 3:32:02 PM 阅读（8102）

图1 乐高指南者车全景图

本文通过乐高指南针传感器和超声波传感器的组合应用，找到智能车前方的三个物体，并把它们分别推出界外。由于乐高超声波传感器的精度为1厘米，指南针传感器的精度为1度，两者的精度并不高，所以通过两次扫描、定位和靠近，以使小车爪子能“抓住”物体，并成功推出界外。

以前的文章也介绍了一种通过两次扫描、定位和靠近来定位物体的方法，文章题目《智能机械手车的寻物取物表演（NXC）》，网址：http://blog.chinaaet.com/detail/24031.html。与本文介绍的方法不同，那篇文章是通过乐高伺服电机的编码器和超声波传感器的组合来定位物体。从这一点看来，前者的工作总是可以对后来者提供启发，这篇文章的编程思路就很大程度上借鉴了上篇文章的思路，使后来者的任务实现更容易达成。

实验视频：

视频网址：http://player.youku.com/player.php/sid/XNjE1MDYxMzEy/v.swf

这辆指南者车的电控系统由NXT控制器、指南针Sensor、超声波Sensor和两个伺服电机Moter组成，如何把它们有序连接在一起，是依靠乐高机械结构件来完成，可以参看下图，来搭建这个指南针车。

图2 指南者车搭建图

数字指南针，也叫电子罗盘，是利用地磁场来定北极的一种方法。古代称为罗经，现代利用先进加工工艺生产的磁阻传感器为罗盘的数字化提供了有力的帮助。现在一般有用磁阻传感器和磁通门加工而成的电子罗盘。传统罗盘用一根被磁化的磁针来感应地球磁场，地球磁场与磁针之间的磁力时磁针转动，直至磁针的两端分别指向地球的磁南极与磁北极。电子罗盘也一样，只不过把磁针换成了磁阻传感器，然后将感受到的地磁信息转换为数字信号输出给用户使用。

图3 Hitechnic指南针传感器

现在做一个实验，来体验一下LEGO 指南针数字传感器的功效。指定指南针车的朝向为二维平面上180度方向，当指南针车朝向偏离指定角度，智能车会原地旋转，直到小车朝向180度。程序如下。

task main(){
   SetSensorLowspeed(S1);//将Hitechnic公司的指南针传感器连到传感器端口1
   int currentCompass; // 指南针传感器的当前角度值
   int power;  //智能车原地旋转时的电机功率
   while(true){
     ClearScreen();  //把NXT控制器的液晶屏清屏
     //把当前指南针传感器的测量数据currentCompass显示在NXT控制器的液晶屏上
     TextOut(10,LCD_LINE1,"Actual reading" );
     NumOut(10,LCD_LINE2,currentCompass);
     Wait(30);
     //把指南针传感器的当前角度值读取到变量currentCompass
     currentCompass = SensorHTCompass(S1);
     //设置智能车原地旋转时的电机功率。当智能车的朝向离指定朝向180度偏差大时，
     //电机功率就大，当小车的朝向越接近180度，则电机功率逐渐减小，以使小车朝向
     //能精确定位在180度方向。
     power=2+abs(currentCompass-180)/10;
     //比较指南针车指定朝向180度与currentCompass的大小关系，如果不相等，
     //则AC电机驱使小车以两轮中心为圆点来旋转，直到两者相等，
     //才用break指令跳出while(true)循环。
     if (currentCompass > 180 )
     {
       OnFwdReg(OUT_A, power, OUT_REGMODE_SPEED);
       OnFwdReg(OUT_C, -power, OUT_REGMODE_SPEED);

     }
     else if(currentCompass < 180 )
     {
       OnFwdReg(OUT_A, -power, OUT_REGMODE_SPEED);
       OnFwdReg(OUT_C, power, OUT_REGMODE_SPEED);
     }
     else
     {
       break;
     }
  }
  Off(OUT_AC);  //小车电子煞车停止
  while(true);
}

由于超声波指向性强，能量消耗缓慢，在介质中传播的距离较远，因而超声波经常用于距离的测量，如测距仪和物位测量仪等都可以通过超声波来实现。超声波测距的原理是利用超声波在空气中的传播速度为已知，测量声波在发射后遇到障碍物反射回来的时间，根据发射和接收的时间差计算出发射点到障碍物的实际距离。由此可见，超声波测距原理与雷达原理是一样的。虽然目前的测距量程上能达到百米，但测量的精度往往只能达到厘米数量级。

图4 LEGO超声波传感器

超声波传感器有测距功能，而指南针传感器有定向功能，两者结合是否可以帮助智能车找到前方最近物体呢？从上文视频演示中，得到了肯定的答案。下面的程序就实现了这一任务。程序任务是通过乐高指南针传感器和超声波传感器的组合应用，找到智能车前方的三个物体，并把它们分别推出界外。

//Closest变量存储超声波传感器检测到与物体之间最近的距离
int Closest;
//ClosestCompass变量存储
//当超声波传感器检测到与物体之间最近的距离时的指南针角度值
int ClosestCompass;
//ClosestCompass变量存储当前指南针实时角度值
int currentCompass;
int power;  //电机功率值
//在一个大角度范围内，扫描离超声波传感器最近的物体
sub Scan_Object()
{
    //小车在A、C电机的带动下，向左旋转，到达扇形扫描的左端
    //RotateMotorEx函数参数含义：
    //A、C电机以40%的功率旋转180度、
    //以两轮中心为圆点来逆时针转弯、启动同步模式、使用电子煞车
     RotateMotorEx(OUT_AC,40,180,-100,true,true);
     //把C电机的转角重置为0
     ResetTachoCount(OUT_C);
     SetOutput(OUT_C,TachoCount,0);
     //小车向右扫描一个大角度扇面，直到C电机转过360度
     //以两轮中心为圆点来顺时针旋转，遇到阻力时保持速度
     OnFwdReg(OUT_A, -5, OUT_REGMODE_SPEED);
     OnFwdReg(OUT_C, 5, OUT_REGMODE_SPEED);
     Closest=255;
     while(MotorTachoCount(OUT_C)<360)
    {
       //把扇面扫描中，检测到的超声波传感器与物体的最近距离赋值给Closest       
       if(SensorUS(S4) < Closest)
       {
          Closest = SensorUS(S4);
          //把扇面扫描中的
          //最近距离时的指南针水平面角度赋值给ClosestCompass
ClosestCompass = SensorHTCompass(S1);
       }
    }
    Off(OUT_AC);  //小车电子煞车停止
    Wait(300);
}
//根据最近距离时的指南针角度值，小车旋转返回，找到离超声波传感器最近的物体
sub Find_Object()
{
   while(true){
     //把指南针传感器的当前角度值读取到currentCompass
     currentCompass = SensorHTCompass(S1);
     power=2+abs(currentCompass-180)/10;
     //比较ClosestCompass与currentCompass的大小关系，如果不相等，
     //则AC电机驱使以两轮中心为圆点来旋转，直到两者相等，
     //才用break指令跳出while(true)循环。
     if (currentCompass > ClosestCompass )
     {
       OnFwdReg(OUT_A, power, OUT_REGMODE_SPEED);
       OnFwdReg(OUT_C, -power, OUT_REGMODE_SPEED);

     }
     else if(currentCompass < ClosestCompass )
     {
       OnFwdReg(OUT_A, -power, OUT_REGMODE_SPEED);
       OnFwdReg(OUT_C, power, OUT_REGMODE_SPEED);
     }
     else if(currentCompass == ClosestCompass )
     {
       Off(OUT_AC); //小车电子煞车停止
       break;
     }
   }
}
//小车定位后，靠近物体
sub Move_Closer()
{
  int deg;
  //小车电机转过“deg”角度，向前靠近物体
  deg=Closest*12;
  RotateMotorEx(OUT_AC,60,deg,0,true,true);
}
//主程序
task main()
{
   Wait(3000); //等待3秒后，开始运行程序
   SetSensorUltrasonic(S4);//将超声波传感器连到传感器端口4
   SetSensorLowspeed(S1);   //将Hitechnic公司的指南针传感器连到传感器端口1
   int i,j;
   //把小车前面的三个物体分三次向前推出界外
   for(i=0;i<3;i++)
   {
     //定位物体和靠近物体的动作循环两次
     //由于超声波传感器的精度为1厘米，指南针传感器的精度为1度，
//两者的精度并不高，所以通过两次扫描、定位和靠近，
//以使小车爪子能“抓住”物体，并成功推出界外。
     for(j=0;j<2;j++)
     {
       Scan_Object();//在一个大角度范围内，扫描离超声波传感器最近的物体
       Find_Object();//定位离超声波传感器最近的物体
       Move_Closer();//小车定位后，靠近物体
     }
     //前进一小段距离,把已经“抓住”的物体，推出界外
     RotateMotorEx(OUT_AC,40,1*360,0,true,true);
     //小车长距离退回原位，
     RotateMotorEx(OUT_AC,-60,3*300,0,true,true);
  }
  while(true);  //停止运行程序
}

LEGO公司在2005年发表了智慧型机器人电控主机NXT。今年2013年夏天又上市了EV3新一代主机，相信以后还会有更新的、更强功能的主机。NXT和EV3除了主机运算速度提高加快，还提供了更多的传感器，还有Hitechnic、Mindsensord和Dexter公司也出品兼容乐高的第三方传感器，例如本文介绍的这款传感器就是Hitechnic公司出品的指南针传感器。

由于乐高机器人系统的流行，除了硬件受到大牌公司的追捧和支持，乐高系统的编程软件也有很多选择，你可以用“她”配套的图形化编程软件NXT-G编程，也可用同样支持图形化编程的LabVIEW软件编程，当然也能用C语言，用C编程的软件有MatLAB、RobotC和NXC等供你选，我选用了NXC。

2005年，Dave Baum先生依照NXT的硬件设备发展了NXC(Not exactly C)。NXC符合C语言语法，并完全支持所有NXT的硬件功能与多种外接传感器。最重要的是NXC完全是免费的，安装也很容易，一套9797机器人NXT套件、一台电脑，马上可以开工！

我推荐《機器人新視界NXC與NXT》一书来学习使用NXC语言，台湾人写的，蛮好的，在淘宝了输入繁体字可以找到它。