Bye Bye Moore

猫マンション建築の野望を胸に零細事業主として資本主義の荒波に漕ぎ出したアラサー男の技術メモ

オープンソースな実用ロボットアームTsukArm その7:動きを関数として分離

shuzo-kino.hateblo.jp

実際のところ

C++クソ雑魚マンなので苦労しましたが……
グローバルで使えるインスタンス作っておいて、関数側で呼び出すような形になるっぽく。
JobBuilderは関数の中で作ってあげれば良いようです。

#include <Angle.h>
#include <BezierFunction.h>
#include <BezierStepper.h>
#include <CartesianJob.h>
#include <CartesianPoint.h>
#include <Constant.h>
#include <Function.h>
#include <GripperController.h>
#include <HeadRotation.h>
#include <Job.h>
#include <JobBuilder.h>
#include <JobQueue.h>
#include <LinearFunction.h>
#include <LinearRotator.h>
#include <PolarPoint.h>
#include <ReferenceRotator.h>
#include <RobotArm.h>
#include <RotateMatrix.h>
#include <Rotator.h>
#include <RoundFunction.h>
#include <ServoPoint.h>
#include <Square.h>
#include <Stepper.h>

JobQueue *queue; //ジョブキュー
RobotArm *arm;   //ロボットアームのパラメーター

void jobCalibrate(){
    JobBuilder builder(arm);

      // スタートポジションに行く
    builder.setTargetPosition(0.0, 130.0, 105.0);
    builder.setInterval(2000);
    builder.setTargetPitch(90);
    builder.setGripper(0.0);
    queue->enqueue(builder.build());
    
    // 右に振る
    builder.setMoving(MOVE_BEZIER);
    builder.setTargetPosition(120.0, 80.0, 130.0);
    builder.setMiddlePosition(MID_FIRST,  0.0, 150.0, 105.0);
    builder.setMiddlePosition(MID_SECOND, 80.0, 80.0, 130.0);
    
    builder.setTargetPitch(90);
    builder.setRollSystem(GLOBAL);
    queue->enqueue(builder.build());
    
    // 左に振る
    builder.setMoving(MOVE_BEZIER);
    builder.setTargetPosition(-120.0, 80.0, 130.0);
    builder.setMiddlePosition(MID_FIRST,  120.0, 160.0, 130.0);
    builder.setMiddlePosition(MID_SECOND, -120.0, 160.0, 130.0);
    builder.setRollSystem(GLOBAL);
    queue->enqueue(builder.build());
    
    // スタートポジションに戻す
    builder.setTargetPosition(0.0, 130.0, 105.0);
    builder.setTargetPitch(0);
    queue->enqueue(builder.build());
    
    // 上に振る
    builder.setMoving(MOVE_BEZIER);
    builder.setTargetPosition(0.0, 100.0, 160.0);
    builder.setMiddlePosition(MID_FIRST,  0.0, 160.0, 200.0);
    builder.setMiddlePosition(MID_SECOND, 0.0, 100.0, 200.0);
    
    builder.setPitchSystem(GLOBAL);
    queue->enqueue(builder.build());
    
    // 下に振る
    builder.setMoving(MOVE_BEZIER);
    builder.setTargetPosition(0.0, 150.0, 0.0);
    builder.setMiddlePosition(MID_FIRST,  0.0, 160.0, 200.0);
    builder.setMiddlePosition(MID_SECOND, 0.0, 180.0, 60.0);
    
    builder.setPitchSystem(GLOBAL);
    queue->enqueue(builder.build());
    
    // スタートポジションに戻す
    builder.setTargetPosition(0.0, 130.0, 105.0);
    builder.setPitchSystem(GLOBAL);
    queue->enqueue(builder.build());
    
    // yawを右に
    builder.setTargetYaw(-90);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // yawを左に
    builder.setTargetYaw(90);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // yawを右に
    builder.setTargetYaw(-90);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // スタートポジションに戻す
    builder.setTargetYaw(0);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // グリッパ開く
    builder.setGripper(1.0);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // グリッパ閉じる
    builder.setGripper(0.0);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // グリッパ開く
    builder.setGripper(1.0);
    builder.setMovingTime(250);
    builder.setInterval(0);
    queue->enqueue(builder.build());
    
    // グリッパ閉じる
    builder.setGripper(0.0);
    builder.setMovingTime(250);
    queue->enqueue(builder.build());
    
    // スタートポジションに戻す
    builder.setTargetPosition(0.0, 100.42, 97.51);
    builder.setTargetPitch(0);
    queue->enqueue(builder.build());
  return;
}

void setup() {
  Serial.begin(115200);
  Serial.println("INIT START");

  arm = new RobotArm(); //ロボットの初期化
  
  // サーボの角度を微調整
  int of[] = {-3, -10, 0, -3, 0, 0, 0};
  arm->offset(of);
  // 初期姿勢をとらせる
  arm->initialMove();

  /*
    TsukArm Libraryでは、動作単位をJobとしている。
    JobBuilderを使ってJobを構築し、それをJobQueueに格納する。
    JobQueueをDELAY_INTERVAL(=10ms)ごとにexecuteすると、キュー内部のJobを順に実行していく。
   */
  queue = new JobQueue();

  jobCalibrate();
  
  delay(1000);
  Serial.println("INIT END");
}

void loop() {
  // JobQueueを実行する
  queue->execute();
  delay(FRAME_INTERVAL);
}