Posts Tagged ‘C#’

蛋疼的小球 – 重力感应器小试牛刀

由 动力老男孩 发表于 2010/07/23 21:53:24

上周，网友“忧郁飞花”提了个建议，说是可以用手机的重力感应器来控制小车，这样就可以把手机变成遥控方向盘。不知道这位同学为何如此忧郁，也许“飞花”形容的是该同学的工资流逝速度吧，在此假惺惺的慰问并感谢一下。

言归正传，手机当方向盘的想法虽然好，细节却还需要仔细推敲，例如倒退，调节转速，波动过大等情况的处理。一个应该可行的方案是用仰角来调节转速，把转速扩展到负值来实现倒退。（这时候我眼前出现了幻觉：圈圈在屋里拿着手机乱晃，小爱在外面剧烈抽风….）

汗，遥控小车还是先缓缓吧，这几天做了一个非常蛋疼的小程序来测试重力感应器。这个程序只有一个小球，在屏幕上滚来滚去。我特地找了个金色的珍珠，阿弥陀佛，保佑程序作者大富大贵，提前退休。

使用的手机是HTC的钻石2代，操作系统是windows mobile 6.5

滚动的金色小球

滚动的金色小球

写这个程序之前，先复习下高中的物理知识。假如一个小球在某时刻 t1，位置坐标是(x1,y1)，速度方向是（vx1，vy1），受到的重力加速度是(ax,ay)，那么当时光流逝到 t2 时，该小球的位置和速度应该是多少（假设t1，t2间隔很短，期间加速度没有变化）。其实x，y是矢量合成，咱们以x分量为例进行计算：

首先根据加速度的定义，vx2=vx1+ax*(t2-t1)，注意ax可能为负值，所以速度方向也可能会改变。
x2的坐标则可以用x2=x1+(t2-t1)*(vx1+vx2)/2 来计算，即位移等于平均速度乘以时间。

然后是考虑碰壁的情况，以x轴负临界值为例，在Δt时间之后，发现x轴坐标已经小于0（我这里按质点做假设，实际上应该是小于小球半径的时候就碰壁了），这时候的速度大多数情况下也应该是负值（不解释了）。这时候假设小球的弹性系数是f，那么反弹回来的速度分量，应该是vx2′=-vx2*f ；而x坐标，可以估算为把负值那部分反射回来，再乘弹性系数，即x2′=-x2*f

如果程序写成这样，会发现大多数情况下，这个小球像模像样的蹦跶着，但是在接近停止的时候会突然跳的更高，永远停不下来。这个是因为临界值处理的不好，如果我们的Δt无限小，这个公式是正确的，但是真正计算时，Δt只能根据手机的承受能力设定（我设置的是50ms），这就会发生下面的情况：

边界条件超出合理范围

边界条件超出合理范围

当x1非常小，而Δt比较大的时候，x2的绝对值已经远超过x1，即使乘上弹性系数仍然大于x1。这就造成了接近壁面时，小球弹起的高度反而比原来更高。最后我加上了一些懒汉修正，当高度较低时，直接把速度和位置全设置成0。

物理复习完了，剩下的事情就简单了。其实就是找个图片，然后用一个timer定时，每隔一段时间更新一下(x,y)坐标即可。看看最终的效果视频：

下面贴一段大家可能有兴趣的“核心代码”：

