Collating Unit Controller Code

/*

COPYRIGHT (C) 2004 BY Cerebellum Automation, SAS.

************************************************************************
*                                                                     *
*     The information set forth in this document is the property       *
*     of Cerebellum Automation, SAS. and is to be held in trust and    *
*     confidence.  Publication, duplication, disclosure, or use        *
*     for any purpose not expressly authorized by Cerebellum Auto-     *
*     mation in writing is prohibited.                                 *
*                                                                      *
*     The information in this document is subject to change           *
*     without notice and should not be construed as a commitment       *
*     by Cerebellum Automation.                                        *
*                                                                      *
*     Cerebellum Automation makes no warranty as to the suitability    *
*     of this material for use by the recipient, and assumes no        *
*     responsibility for any consequences resulting from such use.     *
*                                                                     *
************************************************************************

DESCRIPTION:
    2-axis collating unit controller.

   $Log$


*/

#include <math.h>
#include <stdio.h>
#include <string.h>
#include "cde.h"

// IO Parameters.

#define IN_PART          (IOBUS_LOCAL + 10)  // Whether a part as fallen into the pocket or not
#define IN_CAL1          (IOBUS_LOCAL + 11)  // Used to calibrate the collating unit
#define IN_CAL2          (IOBUS_LOCAL + 12)  // Used to calibrate the collating unit
#define IN_USER_OK       (IOBUS_LOCAL + 13)  // The user needs to press this button to power up the system
#define IN_UNLOADED      (IOBUS_LOCAL + 14)  // Used by the top loader to tell us that it unloaded the collating unit
#define OUT_READY_UNLOAD (IOBUS_LOCAL + 10) // Tell the top loader that the collating unit is ready for unload
#define OUT_ERROR        (IOBUS_LOCAL + 11)  // Turns the error light ON

// Collating Unit Parameters.

const UInt32 nbPocket = 10;     // Number of Pockets in one collating train
const UInt32 pocketSize = 50;   // The size of one pocket (distance between 2 pockets) in mm
const double loadPos = 10;      // Position where the first pocket needs to stop to be loaded, in mm
const double unloadPos = 1500;  // Position where the first pocket needs to stop to be unloaded, in mm
const double cycleLength = 3500;// Length of a complete cycle (from loadPos to loadPos) in mm.
                                 // This is used as the roll-over value
const double maxVel = 2.0;      // limit the velocity at 2 m/s

ANY_VAR checkStatus = TRUE, run_appli = FALSE;
BOOL loading[3];                // Flag used to communicate between the 2 trains

/********************************************************
*
* ABSTRACT:    Driver for each collating train.
*      It is a requirement that this task is started when the 2 trains are in
*      their starting position.
*
*/
void CollatingDriver(UInt32 mtr)
{
   UInt32 otherMtr, pocket;

    if (mtr == 1)
        otherMtr = 2;
    else
       otherMtr = 1;

  CA_SetMaxVel(mtr, maxVel);                      // Use full speed

    while (run_appli) {

       // Wait until other motor is loaded
       while (loading[otherMtr]) CA_Wait(.01);

       // Load first train
       loading[mtr] = TRUE;
        for (pocket = 1; pocket <= nbPocket; pocket++) {
            CA_WaitSig(IN_PART, SIG_HIGH, NO_TIMEOUT);
            CA_MoveAbs(mtr, loadPos + pocket * pocketSize, FALSE);
        }

        // Disengage e-gearing for second train
       CA_GearOut(otherMtr);
       loading[mtr] = FALSE;

        // Move first train to unload position
       CA_MoveAbs(mtr, unloadPos, FALSE);
        CA_WaitEndMotion(mtr, NO_TIMEOUT);

        // Signal to the robot that we're ready to unload
       CA_WriteDIO(OUT_READY_UNLOAD, SIG_HIGH);

        // Wait until the robot has unloaded the collating unit
       CA_WaitSig(IN_UNLOADED, SIG_LOW_TO_HIGH, NO_TIMEOUT);
        CA_WriteDIO(OUT_READY_UNLOAD, SIG_HIGH);    // reset flag

       // Adjust the position to keep within acceptable limits
       CA_AdjustMotorPos(mtr, -cycleLength);

        // Engage e-gearing for first train
       CA_GearInAbs(mtr, otherMtr, - nbPocket * pocketSize, 1.0);

        // Move first train around U-Turn at slower speed
       CA_SetMaxVel(mtr, maxVel * 0.5);            // limit speed to 50%
       while (CA_ReadMotorPos(mtr) < (nbPocket * pocketSize))
            CA_Wait(0.1);

        CA_SetMaxVel(mtr, maxVel);                  // Use full speed

    }
}

/* Initialise() *******************************************************
*
* ABSTRACT:    routine run before the main tasks are started.
*
*/

void Initialise()
{
   ANY_VAR mtr, stt;

    // Do some initialization.
   CA_WriteDIO(OUT_READY_UNLOAD, LOW);     // Tell the robot that we're not ready

    if (CA_ReadDIO(IN_PART) == HIGH) {     // Check whether a part is already in the system
       printf("Error! Please remove part from system\n");
       CA_WriteDIO(OUT_READY_UNLOAD, HIGH);
    }

   // Tell the system which axes are used.
   for(mtr = 1; mtr <= 2; mtr++) {
       stt = CA_ConfigureAxis(mtr, TRUE);
        if (stt<0) {
            printf("Error setting up axis %f: %f", mtr, stt);
        }
   }
}

