スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。


にほんブログ村 その他趣味ブログ LEGO・ブロックへ

お絵かきロボ - プログラミング編 (4/4)

お絵かきロボのRobotCのプログラミングは以下になります。
task main(){
  const float DEG_LIMIT=100;
  const float MOTORPOWER_MAX=100;
  const float R1=106.4927;
  const float R2=131.2091;
  float x, y;
  float r, t;
  float u0=PI/2, v0=0.0, u1, v1;
  float Du, Dv, Dg, DgAbs, Dratio;
  float destDegree;
  float motorPower;
  float ratioPlus;

  float xy_a[][2] = {{0,-100},{-53.6949,73.9219},{0,-10},{-51.0475,84.8523},{-53.0330,88.8917},{-54.5773,102.6735},{-56.3423,105.7625},{-56.3423,108.6139},{-53.4743,107.9011},{-50.3856,103.1487},{-48.1795,97.4459},{-45.0908,97.9212},{-43.1053,96.7331},{-41.1198,91.2679},{-42.4434,83.1889},{-43.7671,82.4761},{-45.5321,90.3174},{-45.5321,93.4065},{-47.9588,95.0698},{-49.5032,86.2780},{-49.9444,74.6347},{-51.2681,71.5457},{-53.9155,72.2586},{-53.6949,73.9219},{0,-100},{-64.9463,76.5357},{0,-10},{-62.2989,82.4761},{-66.2700,88.8917},{-66.2700,95.0698},{-68.4761,97.2083},{-66.4906,99.1093},{-59.4309,98.1588},{-56.1216,92.4560},{-54.1361,90.5551},{-56.1216,84.3770},{-64.9463,76.5357},{0,-100},{-87.2285,107.9011},{0,-10},{-89.4347,116.6929},{-91.1996,118.3562},{-90.7584,119.0691},{-88.3316,118.3562},{-87.0079,115.5048},{-87.2285,107.9011},{0,-100},{-88.1110,67.2686},{0,-10},{-89.8759,72.7338},{-90.0965,75.5852},{-88.3316,74.8724},{-87.0079,71.3081},{-88.1110,67.2686},{0,-100},{-97.3769,101.0102},{0,-10},{-101.1273,104.3368},{-100.2449,114.3167},{-98.9212,115.0296},{-97.3769,101.0102},{0,-100},{-97.8181,62.9915},{0,-10},{-98.2593,75.8228},{-100.2449,72.7338},{-100.6861,68.9319},{-99.8036,64.8925},{-97.8181,62.9915},{0,-100},{-146.7949,75.5852},{0,-10},{-147.6773,79.3871},{-149.6629,91.2679},{-146.7949,108.6139},{-139.0733,116.6929},{-132.0136,120.9700},{-123.8508,125.0095},{-119.6591,130.2371},{-118.7766,131.6628},{-105.9809,134.7518},{-104.4366,126.9104},{-104.2160,111.2277},{-106.4221,111.9406},{-105.0984,109.8020},{-105.5397,101.9607},{-105.0984,95.7826},{-98.4799,90.0798},{-97.1562,90.0798},{-92.0821,90.0798},{-85.4636,91.7431},{-87.4491,94.3569},{-86.5667,95.0698},{-87.6697,96.4955},{-92.7439,96.9707},{-96.2738,106.7130},{-92.0821,109.3268},{-93.1852,118.1186},{-92.0821,120.4948},{-97.5975,133.8013},{-94.5088,137.3656},{-90.5378,137.3656},{-77.7420,133.3261},{-69.7998,127.1480},{-61.8577,123.5838},{-54.7979,120.7324},{-43.5465,117.4058},{-38.0311,111.9406},{-34.5013,104.8121},{-28.7652,93.6441},{-28.3240,86.2780},{-31.1920,80.3375},{-36.4868,74.6347},{-42.6641,69.6448},{-47.5176,64.1796},{-52.8124,61.5658},{-61.6370,61.3282},{-70.9029,57.0511},{-80.1688,54.9126},{-86.7873,54.6749},{-94.5088,56.5759},{-92.7439,63.2291},{-95.8325,67.2686},{-92.0821,69.6448},{-93.4058,78.6742},{-94.9501,78.1990},{-93.4058,81.2880},{-99.1418,79.8623},{-104.6572,74.8724},{-103.1129,68.6943},{-102.0098,57.0511},{-102.6716,56.1006},{-108.4077,55.8630},{-120.5415,57.7639},{-122.5271,58.9520},{-127.1600,58.9520},{-135.5434,61.5658},{-138.4115,64.8925},{-139.5145,63.2291},{-143.9269,68.2191},{-146.7949,75.5852},{0,-100},{-120.6985,44.6602},{0,-10},{-128.4837,47.7840},{-150.9866,60.1401},{-164.2235,72.4962},{-172.1657,87.2284},{-173.9306,113.3663},{-167.7534,138.5536},{-155.3989,156.6125},{-146.8181,162.6882},{0,-100}};

  for (int i=0; i<153; ++i){
    x = xy_a[i][0];
    y = xy_a[i][1];

    if( y < -9 ){
      int upDown = ( y < -99 ) ? -1 : 1;
      float penDegree = ( y < -99 ) ? 200 : 300;
      nMotorEncoder[motorA] = 0;
      wait1Msec(100);
      nMotorEncoderTarget[motorA] = -penDegree;
      motor[motorA] = upDown * 20;
      while(nMotorRunState[motorA] != runStateIdle && nMotorRunState[motorA] != runStateHoldPosition){
      }
      motor[motorA] = 0;
      continue;
    }

    r = sqrt(x*x+y*y);
    t = (abs(x) < 0.000001) ? PI/2 : atan(y/x);
    if( t < 0 ) t += PI;

    v1 = acos((r*r-R1*R1-R2*R2)/(2*R1*R2));
    float degB = atan(R2*sin(v1)/(R1+R2*cos(v1)));
    u1 = (degB < 0) ? t-(degB+PI) : t-degB;

    Du = u1 - u0;
    Dv = v1 - v0;
    Dg = (abs(Du) > abs(Dv)) ? Du : Dv;
    DgAbs = abs(Dg);
    destDegree = radiansToDegrees(DgAbs) * 56;
    motorPower = sgn(Dg) * MOTORPOWER_MAX;
    Dratio = (abs(Du) > abs(Dv)) ? Dv/Dg : Du/Dg;
    ratioPlus = (abs(Dratio * destDegree) < 100) ? 1.0 : 0.0;

    if( destDegree < DEG_LIMIT ) continue;

    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorC] = 0;
    wait1Msec(100);

    if( abs(Du) > abs(Dv) ){
      nMotorEncoderTarget[motorC] = -destDegree;
      nSyncedMotors = synchCB;
      nSyncedTurnRatio = Dratio * 100 + ratioPlus;
      motor[motorC] = motorPower;
      while(nMotorRunState[motorC] != runStateIdle && nMotorRunState[motorC] != runStateHoldPosition){
      }
    }else{
      nMotorEncoderTarget[motorB] = -destDegree;
      nSyncedMotors = synchBC;
      nSyncedTurnRatio = Dratio * 100 + ratioPlus;
      motor[motorB] = motorPower;
      while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition){
      }
    }
    motor[motorB] = 0;
    motor[motorC] = 0;
    nSyncedMotors = synchNone;

    u0 = u1;
    v0 = v1;
    wait1Msec(100);
  }
}

