# 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();