emcsched.cc
1 /******************************************************************** 2 * Description: emcsched.cc 3 * Common functions for program scheduling calls 4 * 5 * Derived from a work by Fred Proctor & Will Shackleford 6 * Further derived from work by jmkasunich, Alex Joni 7 * 8 * Author: Eric H. Johnson 9 * License: GPL Version 2 10 * System: Linux 11 * 12 * Copyright (c) 2009 All rights reserved. 13 * 14 * Last change: 15 ********************************************************************/ 16 17 #include <stdio.h> 18 #include <string.h> 19 #include <stdlib.h> 20 #include <signal.h> 21 #include <unistd.h> 22 #include <ctype.h> 23 #include <math.h> 24 #include <sys/types.h> 25 #include <list> 26 #include <stdint.h> 27 28 #include "rcs.hh" 29 #include "posemath.h" // PM_POSE, TO_RAD 30 #include "emc.hh" // EMC NML 31 #include "emc_nml.hh" 32 #include "canon.hh" // CANON_UNITS, CANON_UNITS_INCHES,MM,CM 33 #include "emcglb.h" // EMC_NMLFILE, TRAJ_MAX_VELOCITY, etc. 34 #include "emccfg.h" // DEFAULT_TRAJ_MAX_VELOCITY 35 #include "inifile.hh" // INIFILE 36 #include "nml_oi.hh" // nmlErrorFormat, NML_ERROR, etc 37 #include "rcs_print.hh" 38 #include "timer.hh" // esleep 39 #include "shcom.hh" // Common NML communications functions 40 #include "emcsched.hh" // Common scheduling functions 41 42 #define MAX_PRIORITY 0x80000000 43 #define POLYNOMIAL 0xD8 /* 11011 followed by 0's */ 44 #define WIDTH (8 * sizeof(crc)) 45 #define TOPBIT (1 << (WIDTH - 1)) 46 47 typedef uint32_t crc; 48 crc crcTable[256]; 49 crc crcResult; 50 int autoTagId = 0; 51 queueStatusType queueStatus = qsStop; 52 53 class SchedEntry { 54 int priority; 55 int tagId; 56 float xpos, ypos, zpos; 57 int zone; 58 string programName; 59 string fileName; 60 float feedOverride; 61 float spindleOverride; 62 int tool; 63 64 public: 65 SchedEntry(); 66 ~SchedEntry(); 67 68 int getPriority() const; 69 void setPriority(int pri); 70 int getTagId() const; 71 void setTagId(int tag); 72 void getOffsets(float &x, float &y, float &z); 73 void setOffsets(float x, float y, float z); 74 int getZone() const; 75 void setZone(int z); 76 string getFileName() const; 77 void setFileName(string s); 78 string getProgramName() const; 79 void setProgramName(string s); 80 float getFeedOverride() const; 81 void setFeedOverride(float f); 82 float getSpindleOverride() const; 83 void setSpindleOverride(float s); 84 int getTool() const; 85 void setTool(int t); 86 }; 87 88 SchedEntry::SchedEntry() { 89 priority = 0; 90 tagId = 0; 91 xpos = 0.0; 92 ypos = 0.0; 93 zpos = 0.0; 94 zone = 0; 95 fileName = ""; 96 feedOverride = 100.0; 97 spindleOverride = 100.0; 98 tool = 1; 99 } 100 101 list<SchedEntry> q; 102 103 bool operator<(const SchedEntry &a, const SchedEntry &b) { 104 return a.getPriority() < b.getPriority(); 105 } 106 107 bool operator==(const SchedEntry &a, const SchedEntry &b) { 108 return a.getPriority() == b.getPriority(); 109 } 110 111 SchedEntry::~SchedEntry() { 112 } 113 114 int SchedEntry::getPriority() const { 115 return priority; 116 } 117 118 void SchedEntry::setPriority(int pri) { 119 priority = pri; 120 } 121 122 int SchedEntry::getTagId() const { 123 return tagId; 124 } 125 126 void SchedEntry::setTagId(int tag) { 127 tagId = tag; 128 } 129 130 void SchedEntry::getOffsets(float &x, float &y, float &z) { 131 x = xpos; 132 y = ypos; 133 z = zpos; 134 } 135 136 void SchedEntry::setOffsets(float x, float y, float z) { 137 xpos = x; 138 ypos = y; 139 zpos = z; 140 } 141 142 int SchedEntry::getZone() const { 143 return zone; 144 } 145 146 void SchedEntry::setZone(int z) { 147 zone = z; 148 } 149 150 string SchedEntry::getFileName() const { 151 return fileName; 152 } 153 154 void SchedEntry::setFileName(string s) { 155 fileName = s; 156 } 157 158 string SchedEntry::getProgramName() const { 159 return programName; 160 } 161 162 void SchedEntry::setProgramName(string s) { 163 programName = s; 164 } 165 166 float SchedEntry::getFeedOverride() const { 167 return feedOverride; 168 } 169 170 void SchedEntry::setFeedOverride(float f) { 171 feedOverride = f; 172 } 173 174 float SchedEntry::getSpindleOverride() const { 175 return spindleOverride; 176 } 177 178 void SchedEntry::setSpindleOverride(float s) { 179 spindleOverride = s; 180 } 181 182 int SchedEntry::getTool() const { 183 return tool; 184 } 185 186 void SchedEntry::setTool(int t) { 187 tool = t; 188 } 189 190 static void crcInit() { 191 crc rmdr; 192 int i; 193 uint32_t bit; 194 195 for (i = 0; i < 256; ++i) 196 { 197 rmdr = i << (WIDTH - 8); 198 for (bit = 8; bit > 0; --bit) 199 { 200 if (rmdr & TOPBIT) 201 { 202 rmdr = (rmdr << 1) ^ POLYNOMIAL; 203 } 204 else 205 { 206 rmdr = (rmdr << 1); 207 } 208 } 209 crcTable[i] = rmdr; 210 } 211 } 212 213 static void crcFast(uint32_t value) { 214 215 uint32_t data; 216 217 data = value ^ (crcResult >> (WIDTH - 8)); 218 crcResult = crcTable[data] ^ (crcResult << 8); 219 } 220 221 static queueStatusType getQueueStatus() { 222 return queueStatus; 223 } 224 225 static void setQueueStatus(queueStatusType qs) { 226 if ((qs == qsResume) && (queueStatus == qsPause)) queueStatus = qsRun; 227 else queueStatus = qs; 228 } 229 230 bool isIdle() 231 { 232 return ((emcStatus->task.interpState != EMC_TASK_INTERP_READING) && 233 (emcStatus->task.interpState != EMC_TASK_INTERP_WAITING) && 234 (emcStatus->task.interpState != EMC_TASK_INTERP_PAUSED)); 235 } 236 237 bool interlocksOk() { 238 if (emcStatus->task.state == EMC_TASK_STATE_ESTOP) { 239 return false; 240 } 241 242 if (emcStatus->task.state != EMC_TASK_STATE_ON) { 243 return false; 244 } 245 return true; 246 } 247 248 void updateQueue() { 249 char fileStr[255]; 250 float x, y, z; 251 char cmd[80]; 252 253 if (queueStatus == qsRun) { 254 if (isIdle() && q.empty()) { 255 queueStatus = qsStop; 256 return; 257 } 258 if (!q.empty()) { 259 if (isIdle()) { 260 q.front().setPriority(MAX_PRIORITY); // Lock job as first job 261 if (interlocksOk()) { 262 sendFeedOverride(((double) q.front().getFeedOverride()) / 100.0); 263 sendSpindleOverride(0, ((double) q.front().getSpindleOverride()) / 100.0); 264 sendMdi(); 265 sendMdiCmd("G92.1\n"); 266 if (emcCommandWaitDone() != 0) { 267 queueStatus = qsError; 268 return; 269 } 270 if (q.front().getZone() != 0) { 271 switch (q.front().getZone()) { 272 case 1: sendMdiCmd("G54\n"); break; 273 case 2: sendMdiCmd("G55\n"); break; 274 case 3: sendMdiCmd("G56\n"); break; 275 case 4: sendMdiCmd("G57\n"); break; 276 case 5: sendMdiCmd("G58\n"); break; 277 case 6: sendMdiCmd("G59\n"); break; 278 case 7: sendMdiCmd("G59.1\n"); break; 279 case 8: sendMdiCmd("G59.2\n"); break; 280 case 9: sendMdiCmd("G59.3\n"); 281 } 282 if (emcCommandWaitDone() != 0) { 283 queueStatus = qsError; 284 return; 285 } 286 } 287 else { 288 q.front().getOffsets(x, y, z); 289 sprintf(cmd, "G0 X%f Y%f Z%f\n", x, y, z); 290 sendMdiCmd(cmd); 291 if (emcCommandWaitDone() != 0) { 292 queueStatus = qsError; 293 return; 294 } 295 sendMdiCmd("G92 X0 Y0 Z0\n"); 296 if (emcCommandWaitDone() != 0) { 297 queueStatus = qsError; 298 return; 299 } 300 } 301 if (sendTaskPlanInit() != 0) { 302 queueStatus = qsError; 303 } 304 sendAuto(); 305 strcpy(fileStr, defaultPath); 306 strcat(fileStr, q.front().getFileName().c_str()); 307 if (sendProgramOpen(fileStr) != 0) { 308 queueStatus = qsError; 309 return; 310 } 311 if (sendProgramRun(0) != 0) { 312 queueStatus = qsError; 313 } 314 q.remove(q.front()); 315 } 316 else queueStatus = qsError; 317 } 318 } 319 } 320 } 321 322 int addProgram(int pri, int tag, float x, float y, float z, int azone, string progName, float feedOvr, float spindleOvr, int toolNum) { 323 SchedEntry p; 324 325 p.setPriority(pri); 326 p.setTagId(tag); 327 if (azone == 0) 328 p.setOffsets(x, y, z); 329 else p.setZone(azone); 330 p.setFileName(progName); 331 p.setFeedOverride(feedOvr); 332 p.setSpindleOverride(spindleOvr); 333 p.setTool(toolNum); 334 q.push_back(p); 335 q.sort(); 336 return q.size(); 337 } 338 339 int getQueueSize() { 340 return q.size(); 341 } 342 343 void clearQueue() { 344 while (!q.empty()) { 345 q.remove(q.front()); 346 } 347 } 348 349 int getStatus() { 350 return (int) getQueueStatus(); 351 } 352 353 void queueStart() { 354 setQueueStatus(qsRun); 355 } 356 357 void queueStop() { 358 setQueueStatus(qsStop); 359 clearQueue(); 360 } 361 362 void queuePause() { 363 setQueueStatus(qsPause); 364 } 365 366 int getProgramById(int id, qRecType *qRec) { 367 list<SchedEntry>::iterator i; 368 369 if (q.size() == 0) return -1; 370 for (i=q.begin(); i!=q.end(); ++i) { 371 if (i->getTagId() == id) break; 372 } 373 if (i->getTagId() != id) return -1; 374 qRec->priority = i->getPriority(); 375 qRec->tagId = i->getTagId(); 376 i->getOffsets(qRec->xpos, qRec->ypos, qRec->zpos); 377 qRec->zone = i->getZone(); 378 strcpy(qRec->fileName, i->getFileName().c_str()); 379 qRec->feedOverride = i->getFeedOverride(); 380 qRec->spindleOverride = i->getSpindleOverride(); 381 qRec->tool = i->getTool(); 382 383 return 0; 384 } 385 386 int getProgramByIndex(int idx, qRecType *qRec) { 387 list<SchedEntry>::iterator i; 388 int index; 389 390 if ((unsigned int) idx >= q.size()) return -1; 391 index = 0; 392 for (i=q.begin(); i!=q.end(); ++i) { 393 if (index == idx) break; 394 index++; 395 } 396 qRec->priority = i->getPriority(); 397 qRec->tagId = i->getTagId(); 398 i->getOffsets(qRec->xpos, qRec->ypos, qRec->zpos); 399 qRec->zone = i->getZone(); 400 strcpy(qRec->fileName, i->getFileName().c_str()); 401 qRec->feedOverride = i->getFeedOverride(); 402 qRec->spindleOverride = i->getSpindleOverride(); 403 qRec->tool = i->getTool(); 404 405 return 0; 406 } 407 408 int getQueueCRC() { 409 list<SchedEntry>::iterator i; 410 411 crcResult = 0; 412 for (i=q.begin(); i!=q.end(); ++i) { 413 crcFast(i->getTagId()); 414 } 415 return crcResult; 416 } 417 418 int getFirstTagId() { 419 if (q.empty()) return 0; 420 return q.front().getTagId(); 421 } 422 423 int getLastTagId() { 424 if (q.empty()) return 0; 425 return q.back().getTagId(); 426 } 427 428 int deleteProgramById(int id) { 429 list<SchedEntry>::iterator i; 430 431 for (i=q.begin(); i!=q.end(); ++i) { 432 if (i->getTagId() == id) { 433 if (i->getPriority() == (int)MAX_PRIORITY) return -1; 434 i = q.erase(i); 435 return 0; 436 } 437 } 438 return -1; 439 } 440 441 int deleteProgramByIndex(int idx) { 442 list<SchedEntry>::iterator i; 443 int index = 0; 444 445 for (i=q.begin(); i!=q.end(); ++i) { 446 if (index == idx) { 447 if (i->getPriority() == (int)MAX_PRIORITY) return -1; 448 i = q.erase(i); 449 return 0; 450 } 451 index++; 452 } 453 return -1; 454 } 455 456 int changePriorityById(int id, int newPriority) { 457 list<SchedEntry>::iterator i; 458 459 for (i=q.begin(); i!=q.end(); ++i) { 460 if (i->getTagId() == id) { 461 if (i->getPriority() == (int)MAX_PRIORITY) return -1; 462 i->setPriority(newPriority); 463 q.sort(); 464 return 0; 465 } 466 } 467 return -1; 468 } 469 470 int changePriorityByIndex(int idx, int newPriority) { 471 list<SchedEntry>::iterator i; 472 int index = 0; 473 474 for (i=q.begin(); i!=q.end(); ++i) { 475 if (index == idx) { 476 if (i->getPriority() == (int)MAX_PRIORITY) return -1; 477 i->setPriority(newPriority); 478 q.sort(); 479 return 0; 480 } 481 index++; 482 } 483 return -1; 484 } 485 486 int getPriorityById(int id, int &pri) { 487 list<SchedEntry>::iterator i; 488 489 for (i=q.begin(); i!=q.end(); ++i) { 490 if (i->getTagId() == id) { 491 pri = i->getPriority(); 492 return 0; 493 } 494 } 495 return -1; 496 } 497 498 int getPriorityByIndex(int idx, int &pri) { 499 list<SchedEntry>::iterator i; 500 int index = 0; 501 502 for (i=q.begin(); i!=q.end(); ++i) { 503 if (index == idx) { 504 pri = i->getPriority(); 505 return 0; 506 } 507 index++; 508 } 509 return -1; 510 } 511 512 int getNextTagId() { 513 return autoTagId++; 514 } 515 516 void resetTagIds(int startId) { 517 autoTagId = startId; 518 } 519 520 void schedInit() { 521 crcInit(); 522 }