Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/GCodeReader/GCodeReader.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1 #include "GCodeReader.h"
e2011220 0:ee7e9405e1c7 2 GCodeReader::GCodeReader(Eigen::Vector3d *current_position, Eigen::Vector3d *target_position)
e2011220 0:ee7e9405e1c7 3 : _current_position(current_position), _target_position(target_position)
e2011220 0:ee7e9405e1c7 4 {}
e2011220 0:ee7e9405e1c7 5
e2011220 0:ee7e9405e1c7 6 void GCodeReader::read_g_code(const std::string &g_code)
e2011220 0:ee7e9405e1c7 7 {
e2011220 0:ee7e9405e1c7 8 Eigen::Vector3d target_position;
e2011220 0:ee7e9405e1c7 9 std::vector<std::string> strings;
e2011220 0:ee7e9405e1c7 10
e2011220 0:ee7e9405e1c7 11 strings = split_space(g_code);
e2011220 0:ee7e9405e1c7 12
e2011220 0:ee7e9405e1c7 13 switch(strings[0][0]){
e2011220 0:ee7e9405e1c7 14 case 'g':
e2011220 0:ee7e9405e1c7 15 case 'G':
e2011220 0:ee7e9405e1c7 16 int code_num;
e2011220 0:ee7e9405e1c7 17 std::stringstream ss;
e2011220 0:ee7e9405e1c7 18
e2011220 0:ee7e9405e1c7 19 ss.str("");
e2011220 0:ee7e9405e1c7 20 ss.clear(std::stringstream::goodbit);
e2011220 0:ee7e9405e1c7 21 ss << strings[0];
e2011220 0:ee7e9405e1c7 22 ss.ignore();
e2011220 0:ee7e9405e1c7 23 ss >> code_num;
e2011220 0:ee7e9405e1c7 24
e2011220 0:ee7e9405e1c7 25 switch(code_num){
e2011220 0:ee7e9405e1c7 26 case 0:
e2011220 0:ee7e9405e1c7 27 _function_g_00(strings);
e2011220 0:ee7e9405e1c7 28 break;
e2011220 0:ee7e9405e1c7 29 case 4:
e2011220 0:ee7e9405e1c7 30 _function_g_04(strings);
e2011220 0:ee7e9405e1c7 31 break;
e2011220 0:ee7e9405e1c7 32 }
e2011220 0:ee7e9405e1c7 33 break;
e2011220 0:ee7e9405e1c7 34 }
e2011220 0:ee7e9405e1c7 35 }
e2011220 0:ee7e9405e1c7 36
e2011220 0:ee7e9405e1c7 37 void GCodeReader::_function_g_00(const std::vector<std::string> &strings)
e2011220 0:ee7e9405e1c7 38 {
e2011220 0:ee7e9405e1c7 39 double value;
e2011220 0:ee7e9405e1c7 40 std::stringstream ss;
e2011220 0:ee7e9405e1c7 41 for(int i = 1; i < strings.size(); i++){
e2011220 0:ee7e9405e1c7 42
e2011220 0:ee7e9405e1c7 43 ss.str("");
e2011220 0:ee7e9405e1c7 44 ss.clear(std::stringstream::goodbit);
e2011220 0:ee7e9405e1c7 45 ss << strings[i];
e2011220 0:ee7e9405e1c7 46 ss.ignore();
e2011220 0:ee7e9405e1c7 47 ss >> value;
e2011220 0:ee7e9405e1c7 48
e2011220 0:ee7e9405e1c7 49 switch(strings[i][0]){
e2011220 0:ee7e9405e1c7 50 case 'x':
e2011220 0:ee7e9405e1c7 51 case 'X':
e2011220 0:ee7e9405e1c7 52 (*_target_position)(Movement::X) = value;
e2011220 0:ee7e9405e1c7 53 break;
e2011220 0:ee7e9405e1c7 54 case 'y':
e2011220 0:ee7e9405e1c7 55 case 'Y':
e2011220 0:ee7e9405e1c7 56 (*_target_position)(Movement::Y) = value;
e2011220 0:ee7e9405e1c7 57 break;
e2011220 0:ee7e9405e1c7 58 case 'z':
e2011220 0:ee7e9405e1c7 59 case 'Z':
e2011220 0:ee7e9405e1c7 60 (*_target_position)(Movement::Yaw) = value * M_PI;
e2011220 0:ee7e9405e1c7 61 break;
e2011220 0:ee7e9405e1c7 62 }
e2011220 0:ee7e9405e1c7 63 }
e2011220 0:ee7e9405e1c7 64 }
e2011220 0:ee7e9405e1c7 65
e2011220 0:ee7e9405e1c7 66 void GCodeReader::_function_g_04(const std::vector<std::string> &strings)
e2011220 0:ee7e9405e1c7 67 {
e2011220 0:ee7e9405e1c7 68 double value;
e2011220 0:ee7e9405e1c7 69 std::stringstream ss;
e2011220 0:ee7e9405e1c7 70 ss.str("");
e2011220 0:ee7e9405e1c7 71 ss.clear(std::stringstream::goodbit);
e2011220 0:ee7e9405e1c7 72 ss << strings[1];
e2011220 0:ee7e9405e1c7 73 ss.ignore();
e2011220 0:ee7e9405e1c7 74 ss >> value;
e2011220 0:ee7e9405e1c7 75
e2011220 0:ee7e9405e1c7 76 switch(strings[1][0]){
e2011220 0:ee7e9405e1c7 77 case 'x':
e2011220 0:ee7e9405e1c7 78 case 'X':
e2011220 0:ee7e9405e1c7 79 wait(value);
e2011220 0:ee7e9405e1c7 80 break;
e2011220 0:ee7e9405e1c7 81 case 'p':
e2011220 0:ee7e9405e1c7 82 case 'P':
e2011220 0:ee7e9405e1c7 83 wait(value * 0.001);
e2011220 0:ee7e9405e1c7 84 break;
e2011220 0:ee7e9405e1c7 85 }
e2011220 0:ee7e9405e1c7 86 }
e2011220 0:ee7e9405e1c7 87
e2011220 0:ee7e9405e1c7 88 std::vector<std::string> GCodeReader::split_space(const std::string &str)
e2011220 0:ee7e9405e1c7 89 {
e2011220 0:ee7e9405e1c7 90 return split(str, ' ');
e2011220 0:ee7e9405e1c7 91 }
e2011220 0:ee7e9405e1c7 92
e2011220 0:ee7e9405e1c7 93
e2011220 0:ee7e9405e1c7 94 std::vector<std::string> GCodeReader::split(const std::string &str, char sep)
e2011220 0:ee7e9405e1c7 95 {
e2011220 0:ee7e9405e1c7 96 std::vector<std::string> strings;
e2011220 0:ee7e9405e1c7 97 std::stringstream ss(str);
e2011220 0:ee7e9405e1c7 98 std::string buffer;
e2011220 0:ee7e9405e1c7 99 while( std::getline(ss, buffer, sep) ) {
e2011220 0:ee7e9405e1c7 100 strings.push_back(buffer);
e2011220 0:ee7e9405e1c7 101 }
e2011220 0:ee7e9405e1c7 102 return strings;
e2011220 0:ee7e9405e1c7 103 }