diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 54d0a6826fca60850449984669047632c5994d1f..1530e619ee9a543cf5722fc39bd58c83cf3fb2f2 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -10,9 +10,9 @@ INCLUDE_DIRECTORIES(${robotis_mtn_parser_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${robotis_bin_parser_INCLUDE_DIR})
# application source files
-SET(sources bioloid_motion_pages.cpp)
+SET(sources bioloid_motion_pages.cpp stm32_src_mtn.cpp)
# application header files
-SET(headers bioloid_motion_pages.h)
+SET(headers bioloid_motion_pages.h stm32_src_mtn.h)
# create the executable file
ADD_EXECUTABLE(bioloid_motion_pages ${sources})
diff --git a/src/bioloid_motion_pages.cpp b/src/bioloid_motion_pages.cpp
index 208bd0c59da92db11bfdc7fa0c2afa7811326ec2..4e2494054b67a3243a9981b91ffefc8bc5abd517 100755
--- a/src/bioloid_motion_pages.cpp
+++ b/src/bioloid_motion_pages.cpp
@@ -4,16 +4,19 @@
#include "mtn_file_parser.hpp"
#include "robotis_bin_parser.h"
+#include "stm32_src_mtn.h"
+#include "mtn_exceptions.h"
int main(const int argc,char *argv[])
{
std::vector<unsigned short int> angles;
MTN::CMtnFileParser mtn_parser;
CBinFileParser bin_parser;
+ CSTM32SrcMtn source;
unsigned int i=0,j=0,k=0;
- CRobotisMtn *motions;
- CRobotisPage *page;
- CRobotisStep *step;
+ CRobotisMtn motions;
+ CRobotisPage page;
+ CRobotisStep step;
if(argc!=2)
{
@@ -22,52 +25,53 @@ int main(const int argc,char *argv[])
}
else
{
- if(strstr(argv[1],".mtn")!=NULL)
- {
- mtn_parser.parse(argv[1]);
- motions=mtn_parser.get_motions();
- }
- else if(strstr(argv[1],".bin")!=NULL)
- {
- bin_parser.parse(argv[1]);
- motions=bin_parser.get_motions();
- }
- else
- {
- std::cout << "Invalid file format. Supported formats are .mtn and .bin." << std::endl;
- return (EXIT_FAILURE);
+ try{
+ if(strstr(argv[1],".mtn")!=NULL)
+ {
+ mtn_parser.parse(argv[1]);
+ mtn_parser.get_motions(motions);
+ }
+ else if(strstr(argv[1],".bin")!=NULL)
+ {
+ bin_parser.parse(argv[1]);
+ bin_parser.get_motions(motions);
+ }
+ else
+ {
+ std::cout << "Invalid file format. Supported formats are .mtn and .bin." << std::endl;
+ return (EXIT_FAILURE);
+ }
+ }catch(CMtnException &e){
+ std::cout << e.what() << std::endl;
}
}
/* show motion pages */
- std::cout << "Num. pages: " << motions->get_num_pages() << std::endl;
- for(i=0;i<motions->get_num_pages();i++)
+ std::cout << "Num. pages: " << motions.get_num_pages() << std::endl;
+ for(i=0;i<motions.get_num_pages();i++)
{
- page=motions->get_page(i);
- std::cout << "name: " << page->get_name() << std::endl;
- std::cout << "next page: " << page->get_next_page() << std::endl;
- std::cout << "exit page: " << page->get_exit_page() << std::endl;
- std::cout << "repetitions: " << page->get_repetitions() << std::endl;
- std::cout << "speed rate: " << page->get_speed_rate() << std::endl;
- std::cout << "inertial force: " << page->get_inertial() << std::endl;
- std::cout << "Num. steps: " << page->get_num_steps() << std::endl;
- for(j=0;j<page->get_num_steps();j++)
+ page=motions.get_page(i);
+ std::cout << "name: " << page.get_name() << std::endl;
+ std::cout << "next page: " << page.get_next_page() << std::endl;
+ std::cout << "exit page: " << page.get_exit_page() << std::endl;
+ std::cout << "repetitions: " << page.get_repetitions() << std::endl;
+ std::cout << "speed rate: " << page.get_speed_rate() << std::endl;
+ std::cout << "inertial force: " << page.get_inertial() << std::endl;
+ std::cout << "Num. steps: " << page.get_num_steps() << std::endl;
+ for(j=0;j<page.get_num_steps();j++)
{
- step=page->get_step(j);
+ step=page.get_step(j);
std::cout << "angles: ";
- angles=step->get_angles();
+ angles=step.get_angles();
for(k=0;k<angles.size();k++)
std::cout << angles[k] << ",";
std::cout << std::endl;
- std::cout << "pausetime: " << step->get_pause_time() << std::endl;
- std::cout << "step time: " << step->get_step_time() << std::endl;
+ std::cout << "pausetime: " << step.get_pause_time() << std::endl;
+ std::cout << "step time: " << step.get_step_time() << std::endl;
}
-
}
- /* generate the stm32 c code for the motion pages */
- std::cout << "#include \"motion_pages.h\"\n" << std::endl << std::endl;
- std::cout << "TPage motion_pages[MAX_PAGES] __attribute__ ((section (\".pages\")))={\n" << std::endl;
-
-
+ source.load_motions(motions);
+ source.generate_src_file(std::string("test_src.c"));
+
return(EXIT_SUCCESS);
}
diff --git a/src/stm32_src_mtn.cpp b/src/stm32_src_mtn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7809fa4b2f3fb66d77867e517fd64932f9df6142
--- /dev/null
+++ b/src/stm32_src_mtn.cpp
@@ -0,0 +1,171 @@
+#include "robotis_bin_parser.h"
+#include "stm32_src_mtn.h"
+#include <string.h>
+#include <iomanip>
+
+CSTM32SrcMtn::CSTM32SrcMtn()
+{
+}
+
+void CSTM32SrcMtn::write_byte_vector(std::ofstream &file,const std::vector<unsigned char> &vector,unsigned int level,bool last)
+{
+ unsigned int i=0;
+
+ if(vector.size()>1)
+ {
+ for(i=0;i<level;i++)
+ file << " ";
+ file << "{";
+ }
+ else
+ {
+ for(i=0;i<level;i++)
+ file << " ";
+ }
+ for(i=0;i<vector.size();i++)
+ {
+ file << "0x" << std::hex << std::setw(2) << std::setfill('0') << (int)vector[i];
+ if(i<(vector.size()-1))
+ file << ",";
+ }
+ if(vector.size()>1)
+ file << "}";
+ if(!last)
+ file << "," << std::endl;
+ else
+ file << std::endl;
+}
+
+void CSTM32SrcMtn::write_word_vector(std::ofstream &file,const std::vector<unsigned short int> &vector,unsigned int level,bool last)
+{
+ unsigned int i=0;
+
+ if(vector.size()>1)
+ {
+ for(i=0;i<level;i++)
+ file << " ";
+ file << "{";
+ }
+ else
+ {
+ for(i=0;i<level;i++)
+ file << " ";
+ }
+ for(i=0;i<vector.size();i++)
+ {
+ file << "0x" << std::hex << std::setw(4) << std::setfill('0') << vector[i];
+ if(i<(vector.size()-1))
+ file << ",";
+ }
+ if(vector.size()>1)
+ file << "}";
+ if(!last)
+ file << "," << std::endl;
+ else
+ file << std::endl;
+}
+
+unsigned char CSTM32SrcMtn::compute_checksum(CRobotisPage &page)
+{
+ return 0x00;
+}
+
+void CSTM32SrcMtn::load_motions(CRobotisMtn &motions)
+{
+ this->motions.clear();
+ this->motions=motions;
+}
+
+void CSTM32SrcMtn::generate_src_file(const std::string &filename)
+{
+ std::vector<unsigned short int> word_vector;
+ std::vector<unsigned char> byte_vector;
+ unsigned int i=0,j=0;
+ CRobotisPage page;
+ CRobotisStep step;
+ std::ofstream file;
+
+ file.open(filename.c_str(),std::ofstream::trunc);
+ if(file.is_open())
+ {
+ file << "#include \"motion_pages.h\"\n" << std::endl << std::endl;
+ file << "TPage motion_pages[MAX_PAGES] __attribute__ ((section (\".pages\")))=" << std::endl;
+ file << "{" << std::endl;
+ for(i=0;i<this->motions.get_num_pages();i++)
+ {
+ file << " {" << std::endl;// page start
+ file << " {" << std::endl;// header start
+ byte_vector.clear();
+ page=this->motions.get_page(i);
+ // motion name
+ byte_vector.resize(14);
+ memcpy(byte_vector.data(),page.get_name().data(),page.get_name().size());
+ memset(&byte_vector.data()[page.get_name().size()],' ',14-page.get_name().size());
+ this->write_byte_vector(file,byte_vector,3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,0),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_repetitions()),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,0),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,3),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_num_steps()),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,0),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_speed_rate()*32),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,0),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_inertial()),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_next_page()),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_exit_page()),3);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,4),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());
+ 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
+ file << " {" << std::endl;// steps start
+ // motion steps
+ for(j=0;j<7;j++)
+ {
+ file << " {" << std::endl;
+ if(j<page.get_num_steps())
+ {
+ step=page.get_step(j);
+ word_vector.resize(PAGE_MAX_NUM_SERVOS);
+ memcpy(word_vector.data(),step.get_angles().data(),step.get_angles().size()*sizeof(unsigned short int));
+ memset(&word_vector.data()[step.get_angles().size()],0x00,(PAGE_MAX_NUM_SERVOS-step.get_angles().size())*sizeof(unsigned short int));
+ this->write_word_vector(file,word_vector,4);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,(unsigned char)(step.get_pause_time()/0.0078)),4);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,step.get_step_time()/0.0078),4,true);
+ }
+ else
+ {
+ word_vector.resize(PAGE_MAX_NUM_SERVOS);
+ memset(word_vector.data(),0x00,PAGE_MAX_NUM_SERVOS*sizeof(unsigned short int));
+ this->write_word_vector(file,word_vector,4);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,0),4);
+ this->write_byte_vector(file,std::vector<unsigned char>(1,0),4,true);
+ }
+ file << " }";
+ if(j<6)
+ file << ","<< std::endl;
+ else
+ file << std::endl;
+ }
+ file << " }" << std::endl;// steps end
+ if(i<254)
+ file << " }," << std::endl;//page end
+ else
+ file << " }" << std::endl;//page end
+ }
+ file << "};" << std::endl;
+
+ file.close();
+ }
+ else
+ {
+ /* handle errors */
+ }
+}
+
+CSTM32SrcMtn::~CSTM32SrcMtn()
+{
+}
+
diff --git a/src/stm32_src_mtn.h b/src/stm32_src_mtn.h
index 8b0b6bed2ac8197ee5ae870d829218f8648152ca..7514450a8000dd7faa624ffeb9af4256eec7d092 100644
--- a/src/stm32_src_mtn.h
+++ b/src/stm32_src_mtn.h
@@ -1,14 +1,20 @@
#ifndef _STM32_SRC_MTN_H
#define _STM32_SRC_MTN_H
-#include "mtn_file.h";
+#include "robotis_mtn.h"
+#include <fstream>
class CSTM32SrcMtn
{
private:
- CMtnFile *motions;
+ CRobotisMtn motions;
+ void write_byte_vector(std::ofstream &file,const std::vector<unsigned char> &vector,unsigned int level=0,bool last=false);
+ void write_word_vector(std::ofstream &file,const std::vector<unsigned short int> &vector,unsigned int level=0,bool last=false);
+ unsigned char compute_checksum(CRobotisPage &page);
public:
CSTM32SrcMtn();
+ void load_motions(CRobotisMtn &motions);
+ void generate_src_file(const std::string &filename);
~CSTM32SrcMtn();
};