ポイントをいくつか。
・14行目が先日のエントリーの最終産物です。16行目の153という点の数も。
・20行目から31行目は、xy座標が{0,-100}の時はペン先を上げて、{0,-10}の時はペン先を下げています。
・33行目から42行目は、xy座標からuvの計算で、uvの増減すべき値の分だけモーターを回転させます。
・43行目から73行目は、uとvのどちらを大きく動かすかを判断して、小さい方を大きい方にあわせて同期させます。
残りの部分は、以前のエントリーを参考にしています。

次回は製作途中の動画を公開します。
スポンサーサイト


にほんブログ村 その他趣味ブログ LEGO・ブロックへ

コメントの投稿

非公開コメント

リンク
フリーエリア
プロフィール

tsukuba.lego

Author:tsukuba.lego
●主な登場人物
koba : 会長. RCX, WeDo, NXT1.0, NXT2.0を持つ.
matk : 副会長. 2010年夏にNXT2.0を購入, 20年来のレゴ熱復活.
●会員募集中です!小学生から歓迎. 近日月1ぐらいで集まる予定.
mail:tsukuba.lego@gmail.com
YouTube (tsukubalego)
Twitter(tsukubalego)

カテゴリ
最新記事
最新コメント
月別アーカイブ
検索フォーム
RSSリンクの表示
ブロとも申請フォーム

この人とブロともになる

QRコード
QR
最新トラックバック
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。