pathMotion.cpp

Command a two axis path motion.

/*
 Copyright(c) 1998-2009 by Robotic Systems Integration, Inc. All rights reserved.
 This software contains proprietary and confidential information of Robotic 
 Systems Integration, Inc. (RSI) and its suppliers. Except as may be set forth 
 in the license agreement under which this software is supplied, disclosure, 
 reproduction, or use with controls other than those provided by RSI or suppliers
 for RSI is strictly prohibited without the prior express written consent of 
 Robotic Systems Integration.

 This sample code presumes that the user has set the tuning paramters(PID, PIV, etc.) 
 prior to running this program so that the motor can rotate in a stable manner.

 pathMotion.cpp: Will command a path motion for two axis with different points and desired 
 velocity, acceleration and deceleration specified by user.

 Different positions have to be secified within 'PathListStart' and 'PathListEnd' using
 function 'PathLineAdd' where the coordinates are declared in line_A, line_B and etc.

 Function 'PathRatioSet(ratio)' is used to define different ratios between the two different drive's
 encoder resolution. As the name 'PathMotionStart' suggests will simply execute motion for axes. 

 For any questions regarding this sample code please visit our documentation at www.roboticsys.com


 Warning!  This is a sample program to assist in the integration of your motion 
 controller with your application.  It may not contain all of the logic and safety
 features that your application requires.
*/


#include "rsi.h"

using namespace RSI::RapidCode::SynqNet;

#define X_AXIS          0
#define Y_AXIS          1


void pathMotionMain()
{
  try
  {
    MotionController    *controller ;
    Axis        *axisX ;
    Axis        *axisY ;
    MultiAxis     *multiAxis ;

  
    double line_A[2] = {0, 1000};
    double line_B[2] = {1000,1000};
    double line_C[2] = {1000, 0};
    double line_D[2] = {0, 0};

    double arc_center[2] = {1000,1000};

    // initialize MotionController class
    controller = MotionController::CreateFromBoard(0);  
    
    // initialize Axis classes  
    axisX = controller->AxisGet( X_AXIS);
    axisY = controller->AxisGet( Y_AXIS);
    multiAxis = controller->MultiAxisGet(axisX);
    multiAxis->AxisAdd(axisY);
    
    // make sure all axes are enabled and ready
    multiAxis->ClearFaults();
    multiAxis->AmpEnableSet(true);

    // set the trajectory info
    multiAxis->VectorVelocitySet(1000.0);
    multiAxis->VectorAccelerationSet(10000.0);
    multiAxis->VectorDecelerationSet(10000.0);

    // start path list
    double start_positions[2] = {axisX->CommandPositionGet(),axisY->CommandPositionGet()};
    multiAxis->PathListStart(start_positions);
      
      // turn on blending (smooth corners)
      multiAxis->PathBlendSet(true);
      
      // an X-Y circle
      multiAxis->PathArcAdd(arc_center, 360.0);
      
      // a rectangle
      multiAxis->PathLineAdd(line_A);
      multiAxis->PathLineAdd(line_B);
      multiAxis->PathLineAdd(line_C);
      multiAxis->PathLineAdd(line_D);

    // end path list
    multiAxis->PathListEnd();
    
    // execute the motion
    multiAxis->PathMotionStart();

    // wait for motion to complete
    multiAxis->MotionDoneWait();

  }
  catch (RsiError *err)
  {
    printf("%s\n", err->text);
  }
}