// http://www.diy-robots.com
// apply physics to a ball
Ball ApplyDevicePhysics(GVector gVector, Ball ball, int millisecondsElapsed)
{
    int maxX = ClientSize.Width - ball.Radius;
    int maxY = ClientSize.Height - ball.Radius;
    double t = millisecondsElapsed / 1000.0;

    #region calculate x values
    gVector.X *= frictionForce;
    double v1 = ball.Velocity.X;
    double x1 = ball.Position.X;
    ball.Velocity.X = v1 + gVector.X * t;
    ball.Position.X = x1 + v1 * t + 0.5 * gVector.X * t * t;
    if (ball.Position.X < ball.Radius)
    {
        ball.Position.X = ball.Radius;
        if (ball.Velocity.X < 0)
        {
            ball.Velocity.X *= (-elasticRation);
            if (gVector.X < 0 && (-ball.Velocity.X / gVector.X) < (t * 2 * elasticRation))
            {
                ball.Velocity.X = 0;
            }
        }
    }
    if (ball.Position.X > maxX)
    {
        ball.Position.X = maxX;
        if (ball.Velocity.X > 0)
        {
            ball.Velocity.X *= (-elasticRation);
            if (gVector.X > 0 && (-ball.Velocity.X / gVector.X) < (t * 2 * elasticRation))
            {
                ball.Velocity.X = 0;
            }
        }
    }
    #endregion

    #region calculate y values
    gVector.Y *= frictionForce;
    v1 = ball.Velocity.Y;
    double y1 = ball.Position.Y;
    ball.Velocity.Y = v1 + gVector.Y * t;
    ball.Position.Y = y1 + v1 * t + 0.5 * gVector.Y * t * t;
    if (ball.Position.Y < ball.Radius)
    {
        ball.Position.Y = ball.Radius;
        if (ball.Velocity.Y < 0)
        {
            ball.Velocity.Y *= (-elasticRation);
            if (gVector.Y < 0 && (-ball.Velocity.Y / gVector.Y) < (t * 2 * elasticRation))
            {
                ball.Velocity.Y = 0;
            }
        }
    }
    else if (ball.Position.Y > maxY)
    {
        ball.Position.Y = maxY;
        if (ball.Velocity.Y > 0)
        {
            ball.Velocity.Y *= (-elasticRation);
            if (gVector.Y > 0 && (-ball.Velocity.Y / gVector.Y) < (t * 2 * elasticRation))
            {
                ball.Velocity.Y = 0;
            }
        }
    }
    #endregion
    return ball;
}

实验证明，HTC手机的重力感应器还是挺灵敏的，我设置的时间是50毫秒。因为暂时不方便加工小爱，接下来我打算试着做一个简易的两轮平衡小车。

阅读全文 (17,568) 12 条评论

解魔方的机器人攻略22 – 蓝牙通讯

由 动力老男孩 发表于 2010/02/03 00:46:37

前面提到了分辨颜色的三部曲，今天给大家介绍一下NXT和电脑之间的蓝牙通讯。其中在NXT端使用的是Lejos自带的Bluetooth类，在PC端使用的开发工具是VS2008，使用的语言是c#。

有些人鄙视这种连接PC的做法，在他们的眼里，连接了PC以后，乐高就变成了一个遥控玩具。其实对编程开发来说，用Java还是用c#并没有本质的区别。魔方的算法也可以写成Java的版本，无奈的是NXT的内存不足，只能把这种体力活交给电脑了。

1. 蓝牙配对

正所谓千里姻缘一线牵，首先我们要给NXT和PC安排一个相亲大会。NXT已经内置了蓝牙模块，要把它设置成打开并且可见的状态。设置方法请看Lejos的中文教程“蓝牙菜单”。现在很多笔记本也自带了蓝牙模块，如果没有的话，必须买一个蓝牙适配器。注意WinXP开始就都已经自带蓝牙驱动了，如果你的电脑安装了第三方的蓝牙驱动，最好先删除。

蓝牙适配器

蓝牙适配器

准备好定情信物以后，就该安排PC和NXT见面了。PC比较主动，由他开负责寻找：

控制面板中旋转“添加新的蓝牙设备”，可以找到当前可见的NXT

控制面板中旋转“添加新的蓝牙设备”，可以找到当前可见的NXT

找到NXT后，两人会羞答答的先来个握手协议，接下来是交换电话号码。Lejos设置的蓝牙连接密码是1234。

输入蓝牙连接密码

输入蓝牙连接密码

你看他们一个是能力超强，名车豪宅，另一个能歌善舞，秀色可餐。简直就是一拍即合啊。到此牵线完毕，以后他们就可以直接通讯了。我们查看一下电脑上的NXT属性，可以看到有个带“DevB”的端口，这个相当于是他们之间的私人电话，记下来后面会用到。

注意看端口号

注意看端口号

2. C#中使用蓝牙通讯

其实配对以后，蓝牙就被模拟成了一个端口，我们可以用最简单的端口通讯来收发信息。首先，在每次启动时，需要连接端口：

BluetoothConnection = new SerialPort();
ConnectButton.Enabled = false;
BluetoothConnection.PortName = PortList.SelectedItem.ToString();
BluetoothConnection.Open();
BluetoothConnection.ReadTimeout = 10000;
BluetoothConnection.DataReceived += new SerialDataReceivedEventHandler(BlueToothDataReceived);

然后可以通过这个端口来发送信息。需要注意的是，在发送的原始数据之前，需要添加两个表示长度的字节，Byte[0]+Byte[1]*255=length。所以发送数据的函数如下：

