mx/smargopolo
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
smargopolo/c_algorithms/serialMCS/SmarActMCS.cpp
ROS Station f950537143 Added serialMCS
2019-08-20 18:08:25 +02:00

523 lines
22 KiB
C++
Raw Permalink Blame History

/***********************************************************************
*
* File name: SmarActMCS.cpp
* Author : Wayne Glettig, Marco Salathe
* Version : Dez. 2011
*
* Modul to communicate with the SmarAct MCS. It sends orders for the
* respective movement and recives the current positions and states
*
**********************************************************************/
#define REALTIME // use this if RTAI serial port access is required (is used in SmarActMCS_lib.h)
#include <DLCInterface.hpp>
#include <cmath>
#include <cstdio>
#include "prigoStateClass.h"
#include "SmarActMCS_lib.h"
using namespace std;
INITIALIZE_BEGIN
// create SmarAct MCS Object:
ALLOCATE_EXTRA(0, 1, SmarActMCS);
// create SmarAct MCS Object:
ALLOCATE_EXTRA(1, 1, prigoStateClass *);
// Initialize STATUS variables:
printf("INITIALIZATION\n");
STATUS(0, 0) = 0.0;
STATUS(1, 0) = 0.0;
STATUS(2, 0) = 0.0;
// Default Status of the axes: Uninitialized;
STATUS(3, 0) = 0.0;
STATUS(3, 1) = 0.0;
STATUS(3, 2) = 0.0;
STATUS(3, 3) = 0.0;
STATUS(3, 4) = 0.0;
//Open Serial Port, it stays open and doesn't have to be opened again
EXTRA(0,0).openSerialPort(0,115200);
//disable hand control module (shows still sensor data)
EXTRA(0,0).SHE(2);
INITIALIZE_END
STEP_BEGIN
// Open Serial port in the first step
// if (STEP_NUMBER == 1) EXTRA(0,0).openSerialPort(0,115200);
// Read Inputs:
int err=0;
int I_Modus = (int)INPUT(0, 0);
int I_Selector = (int)INPUT(1, 0);
double I_pos[5] = {INPUT(2, 0),
INPUT(4, 0),
INPUT(6, 0),
INPUT(8, 0),
INPUT(10, 0)};
double I_v[5] = {INPUT(3, 0),
INPUT(5, 0),
INPUT(7, 0),
INPUT(9, 0),
INPUT(11, 0)};
// connect to prigoStateClass (rename pointer to a readable 'ptPrigoStateClass')
EXTRA(1,0) = (prigoStateClass *)(unsigned long)INPUT(12,0);
prigoStateClass * ptPrigoStateClass = EXTRA(1,0);
// Read Status:
int S_subStep = (int)STATUS(1, 0); // Sub Step Counter
int S_currentaxis = (int)STATUS(2, 0); // currentaxis
double S_axisInitOK[5] = {(int)STATUS(3, 0), // status to see if axis0 is initialized
(int)STATUS(3, 1), // status to see if axis1 is initialized
(int)STATUS(3, 2), // status to see if axis2 is initialized
(int)STATUS(3, 3), // status to see if axis3 is initialized
(int)STATUS(3, 4)}; // status to see if axis4 is initialized
double S_sensorfeedback[5] = {(int)STATUS(4, 0), // status to hold axis0 sensor feedback
(int)STATUS(4, 1), // status to hold axis1 sensor feedback
(int)STATUS(4, 2), // status to hold axis2 sensor feedback
(int)STATUS(4, 3), // status to hold axis3 sensor feedback
(int)STATUS(4, 4)}; // status to hold axis4 sensor feedback
double S_axisStatus[5] = {(int)STATUS(5, 0), // status to hold axis0 status (MCS)
(int)STATUS(5, 1), // status to hold axis1 status (MCS)
(int)STATUS(5, 2), // status to hold axis2 status (MCS)
(int)STATUS(5, 3), // status to hold axis3 status (MCS)
(int)STATUS(5, 4)}; // status to hold axis4 status (MCS)
// Read PARAMs:
int P_MCS_connected = (int)PARAM("MCS_connected", 0);
// On Modus Change set subStep to 0.
int S_lastModus = (int)STATUS(0, 0);
if (I_Modus != S_lastModus) {
S_subStep = 0;
}
// Modus Switch
switch(I_Modus) {
/***********************************************************************
* Mode 1: READY -> Move positioners corresponding to SOLL positions
**********************************************************************/
case 1:
if (S_subStep < 10) {
S_subStep++;
} else {
if(I_Selector<5){
// moving the stages
int pos,v;
pos = (int)(I_pos[I_Selector]*1000000);
v = (int)(fabs(I_v[I_Selector])*1000000);
// an attempt for safety:
ptPrigoStateClass->setCurrentWrite(I_Selector, pos, v, STEP_NUMBER);
if (ptPrigoStateClass->chkOkToWrite(I_Selector)) {
if (I_Selector == 4.) {
// move phimotor and set speed
err=EXTRA(0,0).SCLS(I_Selector, v);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).MAA(I_Selector, pos, 60000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
} else {
// move sliders and set speed
err=EXTRA(0,0).SCLS(I_Selector, v);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).MPA(I_Selector, pos, 60000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
}
}
} else {
// updating values
if (S_subStep == 10){
// Send request for current position
err=EXTRA(0,0).sendGP(0);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGP(1);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGP(2);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGP(3);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGA(4);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep=11.;
} else {
// read current position and update prigoStateClass
double position=0;
err=EXTRA(0,0).setTimestamp(STEP_NUMBER);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).readGP(0.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets1(position/1000000.);
S_sensorfeedback[0]=position;
err=EXTRA(0,0).readGP(1.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets2(position/1000000.);
S_sensorfeedback[1]=position;
err=EXTRA(0,0).readGP(2.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets3(position/1000000.);
S_sensorfeedback[2]=position;
err=EXTRA(0,0).readGP(3.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets4(position/1000000.);
S_sensorfeedback[3]=position;
err=EXTRA(0,0).readGA(4.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->setphimotor(position/1000000.);
S_sensorfeedback[4]=position;
S_subStep=10.;
}
}
}
break;
/***********************************************************************
* Mode 2 and 3: INITIALIZING
* 2: FRM (Find Reference Mark in POS direction)
* 3: FRM (Find Reference Mark in NEG direction)
**********************************************************************/
case 2:
case 3:
int dir;
dir = I_Modus-2; // Modus 2 -> dir=0(POS) ; Modus 3 -> dir=1(NEG);
// First Stop all motion.
if (S_subStep == 0) {
err=EXTRA(0,0).S(); // :STOP
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets1_PPK(0.); // Set all PPK Variables to 0.
ptPrigoStateClass->sets2_PPK(0.);
ptPrigoStateClass->sets3_PPK(0.);
ptPrigoStateClass->sets4_PPK(0.);
ptPrigoStateClass->setphimotor_PPK(0.);
S_subStep++;
} else if (S_subStep == 1) { // :SCLS0,5000000
err=EXTRA(0,0).SCLS(0, 5000000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 2) { // :SCLS0,5000000
err=EXTRA(0,0).SCLS(1, 5000000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 3) { // :SCLS0,5000000
err=EXTRA(0,0).SCLS(2, 5000000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 4) { // :SCLS0,5000000
err=EXTRA(0,0).SCLS(3, 5000000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 5) { // :SCLS0,5000000
//err=EXTRA(0,0).SCLS(4, 5000000);
//if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 6) {
// then send Command to find reference mark Channel0
err=EXTRA(0,0).FRM(0,dir,60000,0); // :FRM0,0,60000,0
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 7) {
// then send Command to find reference mark Channel1
err=EXTRA(0,0).FRM(1,dir,60000,0); // :FRM1,0,60000,0
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 8) {
// then send Command to find reference mark Channel2
err=EXTRA(0,0).FRM(2,dir,60000,0); // :FRM2,0,60000,0
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 9) {
// then send Command to find reference mark Channel3
err=EXTRA(0,0).FRM(3,dir,60000,0); // :FRM3,0,60000,0
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 10) {
// then send Command to find reference mark Channel4
//err=EXTRA(0,0).FRM(4,0,60000,0); // :FRM4,0,60000,0, negative direction always so that phi cable doesn't wind up
//if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 11) {
// now start polling MCS, to see what the axis are doing
S_currentaxis = 0;
S_subStep++;
} else if (S_subStep == 12) {
// Send "physical position known?" request
err=EXTRA(0,0).sendGPPK(S_currentaxis);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
// Send status request
err=EXTRA(0,0).sendGS(S_currentaxis);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep > 12 && S_subStep < 32) {
// then wait a couple of steps (20)...
S_subStep++;
} else if (S_subStep == 32) {
double status;
err=EXTRA(0,0).setTimestamp(STEP_NUMBER);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
// Get "physical position known" reply
err=EXTRA(0,0).readGPPK(S_currentaxis,&status);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
// update Axis status flag
S_axisInitOK[S_currentaxis] = status;
printf ("Axis %d: PPK %.0f, ",S_currentaxis, status);
// Get status reply
err=EXTRA(0,0).readGS(S_currentaxis,&status);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
// update Axis status flag
S_axisStatus[S_currentaxis] = status;
printf ("Move status %.0f\n", status);
S_subStep++;
} else if (S_subStep == 33) {
// Select next currentaxis (until 4) (like for(i=0;i<4;i++))
if (S_currentaxis < 4){
S_currentaxis++;
S_subStep=12; // Loop back to subStep 12
} else {
// if 5th axis (axis4) is reached move on.
S_subStep++;
}
} else if (S_subStep == 34) {
// check if all axis are initialized
if (S_axisInitOK[0] == 1 && S_axisStatus[0] == 3 &&
S_axisInitOK[1] == 1 && S_axisStatus[1] == 3 &&
S_axisInitOK[2] == 1 && S_axisStatus[2] == 3 &&
S_axisInitOK[3] == 1 && S_axisStatus[3] == 3) {
//S_axisInitOK[4] == 1 && S_axisStatus[4] == 3) {
// INITIALIZATION SUCCESSFUL - REPORT TO prigoCOMMAND!!!
ptPrigoStateClass->sets1_PPK(1.);
ptPrigoStateClass->sets2_PPK(1.);
ptPrigoStateClass->sets3_PPK(1.);
ptPrigoStateClass->sets4_PPK(1.);
ptPrigoStateClass->setphimotor_PPK(1.);
printf("Found Absolute Reference.\n");
S_subStep++;
}
else if (S_axisInitOK[0] == 1 && S_axisStatus[0] == 0) ALARM_WITH_MESSAGE(50, "SmarActMCS: Axis 1 has Status STOP");
else if (S_axisInitOK[1] == 1 && S_axisStatus[1] == 0) ALARM_WITH_MESSAGE(50, "SmarActMCS: Axis 2 has Status STOP");
else if (S_axisInitOK[2] == 1 && S_axisStatus[2] == 0) ALARM_WITH_MESSAGE(50, "SmarActMCS: Axis 3 has Status STOP");
else if (S_axisInitOK[3] == 1 && S_axisStatus[3] == 0) ALARM_WITH_MESSAGE(50, "SmarActMCS: Axis 4 has Status STOP");
//else if (S_axisInitOK[4] == 1 && S_axisStatus[4] == 0) ALARM_WITH_MESSAGE(50, "SmarActMCS: Axis 5 has Status STOP");
else {
S_subStep=11; // Loop back to subStep 11
}
} else if (S_subStep > 34 && S_subStep < 55) {
// then wait a couple of steps (20)...
S_subStep++;
} else if (S_subStep == 55) {
// Drive to initial position.
int pos = (int)(I_pos[0]*1000000);
err=EXTRA(0,0).SCLS(0, 2000000); // this is the SPEED to use to drive to initial position
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).MPA(0, pos, 60000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 56) {
int pos = (int)(I_pos[1]*1000000);
err=EXTRA(0,0).SCLS(1, 2000000); // this is the SPEED to use to drive to initial position
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).MPA(1, pos, 60000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 57) {
int pos = (int)(I_pos[2]*1000000);
err=EXTRA(0,0).SCLS(2, 2000000); // this is the SPEED to use to drive to initial position
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).MPA(2, pos, 60000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 58) {
int pos = (int)(I_pos[3]*1000000);
err=EXTRA(0,0).SCLS(3, 2000000); // this is the SPEED to use to drive to initial position
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).MPA(3, pos, 60000);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 59) {
//int pos = (int)(I_pos[4]*1000000);
//err=EXTRA(0,0).SCLS(4, 40000000); // this is the SPEED to use to drive to initial position
//if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
//err=EXTRA(0,0).MAA(4, pos, 60000);
//if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 60) {
// request to Read current position
err=EXTRA(0,0).sendGP(0);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 61) {
err=EXTRA(0,0).sendGP(1);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 62) {
err=EXTRA(0,0).sendGP(2);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 63) {
err=EXTRA(0,0).sendGP(3);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 64) {
err=EXTRA(0,0).sendGA(4);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 65) {
// Read current position reply
double position=0;
err=EXTRA(0,0).setTimestamp(STEP_NUMBER);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).readGP(0.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets1(position/1000000.); // update prigoStateClass
err=EXTRA(0,0).readGP(1.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets2(position/1000000.); // update prigoStateClass
err=EXTRA(0,0).readGP(2.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets3(position/1000000.); // update prigoStateClass
err=EXTRA(0,0).readGP(3.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets4(position/1000000.); // update prigoStateClass
err=EXTRA(0,0).readGA(4.,&position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->setphimotor(position/1000000.); // update prigoStateClass
// See if position is reached:
if ((fabs(I_pos[0]-ptPrigoStateClass->s1.value)<0.001) &&
(fabs(I_pos[1]-ptPrigoStateClass->s2.value)<0.001) &&
(fabs(I_pos[2]-ptPrigoStateClass->s3.value)<0.001) &&
(fabs(I_pos[3]-ptPrigoStateClass->s4.value)<0.001) ) S_subStep++;
else S_subStep = 60;
} else if (S_subStep == 66) {
// If position reached, change MODE to 2.
ptPrigoStateClass->setMODE(2);
printf("YEAH INITALiZATION SUCCESSFUL\n");
S_subStep++;
} else if (S_subStep == 67) {
// wait
}
break;
/***********************************************************************
* Mode 0: UNINIZIALIZED/IDLE/ERROR -> movements disabled
**********************************************************************/
default: //Modus 0 or other: DISABLE
// First Stop all motion.
if (S_subStep < 20) {
ptPrigoStateClass->sets1_PPK(0.); // Set all PPK Variables to 0.
ptPrigoStateClass->sets2_PPK(0.);
ptPrigoStateClass->sets3_PPK(0.);
ptPrigoStateClass->sets4_PPK(0.);
ptPrigoStateClass->setphimotor_PPK(0.);
S_subStep++;
} else if (S_subStep == 20) {
err=EXTRA(0,0).SCM(1);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).clearBuf();
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 21) {
// then stop motion on all channels
err=EXTRA(0,0).S(); // :STOP
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep++;
} else if (S_subStep == 22 || S_subStep == 23) {
// then start polling MCS, to see if MCS is still attached.
// poll every half second
double pollperiod = 0.1;
if (STEP_NUMBER%(int)(pollperiod/PERIOD/2)==0) {
if (S_subStep == 22) {
// Send status request
err=EXTRA(0,0).sendGP(0);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGP(1);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGP(2);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGP(3);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).sendGA(4);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
S_subStep = 23;
} else {
// Get Status reply
double position;
err=EXTRA(0,0).setTimestamp(STEP_NUMBER);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
err=EXTRA(0,0).readGP(0, &position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets1(position/1000000.); // update prigoStateClass
err=EXTRA(0,0).readGP(1, &position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets2(position/1000000.); // update prigoStateClass
err = EXTRA(0,0).readGP(2, &position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets3(position/1000000.); // update prigoStateClass
err = EXTRA(0,0).readGP(3, &position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->sets4(position/1000000.); // update prigoStateClass
err = EXTRA(0,0).readGA(4, &position);
if (err) ALARM_WITH_MESSAGE(err, "SmarActMCS: Connection error");
ptPrigoStateClass->setphimotor(position/1000000.); // update prigoStateClass
S_subStep = 22;
}
}
}
break;
}
// STATUS
STATUS(0, 0) = I_Modus;
STATUS(1, 0) = S_subStep;
STATUS(2, 0) = S_currentaxis;
STATUS(3, 0) = S_axisInitOK[0];
STATUS(3, 1) = S_axisInitOK[1];
STATUS(3, 2) = S_axisInitOK[2];
STATUS(3, 3) = S_axisInitOK[3];
STATUS(3, 4) = S_axisInitOK[4];
STATUS(4, 0) = S_sensorfeedback[0];
STATUS(4, 1) = S_sensorfeedback[1];
STATUS(4, 2) = S_sensorfeedback[2];
STATUS(4, 3) = S_sensorfeedback[3];
STATUS(4, 4) = S_sensorfeedback[4];
STATUS(5, 0) = S_axisStatus[0];
STATUS(5, 1) = S_axisStatus[1];
STATUS(5, 2) = S_axisStatus[2];
STATUS(5, 3) = S_axisStatus[3];
STATUS(5, 4) = S_axisStatus[4];
// PARAMs:
PARAM("MCS_connected", 0) = P_MCS_connected;
// OUTPUTs:
OUTPUT(0, 0) = ptPrigoStateClass->s1.value;
OUTPUT(1, 0) = ptPrigoStateClass->s2.value;
OUTPUT(2, 0) = ptPrigoStateClass->s3.value;
OUTPUT(3, 0) = ptPrigoStateClass->s4.value;
OUTPUT(4, 0) = ptPrigoStateClass->phimotor.value;
OUTPUT(5, 0) = PARAM("MCS_connected", 0);
STEP_END
FINALIZE_BEGIN
// Setting all the close loop speeds to 0 (default), otherwise axis might not move correctly in close loop mode
for (int i=0; i<5;i++){
EXTRA(0,0).SCLS(i, 0);
usleep(10000);
};
// enable the hand control module
EXTRA(0,0).SHE(1);
usleep(10000);
// close port
EXTRA(0,0).closeSerialPort();
DEALLOCATE_EXTRA(0);
DEALLOCATE_EXTRA(1);
FINALIZE_END
SAFETY_BEGIN
//treat here all SmarAct related errors and what actions should be taken
if (ALARM_CODE > 1 && ALARM_CODE < 100){
ALARM_RESUME_RESET;
}
SAFETY_END
Reference in New Issue View Git Blame Copy Permalink