Corrected the computation of the page checksum.

src/bioloid_motion_pages.cpp

@@ -51,34 +51,39 @@ int main(const int argc,char *argv[])
       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++)
-  {
-    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++)
+  try{
+    /* show motion pages */
+    std::cout << "Num. pages: " << motions.get_num_pages() << std::endl;
+    for(i=0;i<motions.get_num_pages();i++)
     {
-      step=page.get_step(j);
-      std::cout << "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 << i << std::endl;
+      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);
+        std::cout << "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;
+      }
     }
+    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]));
+  }catch(CMtnException &e){
+    std::cout << e.what() << 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]));
   
   return(EXIT_SUCCESS);
 }

src/stm32_src_mtn.cpp

@@ -65,16 +65,18 @@ void CSTM32SrcMtn::write_word_vector(std::ofstream &file,const std::vector<unsig
     file << std::endl;
 }
 
-unsigned char CSTM32SrcMtn::compute_checksum(CRobotisPage &page)
+unsigned char CSTM32SrcMtn::compute_checksum(int page_id)
 {
   std::vector<unsigned short int> angles;
   std::vector<unsigned char> cw_compliance;
   std::vector<unsigned char> ccw_compliance;
   unsigned char checksum=0x00;
   unsigned int i=0,j=0;
+  CRobotisPage page;
   CRobotisStep step;
   std::string name;
   
+  page=this->motions.get_page(page_id);
   name=page.get_name();
   for(i=0;i<14;i++)
   {
@@ -84,6 +86,7 @@ unsigned char CSTM32SrcMtn::compute_checksum(CRobotisPage &page)
       checksum+=' ';
   }
   checksum+=(unsigned char)page.get_repetitions();
+  checksum+=0x0A;
   checksum+=(unsigned char)page.get_num_steps();
   checksum+=(unsigned char)page.get_speed_rate()*32;
   checksum+=(unsigned char)page.get_inertial();
@@ -98,16 +101,15 @@ unsigned char CSTM32SrcMtn::compute_checksum(CRobotisPage &page)
   for(i=0;i<page.get_num_steps();i++)
   {
     step=page.get_step(i);
-    angles=step.get_angles();
-    for(j=0;j<angles.size();j++)
+    for(j=0;j<motions.get_num_servos();j++)
     {
-      checksum+=(unsigned char)angles[i]&0x00FF;
-      checksum+=(unsigned char)(angles[i]/256)&0x00FF;
+      checksum+=(unsigned char)(motions.get_angle(j,page_id,i)*128)&0x00FF;
+      checksum+=(unsigned char)(((unsigned short)(motions.get_angle(j,page_id,i)*128))>>8)&0x00FF;
     }
     checksum+=(unsigned char)(step.get_pause_time()/0.0078);
     checksum+=(unsigned char)(step.get_step_time()/0.0078);
   }
-  return checksum;
+  return 256-checksum;
 }
 
 void CSTM32SrcMtn::load_motions(CRobotisMtn &motions)
@@ -158,7 +160,10 @@ void CSTM32SrcMtn::generate_src_file(const std::string &filename)
       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>(4,0),3); 
-      this->write_byte_vector(file,std::vector<unsigned char>(1,this->compute_checksum(page)),3); 
+      if(i==0)
+        this->write_byte_vector(file,std::vector<unsigned char>(1,0),3);
+      else
+        this->write_byte_vector(file,std::vector<unsigned char>(1,this->compute_checksum(i-1)),3); 
       byte_vector.clear();
       byte_vector.resize(PAGE_MAX_NUM_SERVOS);
       for(j=0;j<page.get_cw_compliances().size();j++)

src/stm32_src_mtn.h

@@ -10,7 +10,7 @@ class CSTM32SrcMtn
     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);
+    unsigned char compute_checksum(int page_id);
   public:
     CSTM32SrcMtn();
     void load_motions(CRobotisMtn &motions);