2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
3 Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
4 Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
9 #include "SimpleShell.h"
10 #include "libs/Kernel.h"
11 #include "libs/nuts_bolts.h"
12 #include "libs/utils.h"
13 #include "libs/SerialMessage.h"
14 #include "libs/StreamOutput.h"
16 #include "DirHandle.h"
19 #include "PublicDataRequest.h"
20 #include "AppendFileStream.h"
21 #include "FileStream.h"
22 #include "checksumm.h"
23 #include "PublicData.h"
26 #include "ToolManagerPublicAccess.h"
27 #include "GcodeDispatch.h"
28 #include "BaseSolution.h"
29 #include "StepperMotor.h"
30 #include "Configurator.h"
33 #include "TemperatureControlPublicAccess.h"
34 #include "EndstopsPublicAccess.h"
35 #include "NetworkPublicAccess.h"
36 #include "platform_memory.h"
37 #include "SwitchPublicAccess.h"
39 #include "Thermistor.h"
42 #include "AutoPushPop.h"
44 #include "system_LPC17xx.h"
47 #include "mbed.h" // for wait_ms()
49 extern unsigned int g_maximumHeapAddress
;
57 extern "C" uint32_t __end__
;
58 extern "C" uint32_t __malloc_free_list
;
59 extern "C" uint32_t _sbrk(int size
);
62 // command lookup table
63 const SimpleShell::ptentry_t
SimpleShell::commands_table
[] = {
64 {"ls", SimpleShell::ls_command
},
65 {"cd", SimpleShell::cd_command
},
66 {"pwd", SimpleShell::pwd_command
},
67 {"cat", SimpleShell::cat_command
},
68 {"rm", SimpleShell::rm_command
},
69 {"mv", SimpleShell::mv_command
},
70 {"mkdir", SimpleShell::mkdir_command
},
71 {"upload", SimpleShell::upload_command
},
72 {"reset", SimpleShell::reset_command
},
73 {"dfu", SimpleShell::dfu_command
},
74 {"break", SimpleShell::break_command
},
75 {"help", SimpleShell::help_command
},
76 {"?", SimpleShell::help_command
},
77 {"version", SimpleShell::version_command
},
78 {"mem", SimpleShell::mem_command
},
79 {"get", SimpleShell::get_command
},
80 {"set_temp", SimpleShell::set_temp_command
},
81 {"switch", SimpleShell::switch_command
},
82 {"net", SimpleShell::net_command
},
83 {"load", SimpleShell::load_command
},
84 {"save", SimpleShell::save_command
},
85 {"remount", SimpleShell::remount_command
},
86 {"calc_thermistor", SimpleShell::calc_thermistor_command
},
87 {"thermistors", SimpleShell::print_thermistors_command
},
88 {"md5sum", SimpleShell::md5sum_command
},
89 {"test", SimpleShell::test_command
},
95 int SimpleShell::reset_delay_secs
= 0;
97 // Adam Greens heap walk from http://mbed.org/forum/mbed/topic/2701/?page=4#comment-22556
98 static uint32_t heapWalk(StreamOutput
*stream
, bool verbose
)
100 uint32_t chunkNumber
= 1;
101 // The __end__ linker symbol points to the beginning of the heap.
102 uint32_t chunkCurr
= (uint32_t)&__end__
;
103 // __malloc_free_list is the head pointer to newlib-nano's link list of free chunks.
104 uint32_t freeCurr
= __malloc_free_list
;
105 // Calling _sbrk() with 0 reserves no more memory but it returns the current top of heap.
106 uint32_t heapEnd
= _sbrk(0);
108 uint32_t freeSize
= 0;
109 uint32_t usedSize
= 0;
111 stream
->printf("Used Heap Size: %lu\n", heapEnd
- chunkCurr
);
113 // Walk through the chunks until we hit the end of the heap.
114 while (chunkCurr
< heapEnd
) {
115 // Assume the chunk is in use. Will update later.
117 // The first 32-bit word in a chunk is the size of the allocation. newlib-nano over allocates by 8 bytes.
118 // 4 bytes for this 32-bit chunk size and another 4 bytes to allow for 8 byte-alignment of returned pointer.
119 uint32_t chunkSize
= *(uint32_t *)chunkCurr
;
120 // The start of the next chunk is right after the end of this one.
121 uint32_t chunkNext
= chunkCurr
+ chunkSize
;
123 // The free list is sorted by address.
124 // Check to see if we have found the next free chunk in the heap.
125 if (chunkCurr
== freeCurr
) {
126 // Chunk is free so flag it as such.
128 // The second 32-bit word in a free chunk is a pointer to the next free chunk (again sorted by address).
129 freeCurr
= *(uint32_t *)(freeCurr
+ 4);
132 // Skip past the 32-bit size field in the chunk header.
134 // 8-byte align the data pointer.
135 chunkCurr
= (chunkCurr
+ 7) & ~7;
136 // newlib-nano over allocates by 8 bytes, 4 bytes for the 32-bit chunk size and another 4 bytes to allow for 8
137 // byte-alignment of the returned pointer.
140 stream
->printf(" Chunk: %lu Address: 0x%08lX Size: %lu %s\n", chunkNumber
, chunkCurr
, chunkSize
, isChunkFree
? "CHUNK FREE" : "");
142 if (isChunkFree
) freeSize
+= chunkSize
;
143 else usedSize
+= chunkSize
;
145 chunkCurr
= chunkNext
;
148 stream
->printf("Allocated: %lu, Free: %lu\r\n", usedSize
, freeSize
);
153 void SimpleShell::on_module_loaded()
155 this->register_for_event(ON_CONSOLE_LINE_RECEIVED
);
156 this->register_for_event(ON_GCODE_RECEIVED
);
157 this->register_for_event(ON_SECOND_TICK
);
159 reset_delay_secs
= 0;
162 void SimpleShell::on_second_tick(void *)
164 // we are timing out for the reset
165 if (reset_delay_secs
> 0) {
166 if (--reset_delay_secs
== 0) {
172 void SimpleShell::on_gcode_received(void *argument
)
174 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
175 string args
= get_arguments(gcode
->get_command());
178 if (gcode
->m
== 20) { // list sd card
179 gcode
->stream
->printf("Begin file list\r\n");
180 ls_command("/sd", gcode
->stream
);
181 gcode
->stream
->printf("End file list\r\n");
183 } else if (gcode
->m
== 30) { // remove file
184 if(!args
.empty() && !THEKERNEL
->is_grbl_mode())
185 rm_command("/sd/" + args
, gcode
->stream
);
190 bool SimpleShell::parse_command(const char *cmd
, string args
, StreamOutput
*stream
)
192 for (const ptentry_t
*p
= commands_table
; p
->command
!= NULL
; ++p
) {
193 if (strncasecmp(cmd
, p
->command
, strlen(p
->command
)) == 0) {
194 p
->func(args
, stream
);
202 // When a new line is received, check if it is a command, and if it is, act upon it
203 void SimpleShell::on_console_line_received( void *argument
)
205 SerialMessage new_message
= *static_cast<SerialMessage
*>(argument
);
206 string possible_command
= new_message
.message
;
208 // ignore anything that is not lowercase or a $ as it is not a command
209 if(possible_command
.size() == 0 || (!islower(possible_command
[0]) && possible_command
[0] != '$')) {
213 // it is a grbl compatible command
214 if(possible_command
[0] == '$' && possible_command
.size() >= 2) {
215 switch(possible_command
[1]) {
218 get_command("state", new_message
.stream
);
219 new_message
.stream
->printf("ok\n");
223 if(THEKERNEL
->is_halted()) {
224 THEKERNEL
->call_event(ON_HALT
, (void *)1); // clears on_halt
225 new_message
.stream
->printf("[Caution: Unlocked]\nok\n");
230 grblDP_command("", new_message
.stream
);
231 new_message
.stream
->printf("ok\n");
235 if(THEKERNEL
->is_halted()) THEKERNEL
->call_event(ON_HALT
, (void *)1); // clears on_halt
236 if(THEKERNEL
->is_grbl_mode()) {
237 // issue G28.2 which is force homing cycle
238 Gcode
gcode("G28.2", new_message
.stream
);
239 THEKERNEL
->call_event(ON_GCODE_RECEIVED
, &gcode
);
241 Gcode
gcode("G28", new_message
.stream
);
242 THEKERNEL
->call_event(ON_GCODE_RECEIVED
, &gcode
);
244 new_message
.stream
->printf("ok\n");
248 // instant jog command
249 jog(possible_command
, new_message
.stream
);
253 new_message
.stream
->printf("error:Invalid statement\n");
259 //new_message.stream->printf("Received %s\r\n", possible_command.c_str());
260 string cmd
= shift_parameter(possible_command
);
262 // Configurator commands
263 if (cmd
== "config-get"){
264 THEKERNEL
->configurator
->config_get_command( possible_command
, new_message
.stream
);
266 } else if (cmd
== "config-set"){
267 THEKERNEL
->configurator
->config_set_command( possible_command
, new_message
.stream
);
269 } else if (cmd
== "config-load"){
270 THEKERNEL
->configurator
->config_load_command( possible_command
, new_message
.stream
);
272 } else if (cmd
== "play" || cmd
== "progress" || cmd
== "abort" || cmd
== "suspend" || cmd
== "resume") {
273 // these are handled by Player module
275 } else if (cmd
== "fire") {
276 // these are handled by Laser module
278 } else if (cmd
== "ok") {
279 // probably an echo so reply ok
280 new_message
.stream
->printf("ok\n");
282 }else if(!parse_command(cmd
.c_str(), possible_command
, new_message
.stream
)) {
283 new_message
.stream
->printf("error:Unsupported command - %s\n", cmd
.c_str());
288 // Act upon an ls command
289 // Convert the first parameter into an absolute path, then list the files in that path
290 void SimpleShell::ls_command( string parameters
, StreamOutput
*stream
)
293 while(!parameters
.empty()) {
294 string s
= shift_parameter( parameters
);
295 if(s
.front() == '-') {
299 if(!parameters
.empty()) {
301 path
.append(parameters
);
307 path
= absolute_from_relative(path
);
311 d
= opendir(path
.c_str());
313 while ((p
= readdir(d
)) != NULL
) {
314 stream
->printf("%s", lc(string(p
->d_name
)).c_str());
317 } else if(opts
.find("-s", 0, 2) != string::npos
) {
318 stream
->printf(" %d", p
->d_fsize
);
320 stream
->printf("\r\n");
324 stream
->printf("Could not open directory %s\r\n", path
.c_str());
328 extern SDFAT mounter
;
330 void SimpleShell::remount_command( string parameters
, StreamOutput
*stream
)
333 stream
->printf("remounted\r\n");
337 void SimpleShell::rm_command( string parameters
, StreamOutput
*stream
)
339 const char *fn
= absolute_from_relative(shift_parameter( parameters
)).c_str();
341 if (s
!= 0) stream
->printf("Could not delete %s \r\n", fn
);
345 void SimpleShell::mv_command( string parameters
, StreamOutput
*stream
)
347 string from
= absolute_from_relative(shift_parameter( parameters
));
348 string to
= absolute_from_relative(shift_parameter(parameters
));
349 int s
= rename(from
.c_str(), to
.c_str());
350 if (s
!= 0) stream
->printf("Could not rename %s to %s\r\n", from
.c_str(), to
.c_str());
351 else stream
->printf("renamed %s to %s\r\n", from
.c_str(), to
.c_str());
354 // Create a new directory
355 void SimpleShell::mkdir_command( string parameters
, StreamOutput
*stream
)
357 string path
= absolute_from_relative(shift_parameter( parameters
));
358 int result
= mkdir(path
.c_str(), 0);
359 if (result
!= 0) stream
->printf("could not create directory %s\r\n", path
.c_str());
360 else stream
->printf("created directory %s\r\n", path
.c_str());
363 // Change current absolute path to provided path
364 void SimpleShell::cd_command( string parameters
, StreamOutput
*stream
)
366 string folder
= absolute_from_relative( parameters
);
369 d
= opendir(folder
.c_str());
371 stream
->printf("Could not open directory %s \r\n", folder
.c_str() );
373 THEKERNEL
->current_path
= folder
;
378 // Responds with the present working directory
379 void SimpleShell::pwd_command( string parameters
, StreamOutput
*stream
)
381 stream
->printf("%s\r\n", THEKERNEL
->current_path
.c_str());
384 // Output the contents of a file, first parameter is the filename, second is the limit ( in number of lines to output )
385 void SimpleShell::cat_command( string parameters
, StreamOutput
*stream
)
387 // Get parameters ( filename and line limit )
388 string filename
= absolute_from_relative(shift_parameter( parameters
));
389 string limit_parameter
= shift_parameter( parameters
);
392 bool send_eof
= false;
393 if ( limit_parameter
== "-d" ) {
394 string d
= shift_parameter( parameters
);
396 delay
= strtol(d
.c_str(), &e
, 10);
397 if (e
<= d
.c_str()) {
401 send_eof
= true; // we need to terminate file send with an eof
404 }else if ( limit_parameter
!= "" ) {
406 limit
= strtol(limit_parameter
.c_str(), &e
, 10);
407 if (e
<= limit_parameter
.c_str())
411 // we have been asked to delay before cat, probably to allow time to issue upload command
413 safe_delay_ms(delay
*1000);
417 FILE *lp
= fopen(filename
.c_str(), "r");
419 stream
->printf("File not found: %s\r\n", filename
.c_str());
426 // Print each line of the file
427 while ((c
= fgetc (lp
)) != EOF
) {
428 buffer
.append((char *)&c
, 1);
429 if ( c
== '\n' || ++linecnt
> 80) {
430 if(c
== '\n') newlines
++;
431 stream
->puts(buffer
.c_str());
433 if(linecnt
> 80) linecnt
= 0;
434 // we need to kick things or they die
435 THEKERNEL
->call_event(ON_IDLE
);
437 if ( newlines
== limit
) {
444 stream
->puts("\032"); // ^Z terminates the upload
448 void SimpleShell::upload_command( string parameters
, StreamOutput
*stream
)
450 // this needs to be a hack. it needs to read direct from serial and not allow on_main_loop run until done
451 // NOTE this will block all operation until the upload is complete, so do not do while printing
452 if(!THECONVEYOR
->is_idle()) {
453 stream
->printf("upload not allowed while printing or busy\n");
457 // open file to upload to
458 string upload_filename
= absolute_from_relative( parameters
);
459 FILE *fd
= fopen(upload_filename
.c_str(), "w");
461 stream
->printf("uploading to file: %s, send control-D or control-Z to finish\r\n", upload_filename
.c_str());
463 stream
->printf("failed to open file: %s.\r\n", upload_filename
.c_str());
468 bool uploading
= true;
470 if(!stream
->ready()) {
471 // we need to kick things or they die
472 THEKERNEL
->call_event(ON_IDLE
);
476 char c
= stream
->_getc();
477 if( c
== 4 || c
== 26) { // ctrl-D or ctrl-Z
481 stream
->printf("uploaded %d bytes\n", cnt
);
485 // write character to file
487 if(fputc(c
, fd
) != c
) {
488 // error writing to file
489 stream
->printf("error writing to file. ignoring all characters until EOF\r\n");
495 if ((cnt
%1000) == 0) {
496 // we need to kick things or they die
497 THEKERNEL
->call_event(ON_IDLE
);
502 // we got an error so ignore everything until EOF
505 if(stream
->ready()) {
508 THEKERNEL
->call_event(ON_IDLE
);
511 } while(c
!= 4 && c
!= 26);
514 // loads the specified config-override file
515 void SimpleShell::load_command( string parameters
, StreamOutput
*stream
)
517 // Get parameters ( filename )
518 string filename
= absolute_from_relative(parameters
);
519 if(filename
== "/") {
520 filename
= THEKERNEL
->config_override_filename();
523 FILE *fp
= fopen(filename
.c_str(), "r");
526 stream
->printf("Loading config override file: %s...\n", filename
.c_str());
527 while(fgets(buf
, sizeof buf
, fp
) != NULL
) {
528 stream
->printf(" %s", buf
);
529 if(buf
[0] == ';') continue; // skip the comments
530 // NOTE only Gcodes and Mcodes can be in the config-override
531 Gcode
*gcode
= new Gcode(buf
, &StreamOutput::NullStream
);
532 THEKERNEL
->call_event(ON_GCODE_RECEIVED
, gcode
);
534 THEKERNEL
->call_event(ON_IDLE
);
536 stream
->printf("config override file executed\n");
540 stream
->printf("File not found: %s\n", filename
.c_str());
544 // saves the specified config-override file
545 void SimpleShell::save_command( string parameters
, StreamOutput
*stream
)
547 // Get parameters ( filename )
548 string filename
= absolute_from_relative(parameters
);
549 if(filename
== "/") {
550 filename
= THEKERNEL
->config_override_filename();
553 THECONVEYOR
->wait_for_idle(); //just to be safe as it can take a while to run
555 //remove(filename.c_str()); // seems to cause a hang every now and then
557 FileStream
fs(filename
.c_str());
558 fs
.printf("; DO NOT EDIT THIS FILE\n");
559 // this also will truncate the existing file instead of deleting it
562 // stream that appends to file
563 AppendFileStream
*gs
= new AppendFileStream(filename
.c_str());
564 // if(!gs->is_open()) {
565 // stream->printf("Unable to open File %s for write\n", filename.c_str());
570 // issue a M500 which will store values in the file stream
571 Gcode
*gcode
= new Gcode("M500", gs
);
572 THEKERNEL
->call_event(ON_GCODE_RECEIVED
, gcode
);
577 stream
->printf("Settings Stored to %s\r\n", filename
.c_str());
581 void SimpleShell::mem_command( string parameters
, StreamOutput
*stream
)
583 bool verbose
= shift_parameter( parameters
).find_first_of("Vv") != string::npos
;
584 unsigned long heap
= (unsigned long)_sbrk(0);
585 unsigned long m
= g_maximumHeapAddress
- heap
;
586 stream
->printf("Unused Heap: %lu bytes\r\n", m
);
588 uint32_t f
= heapWalk(stream
, verbose
);
589 stream
->printf("Total Free RAM: %lu bytes\r\n", m
+ f
);
591 stream
->printf("Free AHB0: %lu, AHB1: %lu\r\n", AHB0
.free(), AHB1
.free());
597 stream
->printf("Block size: %u bytes, Tickinfo size: %u bytes\n", sizeof(Block
), sizeof(Block::tickinfo_t
) * Block::n_actuators
);
600 static uint32_t getDeviceType()
602 #define IAP_LOCATION 0x1FFF1FF1
605 typedef void (*IAP
)(uint32_t *, uint32_t *);
606 IAP iap
= (IAP
) IAP_LOCATION
;
611 iap(command
, result
);
618 // get network config
619 void SimpleShell::net_command( string parameters
, StreamOutput
*stream
)
622 bool ok
= PublicData::get_value( network_checksum
, get_ipconfig_checksum
, &returned_data
);
624 char *str
= (char *)returned_data
;
625 stream
->printf("%s\r\n", str
);
629 stream
->printf("No network detected\n");
633 // print out build version
634 void SimpleShell::version_command( string parameters
, StreamOutput
*stream
)
637 uint32_t dev
= getDeviceType();
638 const char *mcu
= (dev
& 0x00100000) ? "LPC1769" : "LPC1768";
639 stream
->printf("Build version: %s, Build date: %s, MCU: %s, System Clock: %ldMHz\r\n", vers
.get_build(), vers
.get_build_date(), mcu
, SystemCoreClock
/ 1000000);
641 stream
->printf(" CNC Build ");
644 stream
->printf(" NOMSD Build\r\n");
646 stream
->printf("%d axis\n", MAX_ROBOT_ACTUATORS
);
650 void SimpleShell::reset_command( string parameters
, StreamOutput
*stream
)
652 stream
->printf("Smoothie out. Peace. Rebooting in 5 seconds...\r\n");
653 reset_delay_secs
= 5; // reboot in 5 seconds
656 // go into dfu boot mode
657 void SimpleShell::dfu_command( string parameters
, StreamOutput
*stream
)
659 stream
->printf("Entering boot mode...\r\n");
663 // Break out into the MRI debugging system
664 void SimpleShell::break_command( string parameters
, StreamOutput
*stream
)
666 stream
->printf("Entering MRI debug mode...\r\n");
670 static int get_active_tool()
673 bool ok
= PublicData::get_value(tool_manager_checksum
, get_active_tool_checksum
, &returned_data
);
675 int active_tool
= *static_cast<int *>(returned_data
);
682 void SimpleShell::grblDP_command( string parameters
, StreamOutput
*stream
)
685 [G54:95.000,40.000,-23.600]
686 [G55:0.000,0.000,0.000]
687 [G56:0.000,0.000,0.000]
688 [G57:0.000,0.000,0.000]
689 [G58:0.000,0.000,0.000]
690 [G59:0.000,0.000,0.000]
691 [G28:0.000,0.000,0.000]
692 [G30:0.000,0.000,0.000]
693 [G92:0.000,0.000,0.000]
695 [PRB:0.000,0.000,0.000:0]
698 bool verbose
= shift_parameter( parameters
).find_first_of("Vv") != string::npos
;
700 std::vector
<Robot::wcs_t
> v
= THEROBOT
->get_wcs_state();
702 char current_wcs
= std::get
<0>(v
[0]);
703 stream
->printf("[current WCS: %s]\n", wcs2gcode(current_wcs
).c_str());
706 int n
= std::get
<1>(v
[0]);
707 for (int i
= 1; i
<= n
; ++i
) {
708 stream
->printf("[%s:%1.4f,%1.4f,%1.4f]\n", wcs2gcode(i
-1).c_str(),
709 THEROBOT
->from_millimeters(std::get
<0>(v
[i
])),
710 THEROBOT
->from_millimeters(std::get
<1>(v
[i
])),
711 THEROBOT
->from_millimeters(std::get
<2>(v
[i
])));
715 PublicData::get_value( endstops_checksum
, saved_position_checksum
, &rd
);
716 stream
->printf("[G28:%1.4f,%1.4f,%1.4f]\n",
717 THEROBOT
->from_millimeters(rd
[0]),
718 THEROBOT
->from_millimeters(rd
[1]),
719 THEROBOT
->from_millimeters(rd
[2]));
721 stream
->printf("[G30:%1.4f,%1.4f,%1.4f]\n", 0.0F
, 0.0F
, 0.0F
); // not implemented
723 stream
->printf("[G92:%1.4f,%1.4f,%1.4f]\n",
724 THEROBOT
->from_millimeters(std::get
<0>(v
[n
+1])),
725 THEROBOT
->from_millimeters(std::get
<1>(v
[n
+1])),
726 THEROBOT
->from_millimeters(std::get
<2>(v
[n
+1])));
729 stream
->printf("[Tool Offset:%1.4f,%1.4f,%1.4f]\n",
730 THEROBOT
->from_millimeters(std::get
<0>(v
[n
+2])),
731 THEROBOT
->from_millimeters(std::get
<1>(v
[n
+2])),
732 THEROBOT
->from_millimeters(std::get
<2>(v
[n
+2])));
734 stream
->printf("[TL0:%1.4f]\n", THEROBOT
->from_millimeters(std::get
<2>(v
[n
+2])));
737 // this is the last probe position, updated when a probe completes, also stores the number of steps moved after a homing cycle
740 std::tie(px
, py
, pz
, ps
) = THEROBOT
->get_last_probe_position();
741 stream
->printf("[PRB:%1.4f,%1.4f,%1.4f:%d]\n", THEROBOT
->from_millimeters(px
), THEROBOT
->from_millimeters(py
), THEROBOT
->from_millimeters(pz
), ps
);
744 void SimpleShell::get_command( string parameters
, StreamOutput
*stream
)
746 string what
= shift_parameter( parameters
);
748 if (what
== "temp") {
749 struct pad_temperature temp
;
750 string type
= shift_parameter( parameters
);
752 // scan all temperature controls
753 std::vector
<struct pad_temperature
> controllers
;
754 bool ok
= PublicData::get_value(temperature_control_checksum
, poll_controls_checksum
, &controllers
);
756 for (auto &c
: controllers
) {
757 stream
->printf("%s (%d) temp: %f/%f @%d\r\n", c
.designator
.c_str(), c
.id
, c
.current_temperature
, c
.target_temperature
, c
.pwm
);
761 stream
->printf("no heaters found\r\n");
765 bool ok
= PublicData::get_value( temperature_control_checksum
, current_temperature_checksum
, get_checksum(type
), &temp
);
768 stream
->printf("%s temp: %f/%f @%d\r\n", type
.c_str(), temp
.current_temperature
, temp
.target_temperature
, temp
.pwm
);
770 stream
->printf("%s is not a known temperature device\r\n", type
.c_str());
774 } else if (what
== "fk" || what
== "ik") {
775 string p
= shift_parameter( parameters
);
779 p
= shift_parameter( parameters
);
782 std::vector
<float> v
= parse_number_list(p
.c_str());
783 if(p
.empty() || v
.size() < 1) {
784 stream
->printf("error:usage: get [fk|ik] [-m] x[,y,z]\n");
789 float y
= (v
.size() > 1) ? v
[1] : x
;
790 float z
= (v
.size() > 2) ? v
[2] : y
;
793 // do forward kinematics on the given actuator position and display the cartesian coordinates
794 ActuatorCoordinates apos
{x
, y
, z
};
796 THEROBOT
->arm_solution
->actuator_to_cartesian(apos
, pos
);
797 stream
->printf("cartesian= X %f, Y %f, Z %f\n", pos
[0], pos
[1], pos
[2]);
803 // do inverse kinematics on the given cartesian position and display the actuator coordinates
804 float pos
[3]{x
, y
, z
};
805 ActuatorCoordinates apos
;
806 THEROBOT
->arm_solution
->cartesian_to_actuator(pos
, apos
);
807 stream
->printf("actuator= X %f, Y %f, Z %f\n", apos
[0], apos
[1], apos
[2]);
811 // move to the calculated, or given, XYZ
813 snprintf(cmd
, sizeof(cmd
), "G53 G0 X%f Y%f Z%f", x
, y
, z
);
814 struct SerialMessage message
;
815 message
.message
= cmd
;
816 message
.stream
= &(StreamOutput::NullStream
);
817 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
818 THECONVEYOR
->wait_for_idle();
821 } else if (what
== "pos") {
822 // convenience to call all the various M114 variants, shows ABC axis where relevant
824 THEROBOT
->print_position(0, buf
); stream
->printf("last %s\n", buf
.c_str()); buf
.clear();
825 THEROBOT
->print_position(1, buf
); stream
->printf("realtime %s\n", buf
.c_str()); buf
.clear();
826 THEROBOT
->print_position(2, buf
); stream
->printf("%s\n", buf
.c_str()); buf
.clear();
827 THEROBOT
->print_position(3, buf
); stream
->printf("%s\n", buf
.c_str()); buf
.clear();
828 THEROBOT
->print_position(4, buf
); stream
->printf("%s\n", buf
.c_str()); buf
.clear();
829 THEROBOT
->print_position(5, buf
); stream
->printf("%s\n", buf
.c_str()); buf
.clear();
831 } else if (what
== "wcs") {
832 // print the wcs state
833 grblDP_command("-v", stream
);
835 } else if (what
== "state") {
837 // [G0 G54 G17 G21 G90 G94 M0 M5 M9 T0 F0.]
838 stream
->printf("[G%d %s G%d G%d G%d G94 M0 M5 M9 T%d F%1.4f S%1.4f]\n",
839 THEKERNEL
->gcode_dispatch
->get_modal_command(),
840 wcs2gcode(THEROBOT
->get_current_wcs()).c_str(),
841 THEROBOT
->plane_axis_0
== X_AXIS
&& THEROBOT
->plane_axis_1
== Y_AXIS
&& THEROBOT
->plane_axis_2
== Z_AXIS
? 17 :
842 THEROBOT
->plane_axis_0
== X_AXIS
&& THEROBOT
->plane_axis_1
== Z_AXIS
&& THEROBOT
->plane_axis_2
== Y_AXIS
? 18 :
843 THEROBOT
->plane_axis_0
== Y_AXIS
&& THEROBOT
->plane_axis_1
== Z_AXIS
&& THEROBOT
->plane_axis_2
== X_AXIS
? 19 : 17,
844 THEROBOT
->inch_mode
? 20 : 21,
845 THEROBOT
->absolute_mode
? 90 : 91,
847 THEROBOT
->from_millimeters(THEROBOT
->get_feed_rate()),
848 THEROBOT
->get_s_value());
850 } else if (what
== "status") {
851 // also ? on serial and usb
852 stream
->printf("%s\n", THEKERNEL
->get_query_string().c_str());
855 stream
->printf("error:unknown option %s\n", what
.c_str());
859 // used to test out the get public data events
860 void SimpleShell::set_temp_command( string parameters
, StreamOutput
*stream
)
862 string type
= shift_parameter( parameters
);
863 string temp
= shift_parameter( parameters
);
864 float t
= temp
.empty() ? 0.0 : strtof(temp
.c_str(), NULL
);
865 bool ok
= PublicData::set_value( temperature_control_checksum
, get_checksum(type
), &t
);
868 stream
->printf("%s temp set to: %3.1f\r\n", type
.c_str(), t
);
870 stream
->printf("%s is not a known temperature device\r\n", type
.c_str());
874 void SimpleShell::print_thermistors_command( string parameters
, StreamOutput
*stream
)
876 #ifndef NO_TOOLS_TEMPERATURECONTROL
877 Thermistor::print_predefined_thermistors(stream
);
881 void SimpleShell::calc_thermistor_command( string parameters
, StreamOutput
*stream
)
883 #ifndef NO_TOOLS_TEMPERATURECONTROL
884 string s
= shift_parameter( parameters
);
886 // see if we have -sn as first argument
887 if(s
.find("-s", 0, 2) != string::npos
) {
888 // save the results to thermistor n
889 saveto
= strtol(s
.substr(2).c_str(), nullptr, 10);
894 std::vector
<float> trl
= parse_number_list(parameters
.c_str());
895 if(trl
.size() == 6) {
896 // calculate the coefficients
898 std::tie(c1
, c2
, c3
) = Thermistor::calculate_steinhart_hart_coefficients(trl
[0], trl
[1], trl
[2], trl
[3], trl
[4], trl
[5]);
899 stream
->printf("Steinhart Hart coefficients: I%1.18f J%1.18f K%1.18f\n", c1
, c2
, c3
);
901 stream
->printf(" Paste the above in the M305 S0 command, then save with M500\n");
904 size_t n
= snprintf(buf
, sizeof(buf
), "M305 S%d I%1.18f J%1.18f K%1.18f", saveto
, c1
, c2
, c3
);
905 if(n
> sizeof(buf
)) n
= sizeof(buf
);
907 Gcode
gcode(g
, &(StreamOutput::NullStream
));
908 THEKERNEL
->call_event(ON_GCODE_RECEIVED
, &gcode
);
909 stream
->printf(" Setting Thermistor %d to those settings, save with M500\n", saveto
);
914 stream
->printf("Usage: calc_thermistor T1,R1,T2,R2,T3,R3\n");
919 // set or get switch state for a named switch
920 void SimpleShell::switch_command( string parameters
, StreamOutput
*stream
)
922 string type
= shift_parameter( parameters
);
923 string value
= shift_parameter( parameters
);
927 struct pad_switch pad
;
928 bool ok
= PublicData::get_value(switch_checksum
, get_checksum(type
), 0, &pad
);
930 stream
->printf("unknown switch %s.\n", type
.c_str());
933 stream
->printf("switch %s is %d\n", type
.c_str(), pad
.state
);
937 if(value
== "on" || value
== "off") {
938 bool b
= value
== "on";
939 ok
= PublicData::set_value( switch_checksum
, get_checksum(type
), state_checksum
, &b
);
941 float v
= strtof(value
.c_str(), NULL
);
942 ok
= PublicData::set_value( switch_checksum
, get_checksum(type
), value_checksum
, &v
);
945 stream
->printf("switch %s set to: %s\n", type
.c_str(), value
.c_str());
947 stream
->printf("%s is not a known switch device\n", type
.c_str());
952 void SimpleShell::md5sum_command( string parameters
, StreamOutput
*stream
)
954 string filename
= absolute_from_relative(parameters
);
957 FILE *lp
= fopen(filename
.c_str(), "r");
959 stream
->printf("File not found: %s\r\n", filename
.c_str());
965 size_t n
= fread(buf
, 1, sizeof buf
, lp
);
966 if(n
> 0) md5
.update(buf
, n
);
967 THEKERNEL
->call_event(ON_IDLE
);
970 stream
->printf("%s %s\n", md5
.finalize().hexdigest().c_str(), filename
.c_str());
974 // runs several types of test on the mechanisms
975 void SimpleShell::test_command( string parameters
, StreamOutput
*stream
)
977 AutoPushPop app
; // this will save the state and restore it on exit
978 string what
= shift_parameter( parameters
);
981 // jogs back and forth usage: axis distance iterations [feedrate]
982 string axis
= shift_parameter( parameters
);
983 string dist
= shift_parameter( parameters
);
984 string iters
= shift_parameter( parameters
);
985 string speed
= shift_parameter( parameters
);
986 if(axis
.empty() || dist
.empty() || iters
.empty()) {
987 stream
->printf("error: Need axis distance iterations\n");
990 float d
= strtof(dist
.c_str(), NULL
);
991 float f
= speed
.empty() ? THEROBOT
->get_feed_rate() : strtof(speed
.c_str(), NULL
);
992 uint32_t n
= strtol(iters
.c_str(), NULL
, 10);
995 for (uint32_t i
= 0; i
< n
; ++i
) {
997 snprintf(cmd
, sizeof(cmd
), "G91 G0 %c%f F%f G90", toupper(axis
[0]), toggle
? -d
: d
, f
);
998 stream
->printf("%s\n", cmd
);
999 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1000 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1001 if(THEKERNEL
->is_halted()) break;
1004 stream
->printf("done\n");
1006 }else if (what
== "circle") {
1007 // draws a circle around origin. usage: radius iterations [feedrate]
1008 string radius
= shift_parameter( parameters
);
1009 string iters
= shift_parameter( parameters
);
1010 string speed
= shift_parameter( parameters
);
1011 if(radius
.empty() || iters
.empty()) {
1012 stream
->printf("error: Need radius iterations\n");
1016 float r
= strtof(radius
.c_str(), NULL
);
1017 uint32_t n
= strtol(iters
.c_str(), NULL
, 10);
1018 float f
= speed
.empty() ? THEROBOT
->get_feed_rate() : strtof(speed
.c_str(), NULL
);
1020 THEROBOT
->push_state();
1022 snprintf(cmd
, sizeof(cmd
), "G91 G0 X%f F%f G90", -r
, f
);
1023 stream
->printf("%s\n", cmd
);
1024 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1025 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1027 for (uint32_t i
= 0; i
< n
; ++i
) {
1028 if(THEKERNEL
->is_halted()) break;
1029 snprintf(cmd
, sizeof(cmd
), "G2 I%f J0 F%f", r
, f
);
1030 stream
->printf("%s\n", cmd
);
1031 message
.message
= cmd
;
1032 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1035 // leave it where it started
1036 if(!THEKERNEL
->is_halted()) {
1037 snprintf(cmd
, sizeof(cmd
), "G91 G0 X%f F%f G90", r
, f
);
1038 stream
->printf("%s\n", cmd
);
1039 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1040 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1043 THEROBOT
->pop_state();
1044 stream
->printf("done\n");
1046 }else if (what
== "square") {
1047 // draws a square usage: size iterations [feedrate]
1048 string size
= shift_parameter( parameters
);
1049 string iters
= shift_parameter( parameters
);
1050 string speed
= shift_parameter( parameters
);
1051 if(size
.empty() || iters
.empty()) {
1052 stream
->printf("error: Need size iterations\n");
1055 float d
= strtof(size
.c_str(), NULL
);
1056 float f
= speed
.empty() ? THEROBOT
->get_feed_rate() : strtof(speed
.c_str(), NULL
);
1057 uint32_t n
= strtol(iters
.c_str(), NULL
, 10);
1059 for (uint32_t i
= 0; i
< n
; ++i
) {
1062 snprintf(cmd
, sizeof(cmd
), "G91 G0 X%f F%f", d
, f
);
1063 stream
->printf("%s\n", cmd
);
1064 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1065 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1068 snprintf(cmd
, sizeof(cmd
), "G0 Y%f", d
);
1069 stream
->printf("%s\n", cmd
);
1070 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1071 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1074 snprintf(cmd
, sizeof(cmd
), "G0 X%f", -d
);
1075 stream
->printf("%s\n", cmd
);
1076 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1077 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1080 snprintf(cmd
, sizeof(cmd
), "G0 Y%f G90", -d
);
1081 stream
->printf("%s\n", cmd
);
1082 struct SerialMessage message
{&StreamOutput::NullStream
, cmd
};
1083 THEKERNEL
->call_event(ON_CONSOLE_LINE_RECEIVED
, &message
);
1085 if(THEKERNEL
->is_halted()) break;
1087 stream
->printf("done\n");
1089 }else if (what
== "raw") {
1090 // issues raw steps to the specified axis usage: axis steps steps/sec
1091 string axis
= shift_parameter( parameters
);
1092 string stepstr
= shift_parameter( parameters
);
1093 string stepspersec
= shift_parameter( parameters
);
1094 if(axis
.empty() || stepstr
.empty() || stepspersec
.empty()) {
1095 stream
->printf("error: Need axis steps steps/sec\n");
1099 char ax
= toupper(axis
[0]);
1100 uint8_t a
= ax
>= 'X' ? ax
- 'X' : ax
- 'A' + 3;
1101 int steps
= strtol(stepstr
.c_str(), NULL
, 10);
1102 bool dir
= steps
>= 0;
1103 steps
= std::abs(steps
);
1106 stream
->printf("error: axis must be x, y, z, a, b, c\n");
1110 if(a
>= THEROBOT
->get_number_registered_motors()) {
1111 stream
->printf("error: axis is out of range\n");
1115 uint32_t sps
= strtol(stepspersec
.c_str(), NULL
, 10);
1116 sps
= std::max(sps
, 1UL);
1118 uint32_t delayus
= 1000000.0F
/ sps
;
1119 for(int s
= 0;s
<steps
;s
++) {
1120 if(THEKERNEL
->is_halted()) break;
1121 THEROBOT
->actuators
[a
]->manual_step(dir
);
1122 // delay but call on_idle
1123 safe_delay_us(delayus
);
1126 // reset the position based on current actuator position
1127 THEROBOT
->reset_position_from_current_actuator_position();
1129 //stream->printf("done\n");
1132 stream
->printf("usage:\n test jog axis distance iterations [feedrate]\n");
1133 stream
->printf(" test square size iterations [feedrate]\n");
1134 stream
->printf(" test circle radius iterations [feedrate]\n");
1135 stream
->printf(" test raw axis steps steps/sec\n");
1139 void SimpleShell::jog(string parameters
, StreamOutput
*stream
)
1142 int n_motors
= THEROBOT
->get_number_registered_motors();
1144 // get axis to move and amount (X0.1)
1145 // for now always 1 axis
1146 size_t npos
= parameters
.find_first_of("XYZABC");
1147 if(npos
== string::npos
) {
1148 stream
->printf("usage: $J X|Y|Z|A|B|C 0.01 [F0.5]\n");
1152 string s
= parameters
.substr(npos
);
1153 if(s
.empty() || s
.size() < 2) {
1154 stream
->printf("usage: $J X0.01 [F0.5]\n");
1157 char ax
= toupper(s
[0]);
1158 uint8_t a
= ax
>= 'X' ? ax
- 'X' : ax
- 'A' + 3;
1160 stream
->printf("error:bad axis\n");
1164 float d
= strtof(s
.substr(1).c_str(), NULL
);
1166 float delta
[n_motors
];
1167 for (int i
= 0; i
< n_motors
; ++i
) {
1174 npos
= parameters
.find_first_of("F");
1175 if(npos
!= string::npos
&& npos
+1 < parameters
.size()) {
1176 scale
= strtof(parameters
.substr(npos
+1).c_str(), NULL
);
1179 THEROBOT
->push_state();
1180 float rate_mm_s
= THEROBOT
->actuators
[a
]->get_max_rate() * scale
;
1181 THEROBOT
->delta_move(delta
, rate_mm_s
, n_motors
);
1183 // turn off queue delay and run it now
1184 THECONVEYOR
->force_queue();
1185 THEROBOT
->pop_state();
1186 stream
->printf("Jog: %c%f F%f\n", ax
, d
, scale
);
1189 void SimpleShell::help_command( string parameters
, StreamOutput
*stream
)
1191 stream
->printf("Commands:\r\n");
1192 stream
->printf("version\r\n");
1193 stream
->printf("mem [-v]\r\n");
1194 stream
->printf("ls [-s] [folder]\r\n");
1195 stream
->printf("cd folder\r\n");
1196 stream
->printf("pwd\r\n");
1197 stream
->printf("cat file [limit] [-d 10]\r\n");
1198 stream
->printf("rm file\r\n");
1199 stream
->printf("mv file newfile\r\n");
1200 stream
->printf("remount\r\n");
1201 stream
->printf("play file [-v]\r\n");
1202 stream
->printf("progress - shows progress of current play\r\n");
1203 stream
->printf("abort - abort currently playing file\r\n");
1204 stream
->printf("reset - reset smoothie\r\n");
1205 stream
->printf("dfu - enter dfu boot loader\r\n");
1206 stream
->printf("break - break into debugger\r\n");
1207 stream
->printf("config-get [<configuration_source>] <configuration_setting>\r\n");
1208 stream
->printf("config-set [<configuration_source>] <configuration_setting> <value>\r\n");
1209 stream
->printf("get [pos|wcs|state|status|fk|ik]\r\n");
1210 stream
->printf("get temp [bed|hotend]\r\n");
1211 stream
->printf("set_temp bed|hotend 185\r\n");
1212 stream
->printf("switch name [value]\r\n");
1213 stream
->printf("net\r\n");
1214 stream
->printf("load [file] - loads a configuration override file from soecified name or config-override\r\n");
1215 stream
->printf("save [file] - saves a configuration override file as specified filename or as config-override\r\n");
1216 stream
->printf("upload filename - saves a stream of text to the named file\r\n");
1217 stream
->printf("calc_thermistor [-s0] T1,R1,T2,R2,T3,R3 - calculate the Steinhart Hart coefficients for a thermistor\r\n");
1218 stream
->printf("thermistors - print out the predefined thermistors\r\n");
1219 stream
->printf("md5sum file - prints md5 sum of the given file\r\n");