Int32 Calibrate()
{
   ANY_VAR calOffset1 = 0, calOffset2 = 0, timeOut = 0;
    BOOL found1 = FALSE, found2 = FALSE;

    // Move both axis together by the length of one cycle and latch there position
   // in front of the sensor
   CA_SetMaxVel(1, maxVel * 0.05);     // Move at 5% speed
   CA_GearInRel(2, 1, 0.0, 1.0);       // Slave the second motor to the first
   CA_MoveRel(1, cycleLength, FALSE);  // Move the first motor to its starting position

   while (!found1 OR !found2) {
       if (CA_ReadDIO(IN_CAL1)) {
           found1 = TRUE;
            calOffset1 = CA_ReadMotorPos(1);
       }
       if (CA_ReadDIO(IN_CAL2)) {
           found2 = TRUE;
            calOffset2 = CA_ReadMotorPos(2);
       }
       CA_Wait(0.001);

       // if after 100s we haven't seen the sensor, there must be an error.
       if ((timeOut+=0.001) > 100) return ERROR;
    }

   // Make sure there is no gearing while we adjust the speed of the master
   CA_GearOut(2);

    /* Adjust motor position
     */
   CA_AdjustMotorPos(1, -calOffset1);
   CA_AdjustMotorPos(2, -calOffset2);

    // Move first axis to its starting location
   CA_GearInRel(2, 1, 0.0, 1.0);       // Slave the second motor to the first
   CA_SetMaxVel(1, maxVel);            // Move at full speed
   CA_MoveAbs(1, loadPos, FALSE);      // Move the first motor to its starting position

   // Move second axis to its starting location
   CA_WaitEndMotion(1, 5.0);
   CA_GearOut(2);                      // Detach the second axis from the first one
   CA_MoveAbs(1, loadPos - nbPocket * pocketSize, FALSE);
    CA_WaitEndMotion(2, 5.0);

    return SUCCESS;
}

/* controlTask() *******************************************************
*
* ABSTRACT:    main task.
*
*/
void ControlTask()
{
   int stt;

startup:
    /* Start-up / recovery sequence
     */

   CA_WaitSig(IN_USER_OK, SIG_HIGH, NO_TIMEOUT);   // Wait until the user tells us to go on

   /* Enable High Power.
     */
   stt = CA_Power(HIGH);
   if (stt <0 )
        printf("Error enabling High Power : %d\n", stt);
   else
       printf("High Power ON\n");

    Calibrate();

   CA_GearInAbs(2, 1, - nbPocket * pocketSize, 1.0);   // Slave the second motor to the end of the first
   loading[1] = TRUE;
    loading[2] = FALSE;
    run_appli = TRUE;

    /* Main loop
     */
   CA_StartTask(CollatingDriver, 20, 1);
    CA_StartTask(CollatingDriver, 20, 2);

    while (run_appli) CA_Wait(0.1);

   // Something went wrong.
   CA_Power(LOW);
   goto startup;

}

/* CheckStatusTask() *******************************************************
*
* ABSTRACT:    status checking task. Reads the error sensors for each
*          axis and shuts down the system if there is an error.
*
*/
void StatusTask(ANY_VAR argument) {

   ANY_VAR mtr;
    AXIS_ERROR_BITS stt;
    Int32 error=0;

   while (checkStatus) {

       // Read the latched error word for each mtr.
       for(mtr = 1; mtr <= 2; mtr++) {
            // Check if an error occured.
           stt = CA_ReadMtrError(mtr);
           if (stt.reg != 0) {
               run_appli = FALSE;
                error++;
               CA_EStop();
               if (!(error%100)) printf("An error occured on axis %g: %x\n", mtr, (int) stt.reg);
            }
       }

       // Verify the line from the Machine-tool (Digital Input #12)
       if (!CA_ReadDIO(12)){
           run_appli = FALSE;
            error++;
           CA_EStop();
           printf("The machine tool is not ready \n");
       }

        CA_Wait(10);        // check status every 10 ms
   }

}

/* CA_UserProgram() **************************************************************
*
* ABSTRACT:    This is the entry point of the user application program.
*          It initializes all variables and potentially the system and then
*          starts the run time tasks.
*
* INPUT PARAMETERS:
*      none
*
* OUTPUT PARAMETERS:
*
*      none.
*/

void CA_UserProgram()
{
   ANY_VAR control_task_id, status_task_id, priority, robotNum;

    Initialise();
   checkStatus = TRUE; // Set global flag

    priority = 0;   // the highest priority
   robotNum = 1;   // An argument that can be passed to the task

    control_task_id = CA_StartTask( ControlTask, priority, robotNum);
    if (control_task_id < 0)
        printf("The following error occured while creating the task: %f\n", control_task_id);

    priority = 30;  // a lower priority
   robotNum = 1;   // An argument that can be passed to the task

    status_task_id = CA_StartTask( StatusTask, priority, robotNum);
    if (status_task_id < 0)
        printf("The following error occured while creating the task: %f\n", status_task_id);

}