diff --git a/src/bioloid_motion_pages.cpp b/src/bioloid_motion_pages.cpp
index 43233634d58114cb97f52ef5d22c73674b30fbe1..e093ce6840a95d89355462fad239855a97107dc6 100755
--- a/src/bioloid_motion_pages.cpp
+++ b/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);
}
diff --git a/src/stm32_src_mtn.cpp b/src/stm32_src_mtn.cpp
index bc81b87ea67c8cd422e073c3377c24508f551fea..cbd0c5b213b16d11496670410de0e7c8ed44b5b9 100644
--- a/src/stm32_src_mtn.cpp
+++ b/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++)
diff --git a/src/stm32_src_mtn.h b/src/stm32_src_mtn.h
index 7514450a8000dd7faa624ffeb9af4256eec7d092..490113befe494f065794e8c04ae55d16bf73e385 100644
--- a/src/stm32_src_mtn.h
+++ b/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);