private void BlueToothDataSend(byte[] data)
{
    int length = data.Length;
    byte[] readData = new byte[length + 2];
    readData[0] = (byte)(length % 255);
    readData[1] = (byte)(length / 255);
    for (int i = 0; i < length; i++)
    {
        readData[i + 2] = data[i];
    }
    BluetoothConnection.Write(readData, 0, length + 2);
    Status = "发送数据字节数：" + length;
}

收到数据的时候，也是类似的情况，头两个字节表示了数据的长度，然后才是真正的数据内容：

private void BlueToothDataReceived(object o, SerialDataReceivedEventArgs e)
{
    int length = BluetoothConnection.ReadByte();
    length += BluetoothConnection.ReadByte() * 256;

    byte[] data = new byte[length];
    BluetoothConnection.Read(data, 0, length);
    for (int i = 0; i < length; i++)
    {
        BlueToothReceivedData += string.Format("data[{0}] = {1}\r\n", i, data[i]);
    }
}

断开蓝牙连接的命令如下：

BluetoothConnection.Close();
BluetoothConnection.Dispose();
BluetoothConnection = null;

3. Lejos中使用蓝牙通讯

在Lejos中使用蓝牙有几点区别：首先，Lejos中不支持收到消息的事件触发（我怀疑用多线程可以实现，不过对Java不太熟悉，没有调试成功）所以在需要接受PC信息时，只能挂起等候消息传来；其次，虽然PC发来的信息头两个字节表示长度，但是Lejos接收时，是从第三个字节开始显示的；另外，Lejos发送蓝牙信息时，不需要添加那两个字节的长度信息。

下面是建立蓝牙连接的方式：

public static void Connect() throws Exception
{
	LCD.clear();
	LCD.drawString("Waiting BTC...",0,0);
	btc = Bluetooth.waitForConnection();
	LCD.drawString("Connected",0,2);
	LCD.refresh();
	dis = btc.openDataInputStream();
	dos = btc.openDataOutputStream();
}

接受蓝牙信息：

public static byte[] ReadBytes() throws Exception
{
  byte[] buffer = new byte[255];
  int length = btc.read(buffer, buffer.length);
  if(length==-2)
  {
   //lost data, re-sync
   btc.read(null, 255);
   return new byte[0];
  }
  else
  {
   byte[] data = new byte[length];
   for(int i=0;i<length;i++)
   {
    data[i] = buffer[i];
   }
   return data;
  }
}

发送蓝牙信息

public static void WriteBytes(byte[] data) throws Exception
{
 for(int i=0;i<data.length;i++)
 {
  dos.writeByte(data[i]);
 }
 dos.flush();
}

关闭蓝牙连接

public static void Disconnect() throws Exception
{
   if(btc!=null)
   {
    WriteBytes(new byte[]{(byte)255,(byte)255,(byte)255});
    Thread.sleep(100);
    dos.close();
    dis.close();
    btc.close();
   }
}

4. 蓝牙通讯小实验

下面进行一个小实验，在PC上运行一个程序。
当发送1时，NXT初始化魔方底盘位置；
当发送2时，NXT初始化颜色传感器位置；
当发送3时，NXT读取颜色信息，并回传给电脑；
当发送其他数字时，NXT断开蓝牙连接，并退出程序

蓝牙连接通讯实验

蓝牙连接通讯实验

大部分函数在前面都介绍过了，只需要在main函数中指定操作即可：

BlueTooth.Connect();
byte[] colorData = new byte[6];

while(true)
{
 byte[] readData = BlueTooth.ReadBytes();
 if(readData.length > 0)
 {
  int action = readData[0];
   switch(action)
  {
  case 1:
   Robot.FixBasePosition();
   break;
  case 2:
   Robot.FixColorSensorPosition();
   break;
  case 3:
      colorData[0] = (byte) color.getRed();
      colorData[1] = (byte) color.getGreen();
      colorData[2] = (byte) color.getBlue();
      colorData[3] = (byte) (color.getRawRed() / 3);
      colorData[4] = (byte) (color.getRawGreen() / 3);
      colorData[5] = (byte) (color.getRawBlue() / 3);
      BlueTooth.WriteBytes(colorData);
      break;
  default:
   BlueTooth.Disconnect();
   return;
  }
 }
 Thread.sleep(1000);
}

好了，其余部分自己看代码吧，搭车赠送一个生成三维魔方图形的小程序。点此查看运行在NXT中Java源代码代码；点此下载运行在电脑上的C#程序源代码。

阅读全文 (88,217) 25 条评论