GCodeTimeEstimator.cpp 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005
  1. #include "GCodeTimeEstimator.hpp"
  2. #include <boost/bind.hpp>
  3. #include <cmath>
  4. #include <Shiny/Shiny.h>
  5. static const float MMMIN_TO_MMSEC = 1.0f / 60.0f;
  6. static const float MILLISEC_TO_SEC = 0.001f;
  7. static const float INCHES_TO_MM = 25.4f;
  8. static const float DEFAULT_FEEDRATE = 1500.0f; // from Prusa Firmware (Marlin_main.cpp)
  9. static const float DEFAULT_ACCELERATION = 1500.0f; // Prusa Firmware 1_75mm_MK2
  10. static const float DEFAULT_RETRACT_ACCELERATION = 1500.0f; // Prusa Firmware 1_75mm_MK2
  11. static const float DEFAULT_AXIS_MAX_FEEDRATE[] = { 500.0f, 500.0f, 12.0f, 120.0f }; // Prusa Firmware 1_75mm_MK2
  12. static const float DEFAULT_AXIS_MAX_ACCELERATION[] = { 9000.0f, 9000.0f, 500.0f, 10000.0f }; // Prusa Firmware 1_75mm_MK2
  13. static const float DEFAULT_AXIS_MAX_JERK[] = { 10.0f, 10.0f, 0.2f, 2.5f }; // from Prusa Firmware (Configuration.h)
  14. static const float DEFAULT_MINIMUM_FEEDRATE = 0.0f; // from Prusa Firmware (Configuration_adv.h)
  15. static const float DEFAULT_MINIMUM_TRAVEL_FEEDRATE = 0.0f; // from Prusa Firmware (Configuration_adv.h)
  16. static const float PREVIOUS_FEEDRATE_THRESHOLD = 0.0001f;
  17. namespace Slic3r {
  18. void GCodeTimeEstimator::Feedrates::reset()
  19. {
  20. feedrate = 0.0f;
  21. safe_feedrate = 0.0f;
  22. ::memset(axis_feedrate, 0, Num_Axis * sizeof(float));
  23. ::memset(abs_axis_feedrate, 0, Num_Axis * sizeof(float));
  24. }
  25. float GCodeTimeEstimator::Block::Trapezoid::acceleration_time(float acceleration) const
  26. {
  27. return acceleration_time_from_distance(feedrate.entry, accelerate_until, acceleration);
  28. }
  29. float GCodeTimeEstimator::Block::Trapezoid::cruise_time() const
  30. {
  31. return (feedrate.cruise != 0.0f) ? cruise_distance() / feedrate.cruise : 0.0f;
  32. }
  33. float GCodeTimeEstimator::Block::Trapezoid::deceleration_time(float acceleration) const
  34. {
  35. return acceleration_time_from_distance(feedrate.cruise, (distance - decelerate_after), -acceleration);
  36. }
  37. float GCodeTimeEstimator::Block::Trapezoid::cruise_distance() const
  38. {
  39. return decelerate_after - accelerate_until;
  40. }
  41. float GCodeTimeEstimator::Block::Trapezoid::acceleration_time_from_distance(float initial_feedrate, float distance, float acceleration)
  42. {
  43. return (acceleration != 0.0f) ? (speed_from_distance(initial_feedrate, distance, acceleration) - initial_feedrate) / acceleration : 0.0f;
  44. }
  45. float GCodeTimeEstimator::Block::Trapezoid::speed_from_distance(float initial_feedrate, float distance, float acceleration)
  46. {
  47. return ::sqrt(sqr(initial_feedrate) + 2.0f * acceleration * distance);
  48. }
  49. float GCodeTimeEstimator::Block::move_length() const
  50. {
  51. float length = ::sqrt(sqr(delta_pos[X]) + sqr(delta_pos[Y]) + sqr(delta_pos[Z]));
  52. return (length > 0.0f) ? length : std::abs(delta_pos[E]);
  53. }
  54. float GCodeTimeEstimator::Block::is_extruder_only_move() const
  55. {
  56. return (delta_pos[X] == 0.0f) && (delta_pos[Y] == 0.0f) && (delta_pos[Z] == 0.0f) && (delta_pos[E] != 0.0f);
  57. }
  58. float GCodeTimeEstimator::Block::is_travel_move() const
  59. {
  60. return delta_pos[E] == 0.0f;
  61. }
  62. float GCodeTimeEstimator::Block::acceleration_time() const
  63. {
  64. return trapezoid.acceleration_time(acceleration);
  65. }
  66. float GCodeTimeEstimator::Block::cruise_time() const
  67. {
  68. return trapezoid.cruise_time();
  69. }
  70. float GCodeTimeEstimator::Block::deceleration_time() const
  71. {
  72. return trapezoid.deceleration_time(acceleration);
  73. }
  74. float GCodeTimeEstimator::Block::cruise_distance() const
  75. {
  76. return trapezoid.cruise_distance();
  77. }
  78. void GCodeTimeEstimator::Block::calculate_trapezoid()
  79. {
  80. float distance = move_length();
  81. trapezoid.distance = distance;
  82. trapezoid.feedrate = feedrate;
  83. float accelerate_distance = estimate_acceleration_distance(feedrate.entry, feedrate.cruise, acceleration);
  84. float decelerate_distance = estimate_acceleration_distance(feedrate.cruise, feedrate.exit, -acceleration);
  85. float cruise_distance = distance - accelerate_distance - decelerate_distance;
  86. // Not enough space to reach the nominal feedrate.
  87. // This means no cruising, and we'll have to use intersection_distance() to calculate when to abort acceleration
  88. // and start braking in order to reach the exit_feedrate exactly at the end of this block.
  89. if (cruise_distance < 0.0f)
  90. {
  91. accelerate_distance = clamp(0.0f, distance, intersection_distance(feedrate.entry, feedrate.exit, acceleration, distance));
  92. cruise_distance = 0.0f;
  93. trapezoid.feedrate.cruise = Trapezoid::speed_from_distance(feedrate.entry, accelerate_distance, acceleration);
  94. }
  95. trapezoid.accelerate_until = accelerate_distance;
  96. trapezoid.decelerate_after = accelerate_distance + cruise_distance;
  97. }
  98. float GCodeTimeEstimator::Block::max_allowable_speed(float acceleration, float target_velocity, float distance)
  99. {
  100. return ::sqrt(sqr(target_velocity) - 2.0f * acceleration * distance);
  101. }
  102. float GCodeTimeEstimator::Block::estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
  103. {
  104. return (acceleration == 0.0f) ? 0.0f : (sqr(target_rate) - sqr(initial_rate)) / (2.0f * acceleration);
  105. }
  106. float GCodeTimeEstimator::Block::intersection_distance(float initial_rate, float final_rate, float acceleration, float distance)
  107. {
  108. return (acceleration == 0.0f) ? 0.0f : (2.0f * acceleration * distance - sqr(initial_rate) + sqr(final_rate)) / (4.0f * acceleration);
  109. }
  110. GCodeTimeEstimator::GCodeTimeEstimator()
  111. {
  112. reset();
  113. set_default();
  114. }
  115. void GCodeTimeEstimator::calculate_time_from_text(const std::string& gcode)
  116. {
  117. _parser.parse(gcode, boost::bind(&GCodeTimeEstimator::_process_gcode_line, this, _1, _2));
  118. _calculate_time();
  119. reset();
  120. }
  121. void GCodeTimeEstimator::calculate_time_from_file(const std::string& file)
  122. {
  123. _parser.parse_file(file, boost::bind(&GCodeTimeEstimator::_process_gcode_line, this, _1, _2));
  124. _calculate_time();
  125. reset();
  126. }
  127. void GCodeTimeEstimator::calculate_time_from_lines(const std::vector<std::string>& gcode_lines)
  128. {
  129. for (const std::string& line : gcode_lines)
  130. {
  131. _parser.parse_line(line,
  132. [this](GCodeReader &reader, const GCodeReader::GCodeLine &line)
  133. { this->_process_gcode_line(reader, line); });
  134. }
  135. _calculate_time();
  136. reset();
  137. }
  138. void GCodeTimeEstimator::add_gcode_line(const std::string& gcode_line)
  139. {
  140. PROFILE_FUNC();
  141. _parser.parse_line(gcode_line,
  142. [this](GCodeReader &reader, const GCodeReader::GCodeLine &line)
  143. { this->_process_gcode_line(reader, line); });
  144. }
  145. void GCodeTimeEstimator::add_gcode_block(const char *ptr)
  146. {
  147. PROFILE_FUNC();
  148. GCodeReader::GCodeLine gline;
  149. for (; *ptr != 0;) {
  150. gline.reset();
  151. ptr = _parser.parse_line(ptr, gline,
  152. [this](GCodeReader &reader, const GCodeReader::GCodeLine &line)
  153. { this->_process_gcode_line(reader, line); });
  154. }
  155. }
  156. void GCodeTimeEstimator::calculate_time()
  157. {
  158. PROFILE_FUNC();
  159. _calculate_time();
  160. _reset();
  161. }
  162. void GCodeTimeEstimator::set_axis_position(EAxis axis, float position)
  163. {
  164. _state.axis[axis].position = position;
  165. }
  166. void GCodeTimeEstimator::set_axis_max_feedrate(EAxis axis, float feedrate_mm_sec)
  167. {
  168. _state.axis[axis].max_feedrate = feedrate_mm_sec;
  169. }
  170. void GCodeTimeEstimator::set_axis_max_acceleration(EAxis axis, float acceleration)
  171. {
  172. _state.axis[axis].max_acceleration = acceleration;
  173. }
  174. void GCodeTimeEstimator::set_axis_max_jerk(EAxis axis, float jerk)
  175. {
  176. _state.axis[axis].max_jerk = jerk;
  177. }
  178. float GCodeTimeEstimator::get_axis_position(EAxis axis) const
  179. {
  180. return _state.axis[axis].position;
  181. }
  182. float GCodeTimeEstimator::get_axis_max_feedrate(EAxis axis) const
  183. {
  184. return _state.axis[axis].max_feedrate;
  185. }
  186. float GCodeTimeEstimator::get_axis_max_acceleration(EAxis axis) const
  187. {
  188. return _state.axis[axis].max_acceleration;
  189. }
  190. float GCodeTimeEstimator::get_axis_max_jerk(EAxis axis) const
  191. {
  192. return _state.axis[axis].max_jerk;
  193. }
  194. void GCodeTimeEstimator::set_feedrate(float feedrate_mm_sec)
  195. {
  196. _state.feedrate = feedrate_mm_sec;
  197. }
  198. float GCodeTimeEstimator::get_feedrate() const
  199. {
  200. return _state.feedrate;
  201. }
  202. void GCodeTimeEstimator::set_acceleration(float acceleration_mm_sec2)
  203. {
  204. _state.acceleration = acceleration_mm_sec2;
  205. }
  206. float GCodeTimeEstimator::get_acceleration() const
  207. {
  208. return _state.acceleration;
  209. }
  210. void GCodeTimeEstimator::set_retract_acceleration(float acceleration_mm_sec2)
  211. {
  212. _state.retract_acceleration = acceleration_mm_sec2;
  213. }
  214. float GCodeTimeEstimator::get_retract_acceleration() const
  215. {
  216. return _state.retract_acceleration;
  217. }
  218. void GCodeTimeEstimator::set_minimum_feedrate(float feedrate_mm_sec)
  219. {
  220. _state.minimum_feedrate = feedrate_mm_sec;
  221. }
  222. float GCodeTimeEstimator::get_minimum_feedrate() const
  223. {
  224. return _state.minimum_feedrate;
  225. }
  226. void GCodeTimeEstimator::set_minimum_travel_feedrate(float feedrate_mm_sec)
  227. {
  228. _state.minimum_travel_feedrate = feedrate_mm_sec;
  229. }
  230. float GCodeTimeEstimator::get_minimum_travel_feedrate() const
  231. {
  232. return _state.minimum_travel_feedrate;
  233. }
  234. void GCodeTimeEstimator::set_dialect(GCodeTimeEstimator::EDialect dialect)
  235. {
  236. _state.dialect = dialect;
  237. }
  238. GCodeTimeEstimator::EDialect GCodeTimeEstimator::get_dialect() const
  239. {
  240. return _state.dialect;
  241. }
  242. void GCodeTimeEstimator::set_units(GCodeTimeEstimator::EUnits units)
  243. {
  244. _state.units = units;
  245. }
  246. GCodeTimeEstimator::EUnits GCodeTimeEstimator::get_units() const
  247. {
  248. return _state.units;
  249. }
  250. void GCodeTimeEstimator::set_positioning_xyz_type(GCodeTimeEstimator::EPositioningType type)
  251. {
  252. _state.positioning_xyz_type = type;
  253. }
  254. GCodeTimeEstimator::EPositioningType GCodeTimeEstimator::get_positioning_xyz_type() const
  255. {
  256. return _state.positioning_xyz_type;
  257. }
  258. void GCodeTimeEstimator::set_positioning_e_type(GCodeTimeEstimator::EPositioningType type)
  259. {
  260. _state.positioning_e_type = type;
  261. }
  262. GCodeTimeEstimator::EPositioningType GCodeTimeEstimator::get_positioning_e_type() const
  263. {
  264. return _state.positioning_e_type;
  265. }
  266. void GCodeTimeEstimator::add_additional_time(float timeSec)
  267. {
  268. _state.additional_time += timeSec;
  269. }
  270. void GCodeTimeEstimator::set_additional_time(float timeSec)
  271. {
  272. _state.additional_time = timeSec;
  273. }
  274. float GCodeTimeEstimator::get_additional_time() const
  275. {
  276. return _state.additional_time;
  277. }
  278. void GCodeTimeEstimator::set_default()
  279. {
  280. set_units(Millimeters);
  281. set_dialect(Unknown);
  282. set_positioning_xyz_type(Absolute);
  283. set_positioning_e_type(Relative);
  284. set_feedrate(DEFAULT_FEEDRATE);
  285. set_acceleration(DEFAULT_ACCELERATION);
  286. set_retract_acceleration(DEFAULT_RETRACT_ACCELERATION);
  287. set_minimum_feedrate(DEFAULT_MINIMUM_FEEDRATE);
  288. set_minimum_travel_feedrate(DEFAULT_MINIMUM_TRAVEL_FEEDRATE);
  289. for (unsigned char a = X; a < Num_Axis; ++a)
  290. {
  291. EAxis axis = (EAxis)a;
  292. set_axis_max_feedrate(axis, DEFAULT_AXIS_MAX_FEEDRATE[a]);
  293. set_axis_max_acceleration(axis, DEFAULT_AXIS_MAX_ACCELERATION[a]);
  294. set_axis_max_jerk(axis, DEFAULT_AXIS_MAX_JERK[a]);
  295. }
  296. }
  297. void GCodeTimeEstimator::reset()
  298. {
  299. _blocks.clear();
  300. _reset();
  301. }
  302. float GCodeTimeEstimator::get_time() const
  303. {
  304. return _time;
  305. }
  306. std::string GCodeTimeEstimator::get_time_hms() const
  307. {
  308. float timeinsecs = get_time();
  309. int hours = (int)(timeinsecs / 3600.0f);
  310. timeinsecs -= (float)hours * 3600.0f;
  311. int minutes = (int)(timeinsecs / 60.0f);
  312. timeinsecs -= (float)minutes * 60.0f;
  313. char buffer[64];
  314. if (hours > 0)
  315. ::sprintf(buffer, "%dh %dm %ds", hours, minutes, (int)timeinsecs);
  316. else if (minutes > 0)
  317. ::sprintf(buffer, "%dm %ds", minutes, (int)timeinsecs);
  318. else
  319. ::sprintf(buffer, "%ds", (int)timeinsecs);
  320. return buffer;
  321. }
  322. void GCodeTimeEstimator::_reset()
  323. {
  324. _curr.reset();
  325. _prev.reset();
  326. set_axis_position(X, 0.0f);
  327. set_axis_position(Y, 0.0f);
  328. set_axis_position(Z, 0.0f);
  329. set_additional_time(0.0f);
  330. }
  331. void GCodeTimeEstimator::_calculate_time()
  332. {
  333. _forward_pass();
  334. _reverse_pass();
  335. _recalculate_trapezoids();
  336. _time = get_additional_time();
  337. for (const Block& block : _blocks)
  338. {
  339. _time += block.acceleration_time();
  340. _time += block.cruise_time();
  341. _time += block.deceleration_time();
  342. }
  343. }
  344. void GCodeTimeEstimator::_process_gcode_line(GCodeReader&, const GCodeReader::GCodeLine& line)
  345. {
  346. PROFILE_FUNC();
  347. std::string cmd = line.cmd();
  348. if (cmd.length() > 1)
  349. {
  350. switch (::toupper(cmd[0]))
  351. {
  352. case 'G':
  353. {
  354. switch (::atoi(&cmd[1]))
  355. {
  356. case 1: // Move
  357. {
  358. _processG1(line);
  359. break;
  360. }
  361. case 4: // Dwell
  362. {
  363. _processG4(line);
  364. break;
  365. }
  366. case 20: // Set Units to Inches
  367. {
  368. _processG20(line);
  369. break;
  370. }
  371. case 21: // Set Units to Millimeters
  372. {
  373. _processG21(line);
  374. break;
  375. }
  376. case 28: // Move to Origin (Home)
  377. {
  378. _processG28(line);
  379. break;
  380. }
  381. case 90: // Set to Absolute Positioning
  382. {
  383. _processG90(line);
  384. break;
  385. }
  386. case 91: // Set to Relative Positioning
  387. {
  388. _processG91(line);
  389. break;
  390. }
  391. case 92: // Set Position
  392. {
  393. _processG92(line);
  394. break;
  395. }
  396. }
  397. break;
  398. }
  399. case 'M':
  400. {
  401. switch (::atoi(&cmd[1]))
  402. {
  403. case 82: // Set extruder to absolute mode
  404. {
  405. _processM82(line);
  406. break;
  407. }
  408. case 83: // Set extruder to relative mode
  409. {
  410. _processM83(line);
  411. break;
  412. }
  413. case 109: // Set Extruder Temperature and Wait
  414. {
  415. _processM109(line);
  416. break;
  417. }
  418. case 201: // Set max printing acceleration
  419. {
  420. _processM201(line);
  421. break;
  422. }
  423. case 203: // Set maximum feedrate
  424. {
  425. _processM203(line);
  426. break;
  427. }
  428. case 204: // Set default acceleration
  429. {
  430. _processM204(line);
  431. break;
  432. }
  433. case 205: // Advanced settings
  434. {
  435. _processM205(line);
  436. break;
  437. }
  438. case 566: // Set allowable instantaneous speed change
  439. {
  440. _processM566(line);
  441. break;
  442. }
  443. }
  444. break;
  445. }
  446. }
  447. }
  448. }
  449. // Returns the new absolute position on the given axis in dependence of the given parameters
  450. float axis_absolute_position_from_G1_line(GCodeTimeEstimator::EAxis axis, const GCodeReader::GCodeLine& lineG1, GCodeTimeEstimator::EUnits units, GCodeTimeEstimator::EPositioningType type, float current_absolute_position)
  451. {
  452. float lengthsScaleFactor = (units == GCodeTimeEstimator::Inches) ? INCHES_TO_MM : 1.0f;
  453. if (lineG1.has(Slic3r::Axis(axis)))
  454. {
  455. float ret = lineG1.value(Slic3r::Axis(axis)) * lengthsScaleFactor;
  456. return (type == GCodeTimeEstimator::Absolute) ? ret : current_absolute_position + ret;
  457. }
  458. else
  459. return current_absolute_position;
  460. }
  461. void GCodeTimeEstimator::_processG1(const GCodeReader::GCodeLine& line)
  462. {
  463. // updates axes positions from line
  464. EUnits units = get_units();
  465. float new_pos[Num_Axis];
  466. for (unsigned char a = X; a < Num_Axis; ++a)
  467. {
  468. new_pos[a] = axis_absolute_position_from_G1_line((EAxis)a, line, units, (a == E) ? get_positioning_e_type() : get_positioning_xyz_type(), get_axis_position((EAxis)a));
  469. }
  470. // updates feedrate from line, if present
  471. if (line.has_f())
  472. set_feedrate(std::max(line.f() * MMMIN_TO_MMSEC, get_minimum_feedrate()));
  473. // fills block data
  474. Block block;
  475. // calculates block movement deltas
  476. float max_abs_delta = 0.0f;
  477. for (unsigned char a = X; a < Num_Axis; ++a)
  478. {
  479. block.delta_pos[a] = new_pos[a] - get_axis_position((EAxis)a);
  480. max_abs_delta = std::max(max_abs_delta, std::abs(block.delta_pos[a]));
  481. }
  482. // is it a move ?
  483. if (max_abs_delta == 0.0f)
  484. return;
  485. // calculates block feedrate
  486. _curr.feedrate = std::max(get_feedrate(), block.is_travel_move() ? get_minimum_travel_feedrate() : get_minimum_feedrate());
  487. float distance = block.move_length();
  488. float invDistance = 1.0f / distance;
  489. float min_feedrate_factor = 1.0f;
  490. for (unsigned char a = X; a < Num_Axis; ++a)
  491. {
  492. _curr.axis_feedrate[a] = _curr.feedrate * block.delta_pos[a] * invDistance;
  493. _curr.abs_axis_feedrate[a] = std::abs(_curr.axis_feedrate[a]);
  494. if (_curr.abs_axis_feedrate[a] > 0.0f)
  495. min_feedrate_factor = std::min(min_feedrate_factor, get_axis_max_feedrate((EAxis)a) / _curr.abs_axis_feedrate[a]);
  496. }
  497. block.feedrate.cruise = min_feedrate_factor * _curr.feedrate;
  498. for (unsigned char a = X; a < Num_Axis; ++a)
  499. {
  500. _curr.axis_feedrate[a] *= min_feedrate_factor;
  501. _curr.abs_axis_feedrate[a] *= min_feedrate_factor;
  502. }
  503. // calculates block acceleration
  504. float acceleration = block.is_extruder_only_move() ? get_retract_acceleration() : get_acceleration();
  505. for (unsigned char a = X; a < Num_Axis; ++a)
  506. {
  507. float axis_max_acceleration = get_axis_max_acceleration((EAxis)a);
  508. if (acceleration * std::abs(block.delta_pos[a]) * invDistance > axis_max_acceleration)
  509. acceleration = axis_max_acceleration;
  510. }
  511. block.acceleration = acceleration;
  512. // calculates block exit feedrate
  513. _curr.safe_feedrate = block.feedrate.cruise;
  514. for (unsigned char a = X; a < Num_Axis; ++a)
  515. {
  516. float axis_max_jerk = get_axis_max_jerk((EAxis)a);
  517. if (_curr.abs_axis_feedrate[a] > axis_max_jerk)
  518. _curr.safe_feedrate = std::min(_curr.safe_feedrate, axis_max_jerk);
  519. }
  520. block.feedrate.exit = _curr.safe_feedrate;
  521. // calculates block entry feedrate
  522. float vmax_junction = _curr.safe_feedrate;
  523. if (!_blocks.empty() && (_prev.feedrate > PREVIOUS_FEEDRATE_THRESHOLD))
  524. {
  525. bool prev_speed_larger = _prev.feedrate > block.feedrate.cruise;
  526. float smaller_speed_factor = prev_speed_larger ? (block.feedrate.cruise / _prev.feedrate) : (_prev.feedrate / block.feedrate.cruise);
  527. // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
  528. vmax_junction = prev_speed_larger ? block.feedrate.cruise : _prev.feedrate;
  529. float v_factor = 1.0f;
  530. bool limited = false;
  531. for (unsigned char a = X; a < Num_Axis; ++a)
  532. {
  533. // Limit an axis. We have to differentiate coasting from the reversal of an axis movement, or a full stop.
  534. float v_exit = _prev.axis_feedrate[a];
  535. float v_entry = _curr.axis_feedrate[a];
  536. if (prev_speed_larger)
  537. v_exit *= smaller_speed_factor;
  538. if (limited)
  539. {
  540. v_exit *= v_factor;
  541. v_entry *= v_factor;
  542. }
  543. // Calculate the jerk depending on whether the axis is coasting in the same direction or reversing a direction.
  544. float jerk =
  545. (v_exit > v_entry) ?
  546. (((v_entry > 0.0f) || (v_exit < 0.0f)) ?
  547. // coasting
  548. (v_exit - v_entry) :
  549. // axis reversal
  550. std::max(v_exit, -v_entry)) :
  551. // v_exit <= v_entry
  552. (((v_entry < 0.0f) || (v_exit > 0.0f)) ?
  553. // coasting
  554. (v_entry - v_exit) :
  555. // axis reversal
  556. std::max(-v_exit, v_entry));
  557. float axis_max_jerk = get_axis_max_jerk((EAxis)a);
  558. if (jerk > axis_max_jerk)
  559. {
  560. v_factor *= axis_max_jerk / jerk;
  561. limited = true;
  562. }
  563. }
  564. if (limited)
  565. vmax_junction *= v_factor;
  566. // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
  567. // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
  568. float vmax_junction_threshold = vmax_junction * 0.99f;
  569. // Not coasting. The machine will stop and start the movements anyway, better to start the segment from start.
  570. if ((_prev.safe_feedrate > vmax_junction_threshold) && (_curr.safe_feedrate > vmax_junction_threshold))
  571. vmax_junction = _curr.safe_feedrate;
  572. }
  573. float v_allowable = Block::max_allowable_speed(-acceleration, _curr.safe_feedrate, distance);
  574. block.feedrate.entry = std::min(vmax_junction, v_allowable);
  575. block.max_entry_speed = vmax_junction;
  576. block.flags.nominal_length = (block.feedrate.cruise <= v_allowable);
  577. block.flags.recalculate = true;
  578. block.safe_feedrate = _curr.safe_feedrate;
  579. // calculates block trapezoid
  580. block.calculate_trapezoid();
  581. // updates previous
  582. _prev = _curr;
  583. // updates axis positions
  584. for (unsigned char a = X; a < Num_Axis; ++a)
  585. {
  586. set_axis_position((EAxis)a, new_pos[a]);
  587. }
  588. // adds block to blocks list
  589. _blocks.emplace_back(block);
  590. }
  591. void GCodeTimeEstimator::_processG4(const GCodeReader::GCodeLine& line)
  592. {
  593. EDialect dialect = get_dialect();
  594. float value;
  595. if (line.has_value('P', value))
  596. add_additional_time(value * MILLISEC_TO_SEC);
  597. // see: http://reprap.org/wiki/G-code#G4:_Dwell
  598. if ((dialect == Repetier) ||
  599. (dialect == Marlin) ||
  600. (dialect == Smoothieware) ||
  601. (dialect == RepRapFirmware))
  602. {
  603. if (line.has_value('S', value))
  604. add_additional_time(value);
  605. }
  606. }
  607. void GCodeTimeEstimator::_processG20(const GCodeReader::GCodeLine& line)
  608. {
  609. set_units(Inches);
  610. }
  611. void GCodeTimeEstimator::_processG21(const GCodeReader::GCodeLine& line)
  612. {
  613. set_units(Millimeters);
  614. }
  615. void GCodeTimeEstimator::_processG28(const GCodeReader::GCodeLine& line)
  616. {
  617. // TODO
  618. }
  619. void GCodeTimeEstimator::_processG90(const GCodeReader::GCodeLine& line)
  620. {
  621. set_positioning_xyz_type(Absolute);
  622. }
  623. void GCodeTimeEstimator::_processG91(const GCodeReader::GCodeLine& line)
  624. {
  625. // TODO: THERE ARE DIALECT VARIANTS
  626. set_positioning_xyz_type(Relative);
  627. }
  628. void GCodeTimeEstimator::_processM82(const GCodeReader::GCodeLine& line)
  629. {
  630. set_positioning_e_type(Absolute);
  631. }
  632. void GCodeTimeEstimator::_processM83(const GCodeReader::GCodeLine& line)
  633. {
  634. set_positioning_e_type(Relative);
  635. }
  636. void GCodeTimeEstimator::_processG92(const GCodeReader::GCodeLine& line)
  637. {
  638. float lengthsScaleFactor = (get_units() == Inches) ? INCHES_TO_MM : 1.0f;
  639. bool anyFound = false;
  640. if (line.has_x())
  641. {
  642. set_axis_position(X, line.x() * lengthsScaleFactor);
  643. anyFound = true;
  644. }
  645. if (line.has_y())
  646. {
  647. set_axis_position(Y, line.y() * lengthsScaleFactor);
  648. anyFound = true;
  649. }
  650. if (line.has_z())
  651. {
  652. set_axis_position(Z, line.z() * lengthsScaleFactor);
  653. anyFound = true;
  654. }
  655. if (line.has_e())
  656. {
  657. set_axis_position(E, line.e() * lengthsScaleFactor);
  658. anyFound = true;
  659. }
  660. if (!anyFound)
  661. {
  662. for (unsigned char a = X; a < Num_Axis; ++a)
  663. {
  664. set_axis_position((EAxis)a, 0.0f);
  665. }
  666. }
  667. }
  668. void GCodeTimeEstimator::_processM109(const GCodeReader::GCodeLine& line)
  669. {
  670. // TODO
  671. }
  672. void GCodeTimeEstimator::_processM201(const GCodeReader::GCodeLine& line)
  673. {
  674. EDialect dialect = get_dialect();
  675. // see http://reprap.org/wiki/G-code#M201:_Set_max_printing_acceleration
  676. float factor = ((dialect != RepRapFirmware) && (get_units() == GCodeTimeEstimator::Inches)) ? INCHES_TO_MM : 1.0f;
  677. if (line.has_x())
  678. set_axis_max_acceleration(X, line.x() * factor);
  679. if (line.has_y())
  680. set_axis_max_acceleration(Y, line.y() * factor);
  681. if (line.has_z())
  682. set_axis_max_acceleration(Z, line.z() * factor);
  683. if (line.has_e())
  684. set_axis_max_acceleration(E, line.e() * factor);
  685. }
  686. void GCodeTimeEstimator::_processM203(const GCodeReader::GCodeLine& line)
  687. {
  688. EDialect dialect = get_dialect();
  689. // see http://reprap.org/wiki/G-code#M203:_Set_maximum_feedrate
  690. if (dialect == Repetier)
  691. return;
  692. // see http://reprap.org/wiki/G-code#M203:_Set_maximum_feedrate
  693. float factor = (dialect == Marlin) ? 1.0f : MMMIN_TO_MMSEC;
  694. if (line.has_x())
  695. set_axis_max_feedrate(X, line.x() * factor);
  696. if (line.has_y())
  697. set_axis_max_feedrate(Y, line.y() * factor);
  698. if (line.has_z())
  699. set_axis_max_feedrate(Z, line.z() * factor);
  700. if (line.has_e())
  701. set_axis_max_feedrate(E, line.e() * factor);
  702. }
  703. void GCodeTimeEstimator::_processM204(const GCodeReader::GCodeLine& line)
  704. {
  705. float value;
  706. if (line.has_value('S', value))
  707. set_acceleration(value);
  708. if (line.has_value('T', value))
  709. set_retract_acceleration(value);
  710. }
  711. void GCodeTimeEstimator::_processM205(const GCodeReader::GCodeLine& line)
  712. {
  713. if (line.has_x())
  714. {
  715. float max_jerk = line.x();
  716. set_axis_max_jerk(X, max_jerk);
  717. set_axis_max_jerk(Y, max_jerk);
  718. }
  719. if (line.has_y())
  720. set_axis_max_jerk(Y, line.y());
  721. if (line.has_z())
  722. set_axis_max_jerk(Z, line.z());
  723. if (line.has_e())
  724. set_axis_max_jerk(E, line.e());
  725. float value;
  726. if (line.has_value('S', value))
  727. set_minimum_feedrate(value);
  728. if (line.has_value('T', value))
  729. set_minimum_travel_feedrate(value);
  730. }
  731. void GCodeTimeEstimator::_processM566(const GCodeReader::GCodeLine& line)
  732. {
  733. if (line.has_x())
  734. set_axis_max_jerk(X, line.x() * MMMIN_TO_MMSEC);
  735. if (line.has_y())
  736. set_axis_max_jerk(Y, line.y() * MMMIN_TO_MMSEC);
  737. if (line.has_z())
  738. set_axis_max_jerk(Z, line.z() * MMMIN_TO_MMSEC);
  739. if (line.has_e())
  740. set_axis_max_jerk(E, line.e() * MMMIN_TO_MMSEC);
  741. }
  742. void GCodeTimeEstimator::_forward_pass()
  743. {
  744. Block* block[2] = { nullptr, nullptr };
  745. for (Block& b : _blocks)
  746. {
  747. block[0] = block[1];
  748. block[1] = &b;
  749. _planner_forward_pass_kernel(block[0], block[1]);
  750. }
  751. _planner_forward_pass_kernel(block[1], nullptr);
  752. }
  753. void GCodeTimeEstimator::_reverse_pass()
  754. {
  755. Block* block[2] = { nullptr, nullptr };
  756. for (int i = (int)_blocks.size() - 1; i >= 0; --i)
  757. {
  758. block[1] = block[0];
  759. block[0] = &_blocks[i];
  760. _planner_reverse_pass_kernel(block[0], block[1]);
  761. }
  762. }
  763. void GCodeTimeEstimator::_planner_forward_pass_kernel(Block* prev, Block* curr)
  764. {
  765. if (prev == nullptr)
  766. return;
  767. // If the previous block is an acceleration block, but it is not long enough to complete the
  768. // full speed change within the block, we need to adjust the entry speed accordingly. Entry
  769. // speeds have already been reset, maximized, and reverse planned by reverse planner.
  770. // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
  771. if (!prev->flags.nominal_length)
  772. {
  773. if (prev->feedrate.entry < curr->feedrate.entry)
  774. {
  775. float entry_speed = std::min(curr->feedrate.entry, Block::max_allowable_speed(-prev->acceleration, prev->feedrate.entry, prev->move_length()));
  776. // Check for junction speed change
  777. if (curr->feedrate.entry != entry_speed)
  778. {
  779. curr->feedrate.entry = entry_speed;
  780. curr->flags.recalculate = true;
  781. }
  782. }
  783. }
  784. }
  785. void GCodeTimeEstimator::_planner_reverse_pass_kernel(Block* curr, Block* next)
  786. {
  787. if ((curr == nullptr) || (next == nullptr))
  788. return;
  789. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
  790. // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
  791. // check for maximum allowable speed reductions to ensure maximum possible planned speed.
  792. if (curr->feedrate.entry != curr->max_entry_speed)
  793. {
  794. // If nominal length true, max junction speed is guaranteed to be reached. Only compute
  795. // for max allowable speed if block is decelerating and nominal length is false.
  796. if (!curr->flags.nominal_length && (curr->max_entry_speed > next->feedrate.entry))
  797. curr->feedrate.entry = std::min(curr->max_entry_speed, Block::max_allowable_speed(-curr->acceleration, next->feedrate.entry, curr->move_length()));
  798. else
  799. curr->feedrate.entry = curr->max_entry_speed;
  800. curr->flags.recalculate = true;
  801. }
  802. }
  803. void GCodeTimeEstimator::_recalculate_trapezoids()
  804. {
  805. Block* curr = nullptr;
  806. Block* next = nullptr;
  807. for (Block& b : _blocks)
  808. {
  809. curr = next;
  810. next = &b;
  811. if (curr != nullptr)
  812. {
  813. // Recalculate if current block entry or exit junction speed has changed.
  814. if (curr->flags.recalculate || next->flags.recalculate)
  815. {
  816. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
  817. Block block = *curr;
  818. block.feedrate.exit = next->feedrate.entry;
  819. block.calculate_trapezoid();
  820. curr->trapezoid = block.trapezoid;
  821. curr->flags.recalculate = false; // Reset current only to ensure next trapezoid is computed
  822. }
  823. }
  824. }
  825. // Last/newest block in buffer. Always recalculated.
  826. if (next != nullptr)
  827. {
  828. Block block = *next;
  829. block.feedrate.exit = next->safe_feedrate;
  830. block.calculate_trapezoid();
  831. next->trapezoid = block.trapezoid;
  832. next->flags.recalculate = false;
  833. }
  834. }
  835. }