Changed all the motion pages related classes to copy data structures instead of using pointers.

Added a class to generate the stm32 code. It's working.

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})
 

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);
 }
 

src/stm32_src_mtn.cpp

+#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()
+{
+}
+

src/stm32_src_mtn.h

 #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();
 };