void GCodeExport::drill(FILE *fl, const GPoint &point, double drillDepth, int drillingSpeed, int drillingLiftSpeed, double zsafe)
{
char lineBuffer[256];
moveHeadTo(fl, Milling, point, zsafe);
sprintf(lineBuffer,"G01Z%fF%d\n", drillDepth, drillingSpeed);
totalTime += timeForMovement(drillingSpeed, fabs(zsafe - drillDepth));
fputs(lineBuffer, fl);
sprintf(lineBuffer,"G01Z%fF%d\n", zsafe, drillingLiftSpeed);
fputs(lineBuffer, fl);
totalTime += timeForMovement(drillingLiftSpeed, fabs(zsafe - drillDepth));
}
/* Called periodically every getPeriod() seconds */
bool Recognition::updateModule() {
//Here is blocking. What is the non blocking function?
imgLeft = camLeft.read(false);
if(imgLeft != NULL){
left = cvCreateImage(cvSize(imgLeft->width(),imgLeft->height()),IPL_DEPTH_8U, 3 ); //Create an IplImage*
cvCvtColor((IplImage*)imgLeft->getIplImage(), left, CV_RGB2BGR);
Mat leftM = cv::cvarrToMat(left);
if(cameraT == NULL){
cameraT = new camera(left);
}else if(actionDone && left != NULL){
cameraT->getImg(left);
cameraT->getAllObject(actionDone);
//cameraT->getAllObject(true);
if(cameraT->getNbObj() > (int)certaintyLevels.size()){
int nbIter = cameraT->getNbObj() - certaintyLevels.size();
for(int count = 0; count < nbIter; count++){
certaintyLevels.push_back(0);
previousNames.push_back("");
}
}
for(int count = 0; count < cameraT->getNbObj(); count++){
string name = "";
name = cameraT->getObservation(count);
if(name != "nothingNew"){
if(previousNames[count] == name){
certaintyLevels[count] ++;
}else{
certaintyLevels[count] = 0;
previousNames[count] = name;
}
}else{
previousNames[count] == "";
certaintyLevels[count] = 0;
}
if(((certaintyLevels[count] >= 10 && certaintyLevels[count]%10 == 0) || name.find("Moving") != std::string::npos) && actionDone){
Bottle ACKNode;
ACKNode.addString("node");
ACKNode.addInt(2);
switch(cameraT->getShape(count)){
case 0: ACKNode.addString("Marker"); break;
case 1: ACKNode.addString("Car"); break;
}
ACKNode.addString(name);
if((double)cameraT->getPosition(count) > cameraT->getRaw()->width/2.){
ACKNode.addString("Right");
}else{
ACKNode.addString("Left");
}
portAck.write(ACKNode);
}
}
int tempPosition = 0;
if(lookAtHand && actionDone){
tempPosition = cameraT->getHandPosition();
if(tempPosition != -1 && positionHand == tempPosition){
handSeeCount ++;
if(handSeeCount > 30){
bool send = true;
handSeeCount = 0;
Bottle ACKNode;
ACKNode.addString("observation");
ACKNode.addInt(0);
switch(positionHand){
case 0: ACKNode.addString("State_Reach_Push_Left"); break;
case 1: ACKNode.addString("State_Reach_Left"); break;
case 2: ACKNode.addString("State_Reach_Right"); break;
case 3: ACKNode.addString("State_Reach_Push_Right"); break;
default: send = false; break;
}
if(send)
portAck.write(ACKNode);
}
}else{
handSeeCount = 0;
}
}else{
handSeeCount = 0;
}
positionHand = tempPosition;
}
if(left != NULL){
moveHeadTo();
cvWaitKey(1);
cvReleaseImage(&left);
}
}
return true;
}
GCodeExport::GCodeExport(char *outputfile, PathContur *contur1, PathContur *contur2, Mode mode, double depth, double edgedepth, \
double zsafe, int millingSpeed, int edgeMillingSpeed, int spindleRpm, double millingDiameter, \
DrillReader *drillReader, int drillingSpeed, int drillingLiftSpeed, int drillingSpindleSpeed, \
double drillDepth, double drillZSafe, bool sameDiameter , bool isProbe, double filamentDiameter, \
double nozzleDiameter, int extruderTemperature, double printingSpeed, bool edges, double laserEnergy, \
double toolXOffset, double toolYOffset)
{
totalTime = 0.0L;
filamentUsed = 0.0L;
char lineBuffer[1024];
if(contur1==0) {
totalTime = -1.0L;
return;
}
if(contur1->first()==0) {
totalTime = -1.0L;
return;
}
FILE *fl = fopen(outputfile, "w");
if(fl==0) {
totalTime = -1.0L;
return;
}
double firstTime = 0.0L;
double firstFilament = 0.0L;
const double filamentMMPerHeadMM = nozzleDiameter*nozzleDiameter/(filamentDiameter*filamentDiameter);
// writing gcode headers
fputs("%\nG90G40G17G21\n", fl);
switch (mode) {
case Milling:
if(isProbe) {
fputs("G30 (probe Z)\n", fl);
}
fputs("G92 (set founded Z as zero)\n", fl);
sprintf(lineBuffer, "G00Z%f\n", zsafe);
fputs(lineBuffer, fl);
headPosition = GPoint(0.0L,0.0L);
headZPosition = zsafe;
sprintf(lineBuffer, "M03S%d\nG04P%d\n", spindleRpm, spindleRpm/5); // start spindle and wait
fputs(lineBuffer, fl);
doShapes(fl, mode, contur1->first(), millingDiameter, zsafe, depth, millingSpeed, filamentMMPerHeadMM, laserEnergy);
if(edges) {
fputs("(Cutting edges)\n", fl);
doShapes(fl, mode, contur1->edges(), millingDiameter, zsafe, edgedepth, edgeMillingSpeed, filamentMMPerHeadMM, laserEnergy);
fputs("(Cutting edges done)\n", fl);
}
fputs("M5\n", fl);
doDrill(fl, true, drillReader, drillZSafe, sameDiameter, isProbe, drillingSpeed, drillingLiftSpeed, drillingSpindleSpeed, drillDepth, zsafe);
if(contur2) {
if(contur2->first()) {
fputs("G28 (Parking)\n", fl);
headZPosition = zsafe;
changeTool(fl, millingDiameter, zsafe, zsafe, true, isProbe, (char*)"Insert milling tool and turn board", false);
sprintf(lineBuffer, "M03S%d\nG04P%d\n", spindleRpm, spindleRpm/5); // start spindle and wait
fputs(lineBuffer, fl);
doShapes(fl, mode, contur2->first(), millingDiameter, zsafe, depth, millingSpeed, filamentMMPerHeadMM, laserEnergy);
}
}
break;
case Printing:
fputs("G92\n", fl);
sprintf(lineBuffer, "G00Z%f\n", zsafe);
fputs(lineBuffer, fl);
headPosition = GPoint(0.0L,0.0L);
headZPosition = zsafe;
doShapes(fl, mode, contur1->first(), millingDiameter, zsafe, 0.0f, printingSpeed, filamentMMPerHeadMM, laserEnergy);
sprintf(lineBuffer, "G00X%fY%f (Switch to tools coords)\nG92\n", toolXOffset, toolYOffset); // change coords to tool mode
fputs(lineBuffer, fl);
doDrill(fl, true, drillReader, drillZSafe, sameDiameter, isProbe, drillingSpeed, drillingLiftSpeed, drillingSpindleSpeed, drillDepth, zsafe);
if(edges) {
changeTool(fl, millingDiameter, drillZSafe, zsafe, true, isProbe, (char*)"Insert milling tool", true);
sprintf(lineBuffer, "M03S%d\nG04P%d\n", spindleRpm, spindleRpm/5); // start spindle and wait
fputs(lineBuffer, fl);
fputs("(Cutting edges)\n", fl);
doShapes(fl, Milling, contur1->edges(), millingDiameter, zsafe, edgedepth, edgeMillingSpeed, filamentMMPerHeadMM, laserEnergy);
fputs("M5\n", fl);
fputs("(Cutting edges done)\n", fl);
}
if(contur2) {
if(contur2->first()) {
moveHeadTo(fl,mode, GPoint(0.0L, 0.0L), zsafe);
fputs("(Remove milling tool, turn board, prepare pen position manually)\n", fl);
firstTime = totalTime;
firstFilament = filamentUsed;
fl = organizeSecondFile(fl, outputfile, false);
if(fl==0) {
totalTime = -1.0L;
return;
}
headPosition = GPoint(0.0L,0.0L);
headZPosition = 0.0L;
doShapes(fl, mode, contur2->first(), millingDiameter, zsafe, 0.0f, printingSpeed, filamentMMPerHeadMM, laserEnergy);
}
}
break;
case Extrudering:
sprintf(lineBuffer, "M109S%d ; wait temperaturen\n", extruderTemperature);
fputs(lineBuffer, fl);
fputs("M82\n", fl);
fputs("G92 (set all axis to zero)\n", fl);
headPosition = GPoint(0.0L,0.0L);
headZPosition = 0.0L;
ePosition = 0.0L;
fputs("(Start clean extruder)\n", fl);
doShapes(fl, mode, contur1->edges(), nozzleDiameter, zsafe, 0.0f, printingSpeed, filamentMMPerHeadMM, laserEnergy);
fputs("(Extruder cleaning done)\n", fl);
doShapes(fl, mode, contur1->first(), nozzleDiameter, zsafe, 0.0f, printingSpeed, filamentMMPerHeadMM, laserEnergy);
sprintf(lineBuffer, "G00X%fY%f (Switch to tools coords)\nG92\n", toolXOffset, toolYOffset); // change coords to tool mode
fputs(lineBuffer, fl);
doDrill(fl, true, drillReader, drillZSafe, sameDiameter, isProbe, drillingSpeed, drillingLiftSpeed, drillingSpindleSpeed, drillDepth, zsafe);
if(edges) {
changeTool(fl, millingDiameter, drillZSafe, zsafe, true, isProbe, (char*)"Insert milling tool", true);
sprintf(lineBuffer, "M03S%d\nG04P%d\n", spindleRpm, spindleRpm/5); // start spindle and wait
fputs(lineBuffer, fl);
fputs("(Cutting edges)\n", fl);
doShapes(fl, Milling, contur1->edges(), millingDiameter, zsafe, edgedepth, edgeMillingSpeed, filamentMMPerHeadMM, laserEnergy);
fputs("M5\n", fl);
fputs("(Cutting edges done)\n", fl);
}
if(contur2) {
if(contur2->first()) {
moveHeadTo(fl,mode, GPoint(0.0L, 0.0L), zsafe);
fputs("(Turn board, set head Z positon manualy and run second file)\n", fl);
firstTime = totalTime;
firstFilament = filamentUsed;
fl = organizeSecondFile(fl, outputfile, true);
if(fl==0) {
totalTime = -1.0L;
return;
}
headPosition = GPoint(0.0L,0.0L);
headZPosition = 0.0L;
doShapes(fl, mode, contur2->first(), millingDiameter, zsafe, 0.0f, millingSpeed, filamentMMPerHeadMM, laserEnergy);
}
}
break;
case Lasering:
fputs("G92 (set founded Z as zero)\n", fl);
headPosition = GPoint(0.0L,0.0L);
headZPosition = 0.0f;
doShapes(fl, mode, contur1->first(), millingDiameter, 0.0f, depth, printingSpeed, filamentMMPerHeadMM, laserEnergy);
sprintf(lineBuffer, "G00X%fY%f (Switch to tools coords)\nG92\n", toolXOffset, toolYOffset); // change coords to tool mode
fputs(lineBuffer, fl);
doDrill(fl, false, drillReader, drillZSafe, sameDiameter, isProbe, drillingSpeed, drillingLiftSpeed, drillingSpindleSpeed, drillDepth, 0.0f);
if(edges) {
changeTool(fl, millingDiameter, drillZSafe, zsafe, true, isProbe, (char*)"Insert milling tool", true);
sprintf(lineBuffer, "M03S%d\nG04P%d\n", spindleRpm, spindleRpm/5); // start spindle and wait
fputs(lineBuffer, fl);
fputs("(Cutting edges)\n", fl);
doShapes(fl, Milling, contur1->edges(), millingDiameter, zsafe, edgedepth, edgeMillingSpeed, filamentMMPerHeadMM, laserEnergy);
fputs("M5\n", fl);
fputs("(Cutting edges done)\n", fl);
}
fputs("G28 (Parking)\nM0 (Turn board)\n", fl);
sprintf(lineBuffer, "G00X%fY%f (Switch to laser coords)\nG92\n",-toolXOffset, -toolYOffset); // change coords to laser mode
fputs(lineBuffer, fl);
headPosition = GPoint(0.0L,0.0L);
headZPosition = 0.0f;
if(contur2)
if(contur2->first())
doShapes(fl, mode, contur2->first(), millingDiameter, 0.0f, depth, printingSpeed, filamentMMPerHeadMM, laserEnergy);
break;
}
fputs("G28 (parking)\n", fl);
fputs("M30\n", fl);
putFooterComments(fl, mode==Extrudering);
totalTime += firstTime;
filamentUsed += firstFilament;
fclose(fl);
}
void GCodeExport::doShapes(FILE *fl, Mode mode, GShape *shapes, double diameter, double zsafe, double depth, double speed, double filamentMMPerHeadMM, double laserEnergy)
{
// mark all shapes as unused
GShape *nshape = shapes;
while(nshape) {
nshape->used = false;
nshape = nshape->next;
}
while(true) {
double closetShapeDistance = FLT_MAX;
GShape *closestShape = 0;
ShapeClosestpointPlace closestPos = SCP_P1_FROM;
GShape *nextShape = shapes;
while(nextShape) { // looking for the closest shape
if(!nextShape->used) {
double tmpdst;
switch(nextShape->type()) {
case GSHAPE_CIRCLE:
{
GCircle *circle = (GCircle*)nextShape;
if(circle->spanAngle>=2*M_PI) {
if( (tmpdst=fabs(circle->center.distance(headPosition)-circle->radius))<closetShapeDistance ) {
closestShape = nextShape;
closetShapeDistance = tmpdst;
closestPos = SCP_WHOLECIRCLE;
if(tmpdst == 0.0L) goto foundClosest;
}
} else {
if( (tmpdst=circle->fromPoint().distance(headPosition))<closetShapeDistance) {
closestShape = nextShape;
closetShapeDistance = tmpdst;
closestPos = SCP_P1_FROM;
if(tmpdst == 0.0L) goto foundClosest;
}
if( (tmpdst=circle->toPoint().distance(headPosition))<closetShapeDistance) {
closestShape = nextShape;
closetShapeDistance = tmpdst;
closestPos = SCP_P2_TO;
if(tmpdst == 0.0L) goto foundClosest;
}
}
}
break;
case GSHAPE_LINE:
{
GLine *line = (GLine*)nextShape;
if( (tmpdst=line->p1.distance(headPosition))<closetShapeDistance) {
closestShape = nextShape;
closetShapeDistance = tmpdst;
closestPos = SCP_P1_FROM;
if(tmpdst == 0.0L) goto foundClosest;
}
if( (tmpdst=line->p2.distance(headPosition))<closetShapeDistance) {
closestShape = nextShape;
closetShapeDistance = tmpdst;
closestPos = SCP_P2_TO;
if(tmpdst == 0.0L) goto foundClosest;
}
}
break;
case GSHAPE_RECT:
assert("Rect in contur while export!");
break;
}
}
nextShape = nextShape->next;
}
foundClosest:
if(closestShape) { // put shape in gcode file
GPoint st;
GPoint en;
bool isCCW = false;
switch(closestShape->type()){
case GSHAPE_CIRCLE:
{
GCircle *circle = (GCircle*)closestShape;
if(closestPos == SCP_P1_FROM) {
st = circle->fromPoint();
en = circle->toPoint();
isCCW = true;
} else if(closestPos == SCP_P2_TO) {
st = circle->toPoint();
en = circle->fromPoint();
isCCW = false;
} else if(closestPos == SCP_WHOLECIRCLE) {
isCCW = false;
GLine testLine(circle->center, headPosition);
GPoint p1, p2;
int intersectCount = GIntersects::lineWithCircle(testLine,*circle, &p1, &p2, false);
if(intersectCount==0){
st = circle->center+GPoint(circle->radius, 0.0L);
en = st;
} else {
if(intersectCount==1 || p1.distance(headPosition)<p2.distance(headPosition)){
st = p1;
en = p1;
} else {
st = p2;
en = p2;
}
}
}
}
break;
case GSHAPE_LINE:
{
GLine *line = (GLine*)closestShape;
if(closestPos == SCP_P1_FROM) {
st = line->p1;
en = line->p2;
} else if(closestPos == SCP_P2_TO) {
st = line->p2;
en = line->p1;
} else {
assert("Rect in contur while export2!");
}
}
break;
case GSHAPE_RECT:
assert("Rect in contur while export3!");
break;
}
if(st.distance(headPosition)>diameter) {
moveHeadTo(fl, mode, st, zsafe);
} else if (st.distance(headPosition)!=0.0L) {
millingLineTo(fl, mode, st, depth, speed, filamentMMPerHeadMM, laserEnergy, diameter);
}
if(closestShape->type()==GSHAPE_LINE) {
millingLineTo(fl, mode, en, depth, speed, filamentMMPerHeadMM, laserEnergy, diameter);
} else if(closestShape->type()==GSHAPE_CIRCLE) {
GCircle* circle = (GCircle*)closestShape;
millingCircle(fl, mode, ((GCircle*)closestShape)->center,en,depth,speed,isCCW, circle->spanAngle*circle->radius, filamentMMPerHeadMM, laserEnergy*(circle->outer?1.0L:1.5L), diameter);
}
closestShape->used = true;
} else
break; // from while(true)
}
if(headZPosition!=zsafe) {
char lineBuffer[256];
// raise head
sprintf(lineBuffer, "G00Z%f\n", zsafe); // for the same style numeric
fputs(lineBuffer, fl);
totalTime += timeForMovement(SPEED_ON_IDLE, zsafe-headZPosition);
headZPosition = zsafe;
}
}
