Changed the motion_page file generator to use angles instead of servo values.

Changed the default values for the next_page, exit_page and inertial values of a page.

src/bioloid_motion_pages.cpp

@@ -10,6 +10,7 @@
 int main(const int argc,char *argv[])
 {
   std::vector<unsigned short int> angles;
+  std::string servo_type="AX-12A";
   MTN::CMtnFileParser mtn_parser;
   CBinFileParser bin_parser;
   CSTM32SrcMtn source;
@@ -74,6 +75,8 @@ int main(const int argc,char *argv[])
       std::cout << "step time: " << step.get_step_time() << std::endl;
     }
   }
+  for(i=0;i<motions.get_num_servos();i++)
+    motions.set_motor_type(i,servo_type);
   source.load_motions(motions);
   source.generate_src_file(std::string(argv[2]));
   

src/stm32_src_mtn.cpp

@@ -120,7 +120,7 @@ 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;
+  unsigned int i=0,j=0,k=0;
   CRobotisPage page;
   CRobotisStep step;
   std::ofstream file;
@@ -131,20 +131,24 @@ void CSTM32SrcMtn::generate_src_file(const std::string &filename)
     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++)
+    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);
+      if(i==0)
+        page.clear();
+      else
+        page=this->motions.get_page(i-1);
       // motion name
+      byte_vector.clear();
       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,0x0A),3); 
       this->write_byte_vector(file,std::vector<unsigned char>(3,0),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); 
@@ -155,6 +159,7 @@ void CSTM32SrcMtn::generate_src_file(const std::string &filename)
       this->write_byte_vector(file,std::vector<unsigned char>(1,page.get_exit_page()),3); 
       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.clear();
       byte_vector.resize(PAGE_MAX_NUM_SERVOS);
       for(j=0;j<page.get_cw_compliances().size();j++)
       {
@@ -172,8 +177,10 @@ void CSTM32SrcMtn::generate_src_file(const std::string &filename)
         if(j<page.get_num_steps())
         {
           step=page.get_step(j);
+          word_vector.clear();
           word_vector.resize(PAGE_MAX_NUM_SERVOS);
-          memcpy(word_vector.data(),step.get_angles().data(),step.get_angles().size()*sizeof(unsigned short int));
+          for(k=0;k<motions.get_num_servos();k++)
+            word_vector[k]=(unsigned short int)(motions.get_angle(k,i-1,j)*128);
           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); 
@@ -194,7 +201,7 @@ void CSTM32SrcMtn::generate_src_file(const std::string &filename)
           file << std::endl;
       }
       file << "    }" << std::endl;// steps end
-      if(i<254)
+      if(i<255)
         file << "  }," << std::endl;//page end
       else
         file << "  }" << std::endl;//page end