NXT绘图仪测试代码

更新：完整代码见这里。

最近在准备去新加坡马来西亚旅游，NXT绘图仪的更新比较缓慢。先来段绘制正方形的代码，有兴趣的先按NXT绘图仪拼装详解中拼完后，让两个臂成直角，使用NXC加载下面的程序，就可以画出一个四边形了。 如果改一下main函数，就可以绘制你想要的东西了。注意需要使用NXC的固件。

const byte PORT_PEN = OUT_C;
const byte PORT_A = OUT_A;
const byte PORT_B = OUT_B;
const float LEN_ARM = 11;
const float ANGLE2DEGREE = 180 / PI;
const float STEP = 0.01;

// Maths
struct Point {
  float x;
  float y;
};

struct Degrees {
  float a;
  float b;
};

float getD(Point p) {
  return sqrt(p.x*p.x + p.y*p.y);
}

Degrees getDegrees(Point p) {
  Degrees ret;
  float D = getD(p);
  float d = atan2(p.y, p.x) * ANGLE2DEGREE;
  float dp = acos(D/2.0/LEN_ARM) * ANGLE2DEGREE;
  ret.a = d - dp;
  ret.b = 2 * dp - 90;
  return ret;
}

Point getNextPoint(Point cur, Point dest, float step) {
  float dx = dest.x - cur.x;
  float dy = dest.y - cur.y;
  if (dx * dx + dy * dy <= step * step) {
    return dest;
  }
  float a = atan2(dy, dx);
  Point ret;
  ret.x = cur.x + step * cos(a);
  ret.y = cur.y + step * sin(a);
  return ret;
}
// End of Maths


// Basic
Degrees getCurDegrees() {
  Degrees a;
  a.a = MotorRotationCount(PORT_A) / 56.0;
  a.b = MotorRotationCount(PORT_B) / 24.0;
  return a;
}

Point getCurPos() {
  Point p;
  float a = MotorRotationCount(PORT_A) / 56.0 / ANGLE2DEGREE;
  float b = MotorRotationCount(PORT_B) / 24.0 / ANGLE2DEGREE;
  float bp = b + a;
  p.x = LEN_ARM * cos(a) - LEN_ARM * sin(bp);
  p.y = LEN_ARM * sin(a) + LEN_ARM * cos(bp);
  return p;
}

sub moveTo(Point dp) {
  Point next;
  Degrees target;
  Degrees curD;
  Point curPos;
  float da;
  float db;
  while (true) {
    curPos = getCurPos();
    next = getNextPoint(curPos, dp, STEP);
    target = getDegrees(next);
    curD = getCurDegrees();
    da = target.a - curD.a;
    db = target.b - curD.b;
    RotateMotor(PORT_A, 40, da * 56);
    RotateMotor(PORT_B, 40, db * 24);
    if (next.x == dp.x && next.y == dp.y) {
      break;
    }
  }
}

sub penPress() {
  OnFwd(PORT_PEN, 35);
  Wait(50);
  while (MotorRotationCount(PORT_PEN)!=0) {
    ResetRotationCount(PORT_PEN);
    Wait(50);
  }
}

sub penRelease() {
  Coast(PORT_PEN);
}
// End of Basic


task main()
{
  ResetRotationCount(OUT_A);
  ResetRotationCount(OUT_B);
  ResetRotationCount(OUT_C);

  penPress();
  Point cur = getCurPos();
  cur.x = cur.x + 1;
  moveTo(cur);
  cur.y = cur.y + 1;
  moveTo(cur);
  cur.x = cur.x - 1;
  moveTo(cur);
  cur.y = cur.y - 1;
  moveTo(cur);

  penRelease();
}

代码分成几个部分。
一开始是常量的定义，包括了电机端口等定义。
然后是数学运算部分。由于使用了三角函数，需要使用NXC的固件。
再接下来是基础功能，包含了移动画笔，抬笔和下笔三个操作。
最后Main函数分四笔绘制了正方形。

俺下周回来继续更新：
1. 几何模型和计算
2. 优化电机控制
3. 蓝牙与Android通讯