pvtMotion.cpp

Drawing QuadCircle using PVT.

/*  pvtMotion.cpp
 
 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.

 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.


 For any questions regarding this sample code please visit www.roboticsys.com.
 ==================================================================================
 
 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.
 
 This application is designed to demonstrate commanding a Quad Circle movement on
 a two axis system.
 
 */
 

#include "rsi.h"
 
using namespace RSI::RapidCode::SynqNet;
 
#define RSI_BOARD                   (0)
#define AXIS_X                            (0)
#define AXIS_Y                            (1)
#define POINTS                            (3000)  //total points used
#define AXIS_COUNT                        (2)     //two axis computation (X & Y)
#define TIME_SLICE                        (0.01)  //each point processed within 10ms
 
 
void pvtMotionMain()
{
      try
      {
            MotionController  *controller;
            Axis                    *axisX;
            Axis                    *axisY;
            MultiAxis               *multiAxisXY;
 
            // Create and initialize RsiController class (PCI board).
            controller = MotionController::CreateFromBoard(RSI_BOARD);  
            
            // Get Axis X and Y respectively.
            axisX = controller->AxisGet(AXIS_X); 
            axisY = controller->AxisGet(AXIS_Y);
            
        // Initialize an Multi-Axis Mapping by assigning the first axis of the MultiAxis object you wish to create.
            multiAxisXY = controller->MultiAxisGet(axisX);
            multiAxisXY->AxisAdd(axisY);
 
            long radius = 1000;                   //radius of circle
 
            double PI = 3.14159265358979323;  //variable used in equation to convert degrees to radians
 
            double degrees = 90.00/(POINTS); //angle between each point. Used in the equation below.
 
            double position[POINTS * AXIS_COUNT];   //defining size of position array
            double vel[POINTS * AXIS_COUNT];          //defining size of velocity array
            double time[POINTS];                            //defining size of time array
 
            for (int i=0, x = 0, y = 1; i < POINTS; i++)
            {
                  position[x] = (radius*cos((i+1)*degrees*PI/180.0));
                  position[y] = (radius*sin((i+1)*degrees*PI/180.0));
 
                  x = x + 2;
                  y = y + 2;
 
                  time[i] = TIME_SLICE;
            }
 
            for (int i=0, x = 0, y = 1; i < (POINTS-1); i++)
            {
                  vel[x] = (position[x+2] - position[x])/TIME_SLICE;
                  vel[y] = (position[y+2] - position[y])/TIME_SLICE;
 
                  x = x + 2;
                  y = y + 2;
            }
 
            //Final two points (X Axis Final Vel, Y Axis Final Vel) need to be set to 0.
            vel[(POINTS*2)-2] = 0;  //X Axis
            vel[(POINTS*2)-1] = 0;  //Y Axis
 
            multiAxisXY->Abort();
            multiAxisXY->ClearFaults();
            multiAxisXY->AmpEnableSet(true);
 
            axisX->PositionSet(radius);
            axisY->PositionSet(0);
 
            multiAxisXY->MovePVT(position, vel,  time, POINTS, -1, false, true);
            multiAxisXY->MotionDoneWait();
 

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