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

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);
}