# Dessin du squelette
############################################################################################################################ */
void DrawSkelet(void)
{
/* **************************************************************************************************************************** */
int i;
MDFloat tempT[4][4];
MDFloat ZTi[4][4];
MDFloat *ptrZTi = (MDFloat*)ZTi;
MDFloat *ptrtempT = (MDFloat*)tempT;
/* **************************************************************************************************************************** */
MoveToOrigin();
/* **************************************************************************************************************************** */
glLoadName(_DESIRED_EE_ID_);
glDisable(GL_LIGHTING);
glMatrixMode(GL_MODELVIEW);
glBegin(GL_POINTS);
glColor3f(1.0f, 1.0f, 0.2f);
glVertex3dv(PNew);
glEnd();
glEnable(GL_LIGHTING);
/* **************************************************************************************************************************** */
glMatrixMode(GL_MODELVIEW);
/* **************************************************************************************************************************** */
glPushMatrix();
/* **************************************************************************************************************************** */
LoadIdentity(ptrZTi);
for (i=0; i<_NO_OF_LINKS_; i++) {
Skelet[i].Draw();
glPopMatrix();
glPushMatrix();
MultMatrix4x4((MDFloat(*)[4])Skelet[i].Get_im1Ti(), ZTi, tempT);
CopyMatrix4x4(ptrtempT, ptrZTi);
glMultTransposeMatrixd(ptrZTi);
}
/* **************************************************************************************************************************** */
DrawEE();
/* **************************************************************************************************************************** */
void user(void)
{
BYTE numBytesRead;
char message[64];
char movementCommandCode[3], movementCommandType;
double stepDegrees, distancePerRevolution;
//Blink the LEDs according to the USB device status
//BlinkUSBStatus();
// User Application USB tasks
if( (USBDeviceState < CONFIGURED_STATE) || (USBSuspendControl == 1) ) return;
if(USBUSARTIsTxTrfReady())
{
numBytesRead = getsUSBUSART(USB_Out_Buffer,64);
if(numBytesRead != 0)
{
BYTE i;
for(i = 0; i < numBytesRead; i++)
{
USB_In_Buffer[i] = USB_Out_Buffer[i];
}
USB_In_Buffer[i] = '\0';
// RETURN CURRENTSTEPS POSITION
if(!strcmppgm2ram(USB_In_Buffer, (const rom char far *)"position"))
{
sprintf(message, (const rom char far *)"X%ld Y%ld Z%ld|", currentStepsPosition.x, currentStepsPosition.y, currentStepsPosition.z);
putUSBUSART(message, strlen(message));
goto endUser;
}
// RESET CNC - stat: SERIALPORTCONNECTED
if(!strcmppgm2ram(USB_In_Buffer, (const rom char far *)"reset"))
{
limitSensorX = limitSensorY = limitSensorZ = programPaused = false; //configured = programPaused = false;
strcpypgm2ram(message, (const rom char far *)"CNCR|");
putUSBUSART(message, strlen(message));
machineState = SERIALPORTCONNECTED;
goto endUser;
}
// RETURN CNC STATE
if(!strcmppgm2ram(USB_In_Buffer, (const rom char far *)"status"))
{
sprintf(message, (const rom char far *)"CNCS:%d|", machineState);
putUSBUSART(message, strlen(message));
goto endUser;
}
// MOVE TO ORIGIN: absolute 0,0,0
if(!strcmppgm2ram(USB_In_Buffer, (const rom char far *)"origin"))
{
if(programPaused)
{
strcpypgm2ram(message, (const rom char far *)"ERR:PP|");
putUSBUSART(message, strlen(message));
goto endUser;
}
else
{
MoveToOrigin();
if(machineState == EMERGENCYSTOP)
{
sprintf(message, (const rom char far *)"ERR:PE|");
machineState = SERIALPORTCONNECTED;
}
else
{
strcpypgm2ram(message, (const rom char far *)"PO|");
machineState = WAITINGCOMMAND;
}
putUSBUSART(message, strlen(message));
goto endUser;
}
}
// START FREEMOVES
if(strstrrampgm(USB_In_Buffer, (const rom char far *)"FM:"))
{
if(programPaused)
{
strcpypgm2ram(message, (const rom char far *)"ERR:PP|");
putUSBUSART(message, strlen(message));
goto endUser;
}
else
{
freeCode = GetFreeCode(USB_In_Buffer);
if(freeCode != -2)
{
strcpypgm2ram(message, (const rom char far *)"CNCFM|");
machineState = FREEMOVES;
}
else
{
strcpypgm2ram(message, (const rom char far *)"ERR:CNCFM|");
}
putUSBUSART(message, strlen(message));
goto endUser;
}
}
// STOP FREEMOVES
if( (machineState == FREEMOVES) && (!strcmppgm2ram(USB_In_Buffer, (const rom char far *)"stop")) )
{
freeCode = -2;
sprintf(message, (const rom char far *)"CNCSFM_X%ld Y%ld Z%ld|", currentStepsPosition.x, currentStepsPosition.y, currentStepsPosition.z);
putUSBUSART(message, strlen(message));
machineState = WAITINGCOMMAND;
goto endUser;
}
switch(machineState)
{
case SERIALPORTCONNECTED:
// Count characters received and send this number to PC
sprintf(message, (const rom char far *)"%d|", numBytesRead);
putUSBUSART(message, strlen(message));
machineState = HANDSHAKEACKRECEIVED;
break;
case HANDSHAKEACKRECEIVED:
// Compare confirmation message.
if(!strcmppgm2ram(USB_In_Buffer, (const rom char far *)"ok"))
{
strcpypgm2ram(message, (const rom char far *)"MC|");
machineState = CNCMATICCONNECTED;
}
else
{
strcpypgm2ram(message, (const rom char far *)"ERR:MNC|");
machineState = SERIALPORTCONNECTED;
}
putUSBUSART(message, strlen(message));
break;
case WAITINGCOMMAND:
// Tipo de codigo [ G | M ]
movementCommandType = USB_In_Buffer[0];
// Numero de codigo
movementCommandCode[0] = USB_In_Buffer[1];
movementCommandCode[1] = USB_In_Buffer[2];
movementCommandCode[2] = '\0';
// Validamos el comando
if( ValidateCommandReceived(movementCommandType, movementCommandCode, message, &gCode, &mCode) )
{
strcpy(commandReceived, USB_In_Buffer);
machineState = PROCESSINGCOMMAND;
programPaused = false;
}
// mandamos el mensaje correspondiente a la PC
putUSBUSART(message, strlen(message));
break;
default:
break;
}
}
else
{
switch(machineState)
{
case FREEMOVES:
if(freeCode != -2)
{
// seteo a 1 el enable de los motores
LATEbits.LATE2 = 1;
if( freeCode == 1) { StepOnX(0); }
else if( freeCode == 2) { StepOnX(1); }
else if( freeCode == 3) { StepOnY(1); }
else if( freeCode == 4) { StepOnY(0); }
else if( freeCode == 5) { StepOnZ(0); }
else if( freeCode == 6) { StepOnZ(1); }
if(machineState == LIMITSENSOR)
{
freeCode = -2;
limitSensorX = limitSensorY = limitSensorZ = false;
sprintf(message, (const rom char far *)"ERR:SFC_X%ld Y%ld Z%ld|", currentStepsPosition.x, currentStepsPosition.y, currentStepsPosition.z);
putUSBUSART(message, strlen(message));
machineState = FREEMOVES;
}
// seteo a 0 el enable de los motores
LATEbits.LATE2 = 0;
}
break;
case CNCMATICCONNECTED:
MoveToOrigin();
if(machineState == EMERGENCYSTOP)
{
sprintf(message, (const rom char far *)"ERR:PE|");
machineState = SERIALPORTCONNECTED;
}
else
{
strcpypgm2ram(message, (const rom char far *)"PO|");
machineState = WAITINGCOMMAND;
}
putUSBUSART(message, strlen(message));
break;
case PROCESSINGCOMMAND:
// Processing command received
if(gCode != -2)
{
if(gCode == -1)
{
CustomG(commandReceived);
}
else
{
gCodes[gCode](commandReceived);
}
}
if(mCode != -2) { mCodes[mCode](commandReceived); }
// Chequeamos machineState -> si se activo algun fin de carrera
if(machineState == LIMITSENSOR)
{
limitSensorX = limitSensorY = limitSensorZ = false;
sprintf(message, (const rom char far *)"ERR:SFC_X%ld Y%ld Z%ld|", currentStepsPosition.x, currentStepsPosition.y, currentStepsPosition.z);
machineState = SERIALPORTCONNECTED;
}
else if(machineState == EMERGENCYSTOP)
{
sprintf(message, (const rom char far *)"ERR:PE_X%ld Y%ld Z%ld|", currentStepsPosition.x, currentStepsPosition.y, currentStepsPosition.z);
machineState = SERIALPORTCONNECTED;
}
else
{
sprintf(message, (const rom char far *)"CMDDONE_X%ld Y%ld Z%ld|", currentStepsPosition.x, currentStepsPosition.y, currentStepsPosition.z);
if(mCode != 2) machineState = WAITINGCOMMAND;
}
putUSBUSART(message, strlen(message));
// reseteamos variables de comando
gCode = mCode = -2;
break;
default:
break;
}
}
}
endUser:
CDCTxService();
}
############################################################################################################################ */
void motion(int x, int y)
{
/* **************************************************************************************************************************** */
int TempRes;
MDFloat t;
GLint ViewPort[4];
MDFloat Q[4];
MDFloat P[4];
MDFloat s[4];
GLdouble MM[16];
GLdouble PM[16];
/* **************************************************************************************************************************** */
if (MouseState==1) {
xnew=xold+x-xx1;
ynew=yold+y-yy1;
glutPostRedisplay();
}
/* **************************************************************************************************************************** */
if (MouseState==2) {
znew=zold+y-zz1;
glutPostRedisplay();
}
/* **************************************************************************************************************************** */
if ( (MouseState==3) || (MouseState==4) ) {
SetGlobalView(WindowWidth, WindowHeight);
MoveToOrigin();
glGetDoublev(GL_MODELVIEW_MATRIX, MM);
glGetDoublev(GL_PROJECTION_MATRIX, PM);
glGetIntegerv (GL_VIEWPORT, ViewPort);
/*
* Obtain 2 points on line going from cursor
*/
TempRes = gluUnProject( x, ViewPort[3]-y, -1/*z*/,
MM, PM, ViewPort,
&Q[0], &Q[1], &Q[2] );
TempRes = gluUnProject( x, ViewPort[3]-y, 0/*z*/,
MM, PM, ViewPort,
&P[0], &P[1], &P[2] );
/* Find directional vector of this line */
VectSub(P, Q, s);
/* Find intersection with plane in Selected node paralel with projection plane */
t = (-a*Q[0] - b*Q[1] - c*Q[2] - d) / (a*s[0] + b*s[1] + c*s[2]);
PNew[0] = Q[0] + t*s[0];
PNew[1] = Q[1] + t*s[1];
PNew[2] = Q[2] + t*s[2];
if (MouseState==3) FindConfiguration();
glutPostRedisplay();
}
/* **************************************************************************************************************************** */
if (MouseState==6) {
ONew[1] = xold+x-xx1;
if (ONew[1]>= 75) ONew[1] = 74.99;
if (ONew[1]<=-75) ONew[1] = -74.99;
glutPostRedisplay();
}
if (MouseState==7) {
ONew[2] = xold+x-xx1;
if (ONew[2]>= 180) ONew[2] = 179.99;
if (ONew[2]<=-80) ONew[2] = -79.99;
glutPostRedisplay();
}
############################################################################################################################ */
void mouse(int button, int state, int x, int y)
{
/* **************************************************************************************************************************** */
int Modifiers;
/* **************************************************************************************************************************** */
Modifiers = glutGetModifiers();
/* **************************************************************************************************************************** */
if (Modifiers == GLUT_ACTIVE_CTRL) {
if (button==GLUT_LEFT_BUTTON) {
if (state==GLUT_UP) {
MouseState = 0;
glutPostRedisplay();
} else {
selected = PickLink(x, y);
int i;
for (i=0; i<_NO_OF_LINKS_; i++) Skelet[i].UnSelect();
if (selected < _NO_OF_LINKS_) Skelet[selected].Select();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
GLdouble MViewMatrix[4][4];
if ( (selected == _DESIRED_EE_ID_) || (selected == _EE_ID_) ) {
SetGlobalView(WindowWidth, WindowHeight);
MoveToOrigin();
glGetDoublev (GL_MODELVIEW_MATRIX, (MDFloat*)MViewMatrix);
GetXYPlaneEqn(MViewMatrix, PNew, &a, &b, &c, &d);
MouseState = 4;
}
}
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
} else if (Modifiers == GLUT_ACTIVE_SHIFT) {
if (IsEEOrientViewClick(WindowWidth, WindowHeight, x, y)) {
if (state==GLUT_DOWN) {
MouseState = 6;
xx1 = x;
xold = (int)ONew[1];
}
else {
MouseState = 0;
}
} else if (button==GLUT_LEFT_BUTTON) {
if (state==GLUT_UP) {
MouseState = 0;
glutPostRedisplay();
} else {
selected = PickLink(x, y);
int i;
for (i=0; i<_NO_OF_LINKS_; i++) Skelet[i].UnSelect();
if (selected < _NO_OF_LINKS_) Skelet[selected].Select();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
GLdouble MViewMatrix[4][4];
if ( (selected == _DESIRED_EE_ID_) || (selected == _EE_ID_) ) {
SetGlobalView(WindowWidth, WindowHeight);
MoveToOrigin();
glGetDoublev (GL_MODELVIEW_MATRIX, (MDFloat*)MViewMatrix);
GetXYPlaneEqn(MViewMatrix, PNew, &a, &b, &c, &d);
MouseState = 3;
}
}
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
} else {
if (IsEEOrientViewClick(WindowWidth, WindowHeight, x, y)) {
if (state==GLUT_DOWN) {
MouseState = 7;
xold = (int)ONew[2];
xx1 = x;
}
else {
MouseState=0;
}
} else {
if (button==GLUT_LEFT_BUTTON) {
if (state==GLUT_DOWN) {
MouseState=1;
xold=xnew;
yold=ynew;
xx1=x;
yy1=y;
}
else {
MouseState=0;
}
}
if (button==GLUT_RIGHT_BUTTON) {
if (state==GLUT_DOWN) {
MouseState=2;
zold=znew;
zz1=y;
}
else {
MouseState=0;
}
}
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
}
/* **************************************************************************************************************************** */
glutPostRedisplay();
