bool CWindowListener::OnRButtonUp( guint32 nFlags, double x, double y ){
if ( CanProcess() ) {
g_2DView.OnRButtonUp( (int)x, (int)y );
return true;
}
return false;
}
bool CWindowListener::OnLButtonUp( guint32 nFlags, double x, double y ){
if ( CanProcess() ) {
g_pManager->OnLButtonUp( (int)x, (int)y );
return true;
}
return false;
}
static gint keypress (GtkWidget* widget, GdkEventKey* event, gpointer data)
{
unsigned int code = gdk_keyval_to_upper(event->keyval);
if (code == GDK_Escape)
{
gtk_widget_destroy (g_pToolWnd);
g_pToolWnd = NULL;
return TRUE;
}
if (CanProcess ())
{
if (g_2DView.OnKeyDown (code))
return FALSE;
if (code == GDK_Return)
{
Textool_Validate();
return FALSE;
}
}
return TRUE;
}
static gint expose (GtkWidget *widget, GdkEventExpose *event, gpointer data)
{
if (event->count > 0)
return TRUE;
if (!CanProcess ())
return TRUE;
if (g_bTexViewReady)
{
g_2DView.m_rect.bottom = widget->allocation.height;
g_2DView.m_rect.right = widget->allocation.width;
if (!g_QglTable.m_pfn_glwidget_make_current (g_pToolWidget))
{
Sys_Printf("TexTool: glMakeCurrent failed\n");
return TRUE;
}
DoExpose ();
g_QglTable.m_pfn_glwidget_swap_buffers (g_pToolWidget);
}
return TRUE;
}
static void DoPaint ()
{
if (!CanProcess ())
return;
if (g_bTexViewReady)
{
g_2DView.m_rect.bottom = g_pToolWnd->getHeight();
g_2DView.m_rect.right = g_pToolWnd->getWidth();
// set GL_PROJECTION
g_2DView.PreparePaint();
// render the objects
// the master is not rendered the same way, draw over a unified texture background
g_2DView.TextureBackground(g_DrawObjects[0].pObject->getTextureNumber());
if (g_nDrawObjects >= 1)
{
int i;
for (i=1;i<g_nDrawObjects;i++)
{
// we use a first step to the GL_MODELVIEW for the master object
// GL_MODELVIEW will be altered in RenderAuxiliary too
g_DrawObjects[0].pObject->PrepareModelView(g_DrawObjects[i].pTopo);
g_DrawObjects[i].pObject->RenderAuxiliary();
}
}
// draw the polygon outline and control points
g_DrawObjects[0].pObject->PrepareModelView(NULL);
g_DrawObjects[0].pObject->RenderUI();
}
}
/// \brief Verify whether the intended position was achieved
/// \details The MCDC position notify command returns the character 'p' when the position is achieved
/// \pre A corresponding call to SetPosition() was made along with the notify parameter set to true
/// \post Boolean value returned indicating whether or not the position, set in SetPosition(), has been reached
bool MotionController::IsPositionVerified(long timeOut)
{
bool success = false;
if( FAILED(CanProcess()) )
{
return false;
}
if(m_posNotifyCounter < 1)
{
qDebug() << "MotionController : m_posNotifyCounter=" << m_posNotifyCounter;
throw MotionControllerException("Position verified counter out of sync");
}
// If DEBUG_NO_ROBOT is defined as a preprocessor definition DO NOT call ReadCharsAndCompare() method
// as there are no characters to receive (just set to true).
bool equal = true;
#ifndef DEBUG_NO_ROBOT
equal = ReadCharsAndCompare("p\r\n", timeOut);
#endif
if( equal )
{
success = true;
--m_posNotifyCounter;
m_currentState = Verified;
}
return success;
}
/// \brief Set corridor window
/// \details Uses the 'CORRIDOR' command to set the corridor window. Notifcation for position/velocity
/// will be sent whenever the motor enters inside the corridor window (i.e. speeds up or slows down
/// notification). This functionality greatly speeds up the maximum bandwidth of the vibration motor
/// \pre MCDC is still connected on the given COM port
/// \post Corridor value is set for the particular MCDC
void MotionController::SetCorridorValue(int corridor)
{
if( FAILED(CanProcess()) ) return;
m_command = QString("%1CORRIDOR%2\r").arg(DEVICE_ID,corridor);
m_serial.Write(m_command.toStdString().c_str());
}
/// \brief Starts the motor homing sequence
/// \details The motor moves until a limit switch is hit, at which point the position is set to zero
/// \pre The homing velocity and limit behaviors have been previously saved in the MCDC
/// \post The motor is stopped and position is set to 0
void MotionController::HomingSequence(void)
{
if( FAILED(CanProcess()) ) throw MotionControllerException("Cannot perform homing");;
// Construct the string to send with both the LA and the M commands embedded in a single transmission
m_command = QString("%1GOHOSEQ\r").arg(DEVICE_ID);
m_serial.Write(m_command.toStdString().c_str());
m_currentState = Stopped;
}
/// \brief Set position limits on MCDC device
/// \details Uses the 'LL' command to set the minimum and maximum encoder values that the
/// motor can travel. To enable or disable the limits call enablePositionLimits().
/// \pre MCDC is still connected on the given COM port
/// \post Position limits enforced or removed depending on the boolean value of enable
void MotionController::SetPositionLimits(int min, int max)
{
if( FAILED(CanProcess()) ) return;
m_command = QString("%1LL%2\r").arg(DEVICE_ID,min);
m_serial.Write(m_command.toStdString().c_str());
m_command = QString("%1LL%2\r").arg(DEVICE_ID,max);
m_serial.Write(m_command.toStdString().c_str());
}
bool CWindowListener::Paint()
{
if (!CanProcess ())
return false;
if (g_bTexViewReady)
DoExpose();
return true;
}
/// \brief Change the maximum motor velocity that the motor can spin at
/// \pre MCDC is still connected on the given COM port
/// \post MCDC internal mac velocity parameter is set to maxspeed
bool MotionController::ChangeMaxMotorVelocity(int maxSpeed)
{
if( FAILED(CanProcess()) ) return false;
//Tell MCDC to change the maximum speed of travel to the given speed param
m_command = QString("%1SP%2\r").arg(DEVICE_ID).arg(maxSpeed);
m_serial.Write(m_command.toStdString().c_str());
return true;
}
HRESULT CSerialCommHelper::Read_Upto (std::string& data,char chTerminator ,long* Count,long TimeOut)
{
HRESULT hr = CanProcess();
if ( FAILED(hr)) return hr;
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper: Read_Upto called "));
try
{
std::string Tmp;
Tmp.erase ();
long BytesRead;
bool Found = SerialBuffer.Read_Upto(Tmp ,chTerminator,BytesRead,DataRx );
if ( Found )
{
data = Tmp ;
}
else
{//there are either none or less bytes...
long iRead = 0;
bool condition = true;
while ( condition )
{
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_Upto () making blocking read cl "));
DWORD Wait = ::WaitForSingleObject ( DataRx , TimeOut ) ;
if ( Wait == WAIT_TIMEOUT)
{
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_Upto () timed out in blocking read"));
data.erase ();
hr = E_FAIL;
return hr;
}
bool Found = SerialBuffer.Read_Upto(Tmp ,chTerminator,BytesRead,DataRx );
if ( Found )
{
data = Tmp;
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper: Read_Upto WaitForSingleObject data:{%s}len:{%d}"),((Tmp)).c_str(),Tmp.size ());
return S_OK;
}
MTXDBG(CRITICAL,("CSerialCommHelper : CSerialCommHelper: Read_Upto WaitForSingleObject not FOUND "));
}
}
}
catch(...)
{
MTXDBG(CRITICAL,_("CSerialCommHelperUnhandled exception"));
g_assert ( 0 ) ;
}
return hr;
}
bool CWindowListener::OnMouseMove( guint32 nFlags, double x, double y ){
if ( CanProcess() ) {
if ( g_2DView.OnMouseMove( (int)x, (int)y ) ) {
return true;
}
g_pManager->OnMouseMove( (int)x, (int)y );
return true;
}
return false;
}
static void motion (GtkWidget *widget, GdkEventMotion *event, gpointer data)
{
if (CanProcess ())
{
if (g_2DView.OnMouseMove (event->x, event->y))
return;
if (g_pManager->OnMouseMove (event->x, event->y))
return;
}
}
static void button_release( GtkWidget *widget, GdkEventButton *event, gpointer data ){
if ( CanProcess() ) {
switch ( event->button )
{
case 1:
g_pManager->OnLButtonUp( event->x, event->y ); break;
case 3:
g_2DView.OnRButtonUp( event->x, event->y ); break;
}
}
}
/// \brief Clear any data on the serial port
/// \pre none
/// \post Any data waiting to be read on the com port is discarded
void MotionController::ClearExistingData(void)
{
if( FAILED(CanProcess()) )
{
throw MotionControllerException("Cannot clear data due to motion controller state");
}
if( m_serial.Purge() != S_OK )
{
throw MotionControllerException("Purging of serial buffer failed");
}
}
/// \brief Enable position limits on MCDC device
/// \details Uses the 'APL' command to either allow or dis-allow the MCDC device to move past
/// a specific encoder value. The specific encoder values can be changed with setPositionLimits().
/// \pre MCDC is still connected on the given COM port
/// \post Position limits enforced or removed depending on the boolean value of enable
void MotionController::EnablePositionLimits(bool enable)
{
if( FAILED(CanProcess()) ) return;
m_limitsEnabled = enable;
int en = 0;
if(enable)
{
en = 1;
}
m_command = QString("%1APL%2\r").arg(DEVICE_ID).arg(en);
m_serial.Write(m_command.toStdString().c_str());
}
/// \brief Get the current/amperage value of the motor in milli-amps (mA)
/// \details Send the "grc" command followed by a read of the serial port to get the current amperage of the motor
/// \pre MCDC is still connected
/// \post The current in mA is returned
int MotionController::GetCurrent(void)
{
if( FAILED(CanProcess()) ) return false;
m_command = QString("%1GRC\r").arg(DEVICE_ID);
m_serial.Write(m_command.toStdString().c_str());
std::string szRecvData;
QString current = "";
char recvChar = '|';
bool breakFromLoop = false;
bool CR_recv = false; //Bool to indicate if Carriage return was received
while(true)
{
while( !m_serial.IsInputAvailable() ) Sleep(1);
if(m_serial.Read_N(szRecvData, 1, -1) != S_OK)
{
throw MotionControllerException("Current read from serial port failed");
}
recvChar = *szRecvData.c_str();
switch(recvChar)
{
case '|':
break; //No valid chars have come through the serial port yet
case '\r': //CR char (0D in Hex) is the second last char to be received
CR_recv = true;
break;
case '\n': //LF char (0A in Hex) is that last char to be received before breaking from loop
breakFromLoop = true; //All chars have now been recv'd correctly
break;
default:
if( isdigit(recvChar) )
current.append( recvChar );
break;
}
if(breakFromLoop) break; //Break from loop if ending char was recv'd
}
bool ok;
long amperage = current.toInt(&ok, 10);
return amperage;
}
/// \brief Set the digital output of the MCDC
/// \details Sets the FAULT pin to either high-impedance or GND
/// \pre MCDC DIGOUT is set to true
/// \post FAULT output pin is set based on OnOff
void MotionController::setDigitalOutput(bool OnOff)
{
if(!IsInitialized())
return;
if( FAILED(CanProcess()) ) throw MotionControllerException("Cannot control output");;
// Construct the string to send with the appropriate command
if( OnOff)
m_command = QString("%1SO\r").arg(DEVICE_ID);
else
m_command = QString("%1CO\r").arg(DEVICE_ID);
m_serial.Write(m_command.toStdString().c_str());
}
/// \brief Set current limit of MCDC device
/// \details Uses the 'LPC' command to set the maximum allowable MCDC drive current
/// \warning Be very CAREFUL how you change the safety checks in this function if you ever do.
/// Each motor connected to the MCDC has a maximum allowable current and if exceed for
/// long periods of time, the motor can be damaged.
/// \pre MCDC is still connected on the given COM port
/// \post Current limit set for the particular MCDC
void MotionController::SetPeakCurrentLimit(int currentLimit)
{
if( FAILED(CanProcess()) ) return;
//Set Current Limit
/*** WARNING: Make sure the current limit is not set too high ***/
if(currentLimit < 0 || currentLimit > 3000) //If greater than 2A it may be unsafe to operate (i.e damage the motor)
//Even 2A could be too high for smaller motors so be carefull
{
throw MotionControllerException("Current limit may be set too high, re-check value");
return;
}
m_command = QString("%1LPC%2\r").arg(DEVICE_ID,currentLimit);
m_serial.Write(m_command.toStdString().c_str());
}
bool CWindowListener::OnKeyPressed( char *s ){
if ( !strcmp( s,"Escape" ) ) {
Textool_Cancel();
return TRUE;
}
if ( CanProcess() ) {
if ( g_2DView.OnKeyDown( s ) ) {
return TRUE;
}
if ( !strcmp( s,"Return" ) ) {
Textool_Validate();
return TRUE;
}
}
return FALSE;
}
/// \brief Rotate the motor to a given position (in encoder tick format) and request notification if necessary
/// \details If the notify parameter is set to 'true' then the notificationValue parameter should also be set
/// \pre MCDC is still connected on the given COM port
/// \post Motor spins to and maintains a given position (i.e. maintains its location at a specific encoder tick)
bool MotionController::SetPosition(int pos, DeviceNotification *notify/*=NULL*/)
{
if( FAILED(CanProcess()) ) return false;
if( notify != NULL)
{
RequestPositionNotification(notify);
}
//Construct the string to send with both the LA and the M commands embedded in a single transmission
m_command = QString("%1LA%2\r%3M\r").arg(DEVICE_ID).arg(pos).arg(DEVICE_ID);
m_serial.Write(m_command.toStdString().c_str());
m_currentState = Moving;
return true;
}
/// \brief Request verification (send back a 'p' char) when a particular position is reached
/// \details If the 'notify' param does not have a target, then only notify when motor reaches its target position
/// \pre MCDC is still connected on the given COM port
/// \post MCDC is given a request to notify at a specific position when overshot or when the target position is reached
bool MotionController::RequestPositionNotification(DeviceNotification *notify)
{
if( FAILED(CanProcess()) ) return false;
if( notify->HasNotifyValue() )
{
//Notify when motor overshoots/reaches the 'GetNotifyValue()' position
m_command = QString("%1NP%2\r").arg(DEVICE_ID).arg(notify->GetNotifyValue());
}
else
{
//Only notify when motor reaches its TARGET position
m_command = QString("%1NP\r").arg(DEVICE_ID);
}
m_serial.Write(m_command.toStdString().c_str());
++m_posNotifyCounter;
return true;
}
/*-----------------------------------------------------------------------
-- Reads all the data that is available in the local buffer..
does NOT make any blocking calls in case the local buffer is empty
-----------------------------------------------------------------------*/
HRESULT CSerialCommHelper::ReadAvailable(std::string& data)
{
HRESULT hr = CanProcess();
if ( FAILED(hr)) return hr;
try
{
std::string Temp;
bool res = SerialBuffer.Read_Available (Temp,DataRx);
data = Temp;
}
catch(...)
{
MTXDBG(CRITICAL,_("CSerialCommHelper Unhandled exception in ReadAvailable()"));
g_assert ( 0 ) ;
hr = E_FAIL;
}
return hr;
}
/// \brief Verify whether the intended velocity was achieved
/// \details The MCDC velocity notify command returns the character 'v' when the velocity is achieved
/// \pre A corresponding call to SetVelocity() was made along with the notify parameter set to true
/// \post Boolean value returned indicating whether or not the velocity, set in SetVelocity(), has been reached
bool MotionController::IsVelocityVerified(long timeOut)
{
bool success = false;
if( FAILED(CanProcess()) ) return false;
if(m_velNotifyCounter < 1)
{
qDebug() << "MotionController : m_velNotifyCounter=" << m_velNotifyCounter;
throw MotionControllerException("Velocity verified counter out of sync");
}
if( ReadCharsAndCompare("v\r\n", timeOut) ) // "v\r\n"
{
success = true;
--m_velNotifyCounter;
m_currentState = Verified;
}
return success;
}
/// \brief Spin the motor at a given speed (RPM) and request notification if desired
/// \details If the notify parameter is set to 'true' then the notificationValue parameter should also be set
/// \pre MCDC is still connected on the given COM port
/// \post Motor spins at and maintains a constant velocity (and may be notified when it reaches that particular velocity)
bool MotionController::SetVelocity(int rpm, DeviceNotification *notify/*=NULL*/)
{
if( FAILED(CanProcess()) ) return false;
if(m_limitsEnabled)
{
qDebug() << "ERROR: Cannot drive motor in velocity mode when limits are enabled";
return false;
}
if( notify != NULL )
{
RequestVelocityNotification(notify);
}
m_command = QString("%1V%2\r").arg(QString::number(DEVICE_ID)).arg(QString::number(rpm));
m_serial.Write(m_command.toStdString().c_str());
m_currentState = Moving;
return true;
}
HRESULT CSerialCommHelper::Write (const char* data,DWORD Size)
{
HRESULT hr = CanProcess();
if ( FAILED(hr)) return hr;
int res = 0 ;
OVERLAPPED ov;
memset(&ov,0,sizeof(ov));
ov.hEvent = CreateEvent( 0,true,0,0);
DWORD BytesWritten = 0;
//do
{
res = WriteFile (CommPort,data,Size,&BytesWritten ,&ov);
if ( res == 0 )
{
WaitForSingleObject(ov.hEvent ,INFINITE);
}
}// while ( ov.InternHigh != Size ) ;
CloseHandle(ov.hEvent);
std::string Data(data);
MTXDBG(CRITICAL,_("RCSeri:Writing:{%s} len:{%d}"),(Data).c_str(),Data.size());
return S_OK;
}
/// \brief Get the current position value of the motor in encoder ticks
/// \details Send the "pos" command followed by a read of the serial port to get the most current position of the motor
/// \pre MCDC is still connected
/// \post The current position in encoder ticks is returned
long MotionController::GetPosition(void)
{
if( FAILED(CanProcess()) ) return false;
m_command = QString("%1POS\r").arg(DEVICE_ID);
m_serial.Write(m_command.toStdString().c_str());
std::string szRecvData;
QString position = "";
char recvChar = '|';
bool breakFromLoop = false;
bool CR_recv = false; //Bool to indicate if Carriage return was received
while(true)
{
int nTries = 0;
while( !m_serial.IsInputAvailable() && nTries++ < MAX_QUERY_TRIES ) Sleep(1);
if(nTries >= MAX_QUERY_TRIES) {
//printf("Failed stupid\n");
return m_lastPos;
}
//Input is now available...just read it
if(m_serial.Read_N(szRecvData, 1, -1) != S_OK)
{
throw MotionControllerException("Position read from serial port failed");
}
recvChar = *szRecvData.c_str();
switch(recvChar)
{
case '|':
break; //No valid chars have come through the serial port yet
case '\r': //CR char (0D in Hex) is the second last char to be received
CR_recv = true;
break;
case '\n': //LF char (0A in Hex) is that last char to be received before breaking from loop
breakFromLoop = true; //All chars have now been recv'd correctly
break;
case '-': //Minus sign may exist in the first char only, if its doesn't assert
position.append(recvChar);
break;
default:
if( isdigit(recvChar) )
position.append(recvChar);
break;
}
if(breakFromLoop) break; //Break from loop if ending char was recv'd
}
bool ok;
long pos = position.toLong(&ok, 10);
m_lastPos = pos;
return pos;
}
HRESULT CSerialCommHelper::Read_N (std::string& data, long Count ,long TimeOut )
{
HRESULT hr = CanProcess();
if ( FAILED(hr))
{
g_assert(0);
return hr;
}
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N called for %d bytes"),Count);
try
{
std::string Tmp ;
Tmp.erase();
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N (%d) locking the queue "),Count);
int iLoc = SerialBuffer.Read_N(Tmp ,Count ,DataRx );
if ( iLoc == Count )
{
data = Tmp;
}
else
{//there are either none or less bytes...
long iRead = 0;
int iRemaining = Count - iLoc;
while ( 1 )
{
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N (%d) making blocking read() "),Count);
DWORD Wait = WaitForSingleObject ( DataRx , TimeOut ) ;
if ( Wait == WAIT_TIMEOUT )
{
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N (%d) timed out in blocking read"),Count);
data.erase ();
hr = E_FAIL;
return hr;
}
g_assert ( Wait == WAIT_OBJECT_0 );
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N (%d) Woke Up from WaitXXX() locking Q"),Count);
iRead = SerialBuffer.Read_N(Tmp , iRemaining ,DataRx);
iRemaining -= iRead ;
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N (%d) Woke Up from WaitXXX() Unlocking Q"),Count);
if ( iRemaining == 0)
{
MTXDBG(CRITICAL,_("CSerialCommHelper : CSerialCommHelper : Read_N (%d) Woke Up from WaitXXX() Done reading "),Count);
data = Tmp;
return S_OK;
}
}
}
}
catch(...)
{
MTXDBG(CRITICAL,_("CSerialCommHelper Unhandled exception"));
g_assert ( 0 ) ;
}
return hr;
}
