diff --git a/src/bioloid_motion_pages.cpp b/src/bioloid_motion_pages.cpp index 4e2494054b67a3243a9981b91ffefc8bc5abd517..fe6537c8223c556d78ce22cc10a64f90210c75e5 100755 --- a/src/bioloid_motion_pages.cpp +++ b/src/bioloid_motion_pages.cpp @@ -18,9 +18,9 @@ int main(const int argc,char *argv[]) CRobotisPage page; CRobotisStep step; - if(argc!=2) + if(argc!=3) { - std::cout << "A motion file must be provided (either .mtn or .bin)" << std::endl; + std::cout << "A motion file (either .mtn or .bin) and the output filename must be provided" << std::endl; return (EXIT_FAILURE); } else @@ -41,6 +41,11 @@ int main(const int argc,char *argv[]) std::cout << "Invalid file format. Supported formats are .mtn and .bin." << std::endl; return (EXIT_FAILURE); } + if(strstr(argv[2],".c")==NULL) + { + std::cout << "Invalid output file format. Supported format is .c." << std::endl; + return (EXIT_FAILURE); + } }catch(CMtnException &e){ std::cout << e.what() << std::endl; } @@ -70,7 +75,7 @@ int main(const int argc,char *argv[]) } } source.load_motions(motions); - source.generate_src_file(std::string("test_src.c")); + source.generate_src_file(std::string(argv[2])); return(EXIT_SUCCESS); } diff --git a/src/stm32_src_mtn.cpp b/src/stm32_src_mtn.cpp index e036f01bfe630dd45e948bf8b77601c7e5b52f7a..b89fade66c7e3872d180965e1e449f9ce56314f4 100644 --- a/src/stm32_src_mtn.cpp +++ b/src/stm32_src_mtn.cpp @@ -68,7 +68,8 @@ void CSTM32SrcMtn::write_word_vector(std::ofstream &file,const std::vector<unsig unsigned char CSTM32SrcMtn::compute_checksum(CRobotisPage &page) { std::vector<unsigned short int> angles; - std::vector<unsigned char> compliance; + std::vector<unsigned char> cw_compliance; + std::vector<unsigned char> ccw_compliance; unsigned char checksum=0x00; unsigned int i=0,j=0; CRobotisStep step; @@ -88,9 +89,12 @@ unsigned char CSTM32SrcMtn::compute_checksum(CRobotisPage &page) checksum+=(unsigned char)page.get_inertial(); checksum+=(unsigned char)page.get_next_page(); checksum+=(unsigned char)page.get_exit_page(); - compliance=page.get_compliances(); - for(i=0;i<compliance.size();i++) - checksum+=compliance[i]; + cw_compliance=page.get_cw_compliances(); + ccw_compliance=page.get_ccw_compliances(); + for(i=0;i<cw_compliance.size();i++) + { + checksum+=(cw_compliance[i]<<4)+ccw_compliance[i]; + } for(i=0;i<page.get_num_steps();i++) { step=page.get_step(i); @@ -152,7 +156,11 @@ void CSTM32SrcMtn::generate_src_file(const std::string &filename) this->write_byte_vector(file,std::vector<unsigned char>(4,0),3); this->write_byte_vector(file,std::vector<unsigned char>(1,this->compute_checksum(page)),3); byte_vector.resize(PAGE_MAX_NUM_SERVOS); - memcpy(byte_vector.data(),page.get_compliances().data(),page.get_compliances().size()); + for(j=0;j<page.get_cw_compliances().size();j++) + { + byte_vector[j]=page.get_cw_compliance(j)<<4; + byte_vector[j]|=page.get_ccw_compliance(j); + } this->write_byte_vector(file,byte_vector,3); this->write_byte_vector(file,std::vector<unsigned char>(1,0),3,true); file << " }," << std::endl;// header end