NXT绘图仪测试代码

更新:完整代码见这里

最近在准备去新加坡马来西亚旅游,NXT绘图仪的更新比较缓慢。先来段绘制正方形的代码,有兴趣的先按NXT绘图仪拼装详解中拼完后,让两个臂成直角,使用NXC加载下面的程序,就可以画出一个四边形了。 :D 如果改一下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通讯

此条目发表在LEGO, MOC, Technic & NXT分类目录,贴了, , , , , , 标签。将固定链接加入收藏夹。

NXT绘图仪测试代码》有 6 条评论

  1. genghis说:

    很好。咨询一下,代码用什么开发环境编译运行啊?

  2. Robin说:

    是那个版本的rom,是lms_arm_nbcnxc_107.rfw,还是lms_arm_nbcnxc_128.rfw?

  3. Robin说:

    是那个版本的rom?是lms_arm_nbcnxc_107.rfw吗?还是其它?

发表评论

电子邮件地址不会被公开。 必填项已用*标注

您可以使用这些HTML标签和属性: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>