Dario Sassi
d1f365710e
- modifiche varie per feed e accelerazioni calibrate e separate per ogni asse - aggiunto comando pausa G4 Fxxx (millisecondi).
660 lines
22 KiB
C++
660 lines
22 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2025-2025
|
|
//----------------------------------------------------------------------------
|
|
// File : ArctosRobotCnc.cpp Data : 22.05.25 Versione : 2.7e2
|
|
// Contenuto : Controllo numerico per robot Arctos con comunicazione SLCAN.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 22.05.25 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "serial.h"
|
|
#include "slcan.h"
|
|
#include "logger.h"
|
|
#include "/EgtDev/Include/EGnGetModuleVer.h"
|
|
#include "/EgtDEv/Include/EGnScanner.h"
|
|
#include "/EgtDev/Include/EgtIniFile.h"
|
|
#include "/EgtDev/Include/EgtStringConverter.h"
|
|
#include <conio.h>
|
|
|
|
using namespace std ;
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
const int MAX_AXES = 6 ;
|
|
const double DELTA_ANG_MIN = 0.001 ;
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static string s_sTempDir ;
|
|
static string s_sProgDir ;
|
|
static string s_sFileIni ;
|
|
static slcan_port_t s_port = NULL ;
|
|
static int s_nAxesCnt = 0 ;
|
|
static double s_dAxisKmax = 1 ;
|
|
static DBLVECTOR s_vdAxisK = { 1, 1, 1, 1, 1, 1} ;
|
|
static DBLVECTOR s_vdAxisComp = { 1, 1, 1, 1, 1, 1} ;
|
|
static INTVECTOR s_vnAxisAcc = { 2, 2, 2, 2, 2, 2} ;
|
|
static DBLVECTOR s_vdAxisPos = { 0, 0, 0, 0, 0, 0} ;
|
|
static int s_nFeed = 100 ;
|
|
static int s_nRapidFeed = 1000 ;
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
struct GMove {
|
|
string sLine ;
|
|
int nLine ;
|
|
int nG ;
|
|
int nMask ;
|
|
double vdAng[MAX_AXES] ;
|
|
int nFeed ;
|
|
GMove( void) : nLine( 0), nG( 0), nMask( 0), vdAng{ 0, 0, 0, 0, 0 ,0}, nFeed( 0) {}
|
|
} ;
|
|
typedef std::vector<GMove> GMOVEVECTOR ;
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static bool
|
|
CalcCRC( slcan_message_t& slmMsg)
|
|
{
|
|
if ( slmMsg.can_dlc == 0 || slmMsg.can_dlc > 8)
|
|
return false ;
|
|
unsigned int unCrc = slmMsg.can_id ;
|
|
for ( int i = 0 ; i < slmMsg.can_dlc - 1 ; ++ i)
|
|
unCrc += slmMsg.data[i] ;
|
|
slmMsg.data[slmMsg.can_dlc - 1] = ( unCrc & 0xFF) ;
|
|
return true ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static bool
|
|
IsPowerOfTwo( int nVal)
|
|
{
|
|
return ( ( nVal > 0) && ( ( nVal & ( nVal - 1)) == 0)) ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
OpenSLCAN( void)
|
|
{
|
|
// Log versione Api SLCAN
|
|
log_printf( "%s\n", slcan_api_version( NULL, NULL, NULL)) ;
|
|
|
|
// Apertura porta SLCAN
|
|
int nQueueSize = GetPrivateProfileInt( "SLCAN", "Buffer", 8, s_sFileIni.data()) ;
|
|
s_port = slcan_create( (unsigned) nQueueSize) ;
|
|
if ( s_port == NULL) {
|
|
log_printf( "Error: slcan_create returned NULL (%i)\n", errno) ;
|
|
return -4 ;
|
|
}
|
|
string sPort = GetPrivateProfileStringUtf8( "SLCAN", "Port", "COM4", s_sFileIni.data()) ;
|
|
int nRes = slcan_connect( s_port, sPort.data(), NULL) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_connect returned %i (%i)\n", nRes, errno) ;
|
|
return -3 ;
|
|
}
|
|
nRes = slcan_set_ack( s_port, false) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_set_ack false returned %i (%i)\n", nRes, errno) ;
|
|
return -3 ;
|
|
}
|
|
log_printf( "Using CANable protocol (w/o ACK/NACK feedback)\n");
|
|
nRes = slcan_setup_bitrate( s_port, CAN_500K) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_setup_bitrate returned %i (%i)\n", nRes, errno) ;
|
|
return -2 ;
|
|
}
|
|
nRes = slcan_open_channel( s_port) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_open_channel returned %i (%i)\n", nRes, errno) ;
|
|
return -2 ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
CloseSLCAN( int nLev, int nRet)
|
|
{
|
|
if ( nLev > -2) {
|
|
int nRes = slcan_close_channel( s_port) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_close_channel returnd %i (%i)\n", nRes, errno) ;
|
|
}
|
|
}
|
|
|
|
if ( nLev > -3) {
|
|
int nRes = slcan_disconnect( s_port) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_disconnect returnd %i (%i)\n", nRes, errno) ;
|
|
}
|
|
}
|
|
|
|
if ( nLev > -4) {
|
|
int nRes = slcan_destroy( s_port) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: slcan_destroy returnd %i (%i)\n", nRes, errno) ;
|
|
}
|
|
}
|
|
|
|
return nRet ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
Homing( void)
|
|
{
|
|
fprintf( stdout, "Homing\n") ;
|
|
// Impostazioni speciali per alcuni assi
|
|
string sSpecId = GetPrivateProfileStringUtf8( "Robot", "SpecialAxes", "3,4", s_sFileIni.data()) ;
|
|
INTVECTOR vSpecId ;
|
|
FromString( sSpecId, vSpecId) ;
|
|
for ( int nId : vSpecId) {
|
|
if ( nId <= s_nAxesCnt) {
|
|
slcan_message_t slmSpec ;
|
|
slmSpec.can_id = nId ;
|
|
slmSpec.can_dlc = 3 ;
|
|
slmSpec.data[0] = 0x9E ;
|
|
slmSpec.data[1] = 0x01 ;
|
|
CalcCRC( slmSpec) ;
|
|
int nRes = slcan_write_message( s_port, &slmSpec, 0) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: special setting failed on axis %d\n", nId) ;
|
|
return -1 ;
|
|
}
|
|
}
|
|
}
|
|
// Home contemporaneo possibile solo per i primi 4 assi
|
|
int nHomeAxesCnt = min( s_nAxesCnt, 4) ;
|
|
|
|
// Lancio esecuzione homing
|
|
for ( int nId = 1 ; nId <= nHomeAxesCnt ; ++ nId) {
|
|
slcan_message_t slmHome ;
|
|
slmHome.can_id = nId ;
|
|
slmHome.can_dlc = 2 ;
|
|
slmHome.data[0] = 0x91 ;
|
|
CalcCRC( slmHome) ;
|
|
int nRes = slcan_write_message( s_port, &slmHome, 0) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: run homing failed on axis %d\n", nId) ;
|
|
return -1 ;
|
|
}
|
|
}
|
|
// Attendo completamento homing
|
|
const int nTarget = ( 1 << nHomeAxesCnt) - 1 ;
|
|
int nStart = 0 ;
|
|
int nHoming = 0 ;
|
|
int nTry = 0 ;
|
|
int nMaxTry = 200 ;
|
|
slcan_message_t slmRead ;
|
|
int nRes = slcan_read_message( s_port, &slmRead, 20) ;
|
|
while ( nHoming != nTarget && nTry < nMaxTry) {
|
|
if ( nRes == 0) {
|
|
for ( int nId = 1 ; nId <= nHomeAxesCnt ; ++ nId) {
|
|
if ( slmRead.can_id == nId) {
|
|
if ( slmRead.data[1] == 0x01)
|
|
nStart |= ( 1 << ( nId - 1)) ;
|
|
else if ( slmRead.data[1] == 0x02)
|
|
nHoming |= ( 1 << ( nId - 1)) ;
|
|
}
|
|
}
|
|
if ( nStart == nTarget)
|
|
nMaxTry = 3000 ;
|
|
}
|
|
else if ( nRes != -30)
|
|
break ;
|
|
nRes = slcan_read_message( s_port, &slmRead, 20) ;
|
|
++ nTry ;
|
|
}
|
|
if ( nHoming != nTarget) {
|
|
log_printf( "Error: homing failed (%d-%d)\n", nHoming, nTarget) ;
|
|
return -1 ;
|
|
}
|
|
|
|
// Assi 5 e 6 devono essere posizionati in home a mano
|
|
|
|
// Qui posso solo dichiarare che sono a zero
|
|
if ( s_nAxesCnt >= 5) {
|
|
for ( int nId = 5 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
// imposto asse a zero
|
|
slcan_message_t slmHome ;
|
|
slmHome.can_id = nId ;
|
|
slmHome.can_dlc = 2 ;
|
|
slmHome.data[0] = 0x92 ;
|
|
CalcCRC( slmHome) ;
|
|
int nRes = slcan_write_message( s_port, &slmHome, 0) ;
|
|
if ( nRes < 0) {
|
|
log_printf( "Error: run setzero failed on axis %d\n", nId) ;
|
|
return -1 ;
|
|
}
|
|
// controllo asse a zero
|
|
int nStatus = -1 ;
|
|
int nTry = 0 ;
|
|
slcan_message_t slmRead ;
|
|
nRes = slcan_read_message( s_port, &slmRead, 20) ;
|
|
while ( nStatus == -1 && nTry < 200) {
|
|
if ( nRes == 0) {
|
|
if ( slmRead.can_id == nId) {
|
|
if ( slmRead.data[1] == 0x01)
|
|
nStatus = 0 ;
|
|
}
|
|
if ( nStart == nTarget)
|
|
nMaxTry = 3000 ;
|
|
}
|
|
else if ( nRes != -30)
|
|
break ;
|
|
nRes = slcan_read_message( s_port, &slmRead, 20) ;
|
|
++ nTry ;
|
|
}
|
|
if ( nStatus < 0) {
|
|
log_printf( "Error: run setzero status read on axis %d\n", nId) ;
|
|
return -1 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Azzero gli assi
|
|
s_vdAxisPos = { 0, 0, 0, 0, 0, 0} ;
|
|
|
|
return 0 ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
MoveAxis( int nId, double dAng, int nFeed)
|
|
{
|
|
// verifico indice asse
|
|
if ( nId < 1 || nId > s_nAxesCnt) {
|
|
log_printf( "Error: move axis with wrong Id (%d)\n", nId) ;
|
|
return -1 ;
|
|
}
|
|
|
|
// limito i parametri
|
|
unsigned int unSpeed = (unsigned) clamp( int( nFeed * s_vdAxisK[nId-1] / s_dAxisKmax), 5, 3000) ;
|
|
unsigned int unAcc = (unsigned) clamp( s_vnAxisAcc[nId-1], 0, 255) ;
|
|
int nPulses = clamp( int( dAng * s_vdAxisK[nId-1]), -8388607, 8388607) ;
|
|
log_printf( " Id=%d Feed=%u Acc=%u Pulses=%d\n", nId, unSpeed, unAcc, nPulses) ;
|
|
|
|
// eseguo movimento in assoluto
|
|
slcan_message_t slmMove ;
|
|
slmMove.can_id = nId ;
|
|
slmMove.can_dlc = 8 ;
|
|
slmMove.data[0] = 0xFE ;
|
|
slmMove.data[1] = ( ( unSpeed >> 8) & 0xFF) ;
|
|
slmMove.data[2] = ( unSpeed & 0xFF) ;
|
|
slmMove.data[3] = ( unAcc & 0xFF) ;
|
|
slmMove.data[4] = ( ( nPulses >> 16) & 0xFF) ;
|
|
slmMove.data[5] = ( ( nPulses >> 8) & 0xFF) ;
|
|
slmMove.data[6] = ( nPulses & 0xFF) ;
|
|
CalcCRC( slmMove) ;
|
|
int nRes = slcan_write_message( s_port, &slmMove, 0) ;
|
|
|
|
return 0 ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
WaitMoveAxisResult( int nId)
|
|
{
|
|
// verifico indice asse
|
|
if ( nId < 1 || nId > s_nAxesCnt) {
|
|
log_printf( "Error: move axis status with wrong Id (%d)\n", nId) ;
|
|
return -1 ;
|
|
}
|
|
// attendo risultato
|
|
int nStatus = -1 ;
|
|
int nTry = 0 ;
|
|
slcan_message_t slmRead ;
|
|
int nRes = slcan_read_message( s_port, &slmRead, 10) ;
|
|
while ( nStatus != 0 && nStatus != 2 && nStatus != 3 && nTry < 1000) {
|
|
if ( nRes == 0) {
|
|
if ( slmRead.can_id == nId && slmRead.data[0] == 0xFE)
|
|
nStatus = slmRead.data[1] ;
|
|
}
|
|
else if ( nRes != -30)
|
|
break ;
|
|
nRes = slcan_read_message( s_port, &slmRead, 10) ;
|
|
++ nTry ;
|
|
}
|
|
if ( nStatus != 2) {
|
|
log_printf( "Error: move axis status (%d)\n", nStatus) ;
|
|
return -1 ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
WaitMoveAxesResult( int nMask)
|
|
{
|
|
// attendo risultato
|
|
int nStatus = 0 ;
|
|
int nTry = 0 ;
|
|
slcan_message_t slmRead ;
|
|
int nRes = slcan_read_message( s_port, &slmRead, 10) ;
|
|
while ( nStatus != nMask && nTry < 10000) {
|
|
if ( nRes == 0) {
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
if ( ( nMask & ( 1 << ( nId - 1))) != 0 &&
|
|
slmRead.can_id == nId && slmRead.data[0] == 0xFE && slmRead.data[1] == 2)
|
|
nStatus |= ( 1 << ( nId - 1)) ;
|
|
}
|
|
}
|
|
else if ( nRes != -30)
|
|
break ;
|
|
nRes = slcan_read_message( s_port, &slmRead, 1) ;
|
|
++ nTry ;
|
|
}
|
|
if ( nStatus != nMask) {
|
|
log_printf( "Error: wait multi move axis status (%d)\n", nStatus) ;
|
|
return -1 ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
MoveAxes( const GMOVEVECTOR& vGMove)
|
|
{
|
|
fprintf( stdout, "Moving\n") ;
|
|
for ( const auto& CurrGMove : vGMove) {
|
|
log_printf( "Move of line %d (%s)\n", CurrGMove.nLine, CurrGMove.sLine.data()) ;
|
|
fprintf( stdout, "%s\n", CurrGMove.sLine.data()) ;
|
|
// se pausa
|
|
if ( CurrGMove.nG == 4) {
|
|
log_printf( " Pause\n") ;
|
|
Sleep( CurrGMove.nFeed) ;
|
|
}
|
|
// se nessun asse da muovere
|
|
else if ( CurrGMove.nMask <= 0) {
|
|
log_printf( " Empty\n") ;
|
|
}
|
|
// se richiesto movimento di un singolo asse
|
|
else if ( IsPowerOfTwo( CurrGMove.nMask)) {
|
|
// cerco asse da muovere
|
|
int nId = 0 ;
|
|
for ( int i = 1 ; i <= s_nAxesCnt && nId == 0 ; ++ i) {
|
|
if ( ( CurrGMove.nMask & ( 1 << ( i - 1))) != 0)
|
|
nId = i ;
|
|
}
|
|
log_printf( " Single move (%d)\n", nId) ;
|
|
// se trovato e valido, eseguo
|
|
if ( nId > 0 && nId <= s_nAxesCnt) {
|
|
int nRes = MoveAxis( nId, CurrGMove.vdAng[nId-1], CurrGMove.nFeed) ;
|
|
if ( nRes != 0)
|
|
return nRes ;
|
|
nRes = WaitMoveAxisResult( nId) ;
|
|
if ( nRes != 0)
|
|
return nRes ;
|
|
s_vdAxisPos[nId-1] = CurrGMove.vdAng[nId-1] ;
|
|
}
|
|
}
|
|
// altrimenti devo muovere più assi contemporaneamente
|
|
else {
|
|
log_printf( " Multiple move (%d)\n", CurrGMove.nMask) ;
|
|
// calcolo il massimo angolo di movimento e la massima lunghezza compensata di movimento
|
|
double dMaxAng = 0 ;
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
if ( ( CurrGMove.nMask & ( 1 << ( nId - 1))) != 0) {
|
|
double dAng = abs( CurrGMove.vdAng[nId-1] - s_vdAxisPos[nId-1]) ;
|
|
dMaxAng = max( dMaxAng, dAng) ;
|
|
}
|
|
}
|
|
// se movimento significativo
|
|
if ( dMaxAng > DELTA_ANG_MIN) {
|
|
// calcolo le feed di ciascun asse
|
|
INTVECTOR vnFeed( s_nAxesCnt) ;
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId)
|
|
vnFeed[nId-1] = int( CurrGMove.nFeed * abs( CurrGMove.vdAng[nId-1] - s_vdAxisPos[nId-1]) / dMaxAng * s_vdAxisComp[nId-1]) ;
|
|
// spezzo il movimento in passi
|
|
double dStep = ( CurrGMove.nG == 0 ? 360 : 2) ;
|
|
int nStep = max( int( ceil( dMaxAng / dStep)), 1) ;
|
|
for ( int nS = 1 ; nS <= nStep ; ++ nS) {
|
|
double dCoeff = ( 1. * nS) / nStep ;
|
|
// eseguo movimenti
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
if ( ( CurrGMove.nMask & ( 1 << ( nId - 1))) != 0) {
|
|
double dAng = ( 1 - dCoeff) * s_vdAxisPos[nId-1] + dCoeff * CurrGMove.vdAng[nId-1] ;
|
|
int nRes = MoveAxis( nId, dAng, vnFeed[nId-1]) ;
|
|
if ( nRes != 0)
|
|
return nRes ;
|
|
}
|
|
}
|
|
// attendo completamento movimenti
|
|
int nRes = WaitMoveAxesResult( CurrGMove.nMask) ;
|
|
if ( nRes != 0)
|
|
return nRes ;
|
|
}
|
|
// aggiorno posizioni assi
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
if ( ( CurrGMove.nMask & ( 1 << ( nId - 1))) != 0) {
|
|
s_vdAxisPos[nId-1] = CurrGMove.vdAng[nId-1] ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
fprintf( stdout, "Completed\n") ;
|
|
return 0 ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static bool
|
|
ReadOneMove( const string& sMove, int nId, GMove& CurrGMove)
|
|
{
|
|
double dVal ;
|
|
if ( FromString( sMove.substr( 1), dVal)) {
|
|
CurrGMove.nMask |= ( 1 << ( nId - 1)) ;
|
|
CurrGMove.vdAng[nId-1] = dVal ;
|
|
return true ;
|
|
}
|
|
return false ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static bool
|
|
ReadProgram( const string& sProgPath, GMOVEVECTOR& vGMove)
|
|
{
|
|
// Inizializzo la lettura
|
|
Scanner TheScanner ;
|
|
if ( ! TheScanner.Init( sProgPath, ";")) {
|
|
log_printf( "Error: failed opening program %s\n", sProgPath) ;
|
|
return false ;
|
|
}
|
|
|
|
// Leggo le linee
|
|
int nG = 0 ;
|
|
int nFeed = s_nFeed ;
|
|
string sLine ;
|
|
while ( TheScanner.GetLine( sLine)) {
|
|
bool bOk = true ;
|
|
GMove CurrGMove ;
|
|
CurrGMove.sLine = sLine ;
|
|
CurrGMove.nLine = TheScanner.GetCurrLineNbr() ;
|
|
// divido la linea in parti
|
|
STRVECTOR vsTokens ;
|
|
TokenizePlus( sLine, "GXYZABCF", vsTokens) ;
|
|
for ( int i = 0 ; i < int( vsTokens.size()) ; ++ i) {
|
|
// analizzo ogni parte
|
|
switch ( vsTokens[i][0]) {
|
|
case 'G' :
|
|
FromString( vsTokens[i].substr( 1), nG) ;
|
|
break ;
|
|
case 'X' :
|
|
bOk = bOk && ReadOneMove( vsTokens[i], 1, CurrGMove) ;
|
|
break ;
|
|
case 'Y' :
|
|
bOk = bOk && ReadOneMove( vsTokens[i], 2, CurrGMove) ;
|
|
break ;
|
|
case 'Z' :
|
|
bOk = bOk && ReadOneMove( vsTokens[i], 3, CurrGMove) ;
|
|
break ;
|
|
case 'A' :
|
|
bOk = bOk && ReadOneMove( vsTokens[i], 4, CurrGMove) ;
|
|
break ;
|
|
case 'B' :
|
|
bOk = bOk && ReadOneMove( vsTokens[i], 5, CurrGMove) ;
|
|
break ;
|
|
case 'C' :
|
|
bOk = bOk && ReadOneMove( vsTokens[i], 6, CurrGMove) ;
|
|
break ;
|
|
case 'F' :
|
|
bOk = bOk && FromString( vsTokens[i].substr( 1), CurrGMove.nFeed) ;
|
|
break ;
|
|
}
|
|
}
|
|
if ( ! bOk) {
|
|
log_printf( "Error: line %d not correct\n", CurrGMove.nLine) ;
|
|
return false ;
|
|
}
|
|
CurrGMove.nG = nG ;
|
|
if ( nG == 0)
|
|
CurrGMove.nFeed = s_nRapidFeed ;
|
|
else if ( nG != 4) {
|
|
if ( CurrGMove.nFeed == 0)
|
|
CurrGMove.nFeed = nFeed ;
|
|
else
|
|
nFeed = CurrGMove.nFeed ;
|
|
}
|
|
if ( CurrGMove.nMask != 0 || CurrGMove.nG == 4)
|
|
vGMove.emplace_back( CurrGMove) ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
static int
|
|
Exec( int nCmd, const string& sProgram)
|
|
{
|
|
// Recupero i dati degli assi del robot
|
|
s_nAxesCnt = GetPrivateProfileInt( "Robot", "Axes", MAX_AXES, s_sFileIni.data()) ;
|
|
if ( s_nAxesCnt < 1 || s_nAxesCnt > MAX_AXES) {
|
|
log_printf( "Error: wrong robot axes number (%d)\n", s_nAxesCnt) ;
|
|
return -1 ;
|
|
}
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
string sKey = "K" + ToString( nId) ;
|
|
string sTmp = GetPrivateProfileStringUtf8( "Robot", sKey.data(), "1", s_sFileIni.data()) ;
|
|
FromString( sTmp, s_vdAxisK[nId-1]) ;
|
|
}
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId)
|
|
s_dAxisKmax = max( s_dAxisKmax, s_vdAxisK[nId-1]) ;
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
string sKey = "Fcmp" + ToString( nId) ;
|
|
string sTmp = GetPrivateProfileStringUtf8( "Robot", sKey.data(), "1", s_sFileIni.data()) ;
|
|
FromString( sTmp, s_vdAxisComp[nId-1]) ;
|
|
}
|
|
for ( int nId = 1 ; nId <= s_nAxesCnt ; ++ nId) {
|
|
string sKey = "Acc" + ToString( nId) ;
|
|
string sTmp = GetPrivateProfileStringUtf8( "Robot", sKey.data(), "2", s_sFileIni.data()) ;
|
|
FromString( sTmp, s_vnAxisAcc[nId-1]) ;
|
|
}
|
|
s_nFeed = GetPrivateProfileInt( "Robot", "Feed", 300, s_sFileIni.data()) ;
|
|
s_nRapidFeed = GetPrivateProfileInt( "Robot", "RapidFeed", 1000, s_sFileIni.data()) ;
|
|
|
|
// Verifico cosa fare
|
|
bool bHome = ( nCmd == 1 || nCmd == 3) ;
|
|
bool bProg = ( ! IsEmptyOrSpaces( sProgram)) ;
|
|
bool bLoop = ( nCmd == 2 || nCmd == 3) ;
|
|
bool bTestKeyPressed = ( GetPrivateProfileInt( "General", "TestKeyPressed", 0, s_sFileIni.data()) == 1) ;
|
|
int nPause = GetPrivateProfileInt( "General", "LoopPause", 10, s_sFileIni.data()) * 1000 ;
|
|
|
|
// Gestione programma da eseguire
|
|
GMOVEVECTOR vGMove ;
|
|
if ( bProg) {
|
|
// Leggo il programma
|
|
string sProgPath = s_sProgDir + "\\" + sProgram ;
|
|
if ( bHome)
|
|
log_printf( "Homing and executing %s\n", sProgPath.data()) ;
|
|
else
|
|
log_printf( "Executing %s\n", sProgPath.data()) ;
|
|
if ( ! ReadProgram( sProgPath, vGMove))
|
|
return -1 ;
|
|
log_printf( "Reading completed\n") ;
|
|
}
|
|
else
|
|
log_printf( "Homing alone\n") ;
|
|
|
|
// Apertura porta SLCAN
|
|
int nOpenRes = OpenSLCAN() ;
|
|
if ( nOpenRes != 0)
|
|
return CloseSLCAN( nOpenRes, nOpenRes) ;
|
|
|
|
// Eseguo homing degli assi
|
|
if ( bHome) {
|
|
int nHomeRes = Homing() ;
|
|
if ( nHomeRes < 0)
|
|
return CloseSLCAN( 0, nHomeRes) ;
|
|
}
|
|
|
|
// Se non c'è programma, ho finito
|
|
if ( ! bProg)
|
|
return CloseSLCAN( 0, 0) ;
|
|
|
|
// Eseguo movimenti assi
|
|
do {
|
|
int nMoveRes = MoveAxes( vGMove) ;
|
|
if ( nMoveRes < 0)
|
|
return CloseSLCAN( 0, nMoveRes) ;
|
|
if ( bLoop && bTestKeyPressed && _kbhit())
|
|
bLoop = false ;
|
|
if ( bLoop) {
|
|
Sleep( nPause) ;
|
|
if ( bHome) {
|
|
int nHomeRes = Homing() ;
|
|
if ( nHomeRes < 0)
|
|
return CloseSLCAN( 0, nHomeRes) ;
|
|
}
|
|
}
|
|
} while ( bLoop) ;
|
|
|
|
// Chiusura
|
|
return CloseSLCAN( 0, 0) ;
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------------------
|
|
int
|
|
wmain( int argc, wchar_t *argv[])
|
|
{
|
|
// Primo parametro della linea di comando : nome eseguibile
|
|
// Non interessa
|
|
// Secondo parametro della linea di comando : 0=niente, 1=home
|
|
wstring swCmd = ( argc >= 2 ? argv[1] : L"0") ;
|
|
int nCmd = 0 ;
|
|
FromString( wstringtoA( swCmd), nCmd) ;
|
|
// Terzo parametro della linea di comando : nome programma di movimento
|
|
wstring swProgram = ( argc >= 3 ? argv[2] : L"") ;
|
|
string sProgram = wstringtoA( swProgram) ;
|
|
// Recupero il direttorio del programma e lo imposto come direttorio di lavoro
|
|
string sExeDir ;
|
|
GetModuleDirectory( NULL, sExeDir) ;
|
|
SetCurrentDirectory( sExeDir) ;
|
|
// Impostazione path radice per i dati
|
|
string sDataRoot = GetPrivateProfileStringUtf8( "Data", "DataRoot", "", ( sExeDir + "\\DataRoot.ini").c_str()) ;
|
|
if ( sDataRoot.empty())
|
|
sDataRoot = sExeDir ;
|
|
// Impostazione direttorio di configurazione
|
|
string sConfigDir = sDataRoot + "\\Config" ;
|
|
// Impostazione direttorio per i file temporanei
|
|
s_sTempDir = sDataRoot + "\\Temp" ;
|
|
// Impostazione direttorio per i file dei programmi
|
|
s_sProgDir = sDataRoot + "\\Programs" ;
|
|
// Impostazione path del file Ini
|
|
s_sFileIni = sConfigDir + "\\ArctosRobotCnc.ini" ;
|
|
// Impostazione path del file Log
|
|
string sLogFile = s_sTempDir + "\\ArctosRobotLog.txt" ;
|
|
|
|
// Apro log
|
|
log_init( sLogFile.data()) ;
|
|
|
|
// Eseguo
|
|
int nRes = Exec( nCmd, sProgram) ;
|
|
|
|
// Chiudo log
|
|
log_exit() ;
|
|
|
|
return nRes ;
|
|
}
|