diff --git a/src/robotis_bin_file/robotis_bin_parser.cpp b/src/robotis_bin_file/robotis_bin_parser.cpp index e86a0a836616d1dc8c836fe9146fe1c2ea7b7667..725880c5dd4989beedac8b31abc5aff311b5d8a3 100644 --- a/src/robotis_bin_file/robotis_bin_parser.cpp +++ b/src/robotis_bin_file/robotis_bin_parser.cpp @@ -103,7 +103,56 @@ void CBinFileParser::parse(const char *filename) void CBinFileParser::serialize(const char *filename) { + std::ofstream out_file(filename); + unsigned int i=0,j=0,k=0; + CRobotisPage page; + CRobotisStep step; + // insert a null page + memset(&this->pages[0].header.name[page.get_name().size()],' ',14); + for(j=0;j<this->motions.get_num_servos();j++) + this->pages[0].header.slope[j]=0x55; + this->pages[0].header.next=0; + this->pages[0].header.exit=0; + this->pages[0].header.repeat=0; + this->pages[0].header.speed=32; + this->pages[0].header.accel=32; + this->pages[0].header.stepnum=0; + for(j=0;j<POSE_NUMBER_OF_POSES_PER_PAGE;j++) + { + this->pages[0].steps[j].pause=0; + this->pages[0].steps[j].time=0; + for(k=0;k<this->motions.get_num_servos();k++) + this->pages[0].steps[j].position[k]=512; + } + this->pages[0].header.checksum=this->generate_checksum(&this->pages[0]); + for(i=0;i<this->motions.get_num_pages();i++) + { + page=this->motions.get_page(i); + strncpy(this->pages[i+1].header.name,page.get_name().c_str(),page.get_name().size()); + if(page.get_name().size()<14) + memset(&this->pages[i+1].header.name[page.get_name().size()],' ',14-page.get_name().size()); + for(j=0;j<this->motions.get_num_servos();j++) + this->pages[i+1].header.slope[j]=page.get_cw_compliance(j)<<4|page.get_ccw_compliance(j); + this->pages[i+1].header.next=page.get_next_page(); + this->pages[i+1].header.exit=page.get_exit_page(); + this->pages[i+1].header.repeat=page.get_repetitions(); + this->pages[i+1].header.speed=page.get_speed_rate()*32; + this->pages[i+1].header.accel=page.get_inertial(); + this->pages[i+1].header.stepnum=page.get_num_steps(); + for(j=0;j<page.get_num_steps();j++) + { + step=page.get_step(j); + this->pages[i+1].steps[j].pause=step.get_pause_time()/0.0078; + this->pages[i+1].steps[j].time=step.get_step_time()/0.0078; + for(k=0;k<this->motions.get_num_servos();k++) + this->pages[i+1].steps[j].position[k]=step.get_angle(k); + } + // compute the checksum + this->pages[i+1].header.checksum=this->generate_checksum(&this->pages[i+1]); + } + out_file.write((char *)this->pages,sizeof(this->pages)); + out_file.close(); } void CBinFileParser::get_motions(CRobotisMtn &motions) diff --git a/src/robotis_mtn/robotis_mtn.cpp b/src/robotis_mtn/robotis_mtn.cpp index a4e5271ba313c6b925f24e5148149cdb82526fae..dc98a0c2aeb23663733bbd84a73af2e9bf624800 100644 --- a/src/robotis_mtn/robotis_mtn.cpp +++ b/src/robotis_mtn/robotis_mtn.cpp @@ -152,6 +152,13 @@ void CRobotisMtn::set_motor_type(unsigned int servo, int motor_type) this->info[servo].model=SERVO_MX106; this->info[servo].model_string="MX-106"; break; + case SERVO_UNKNOWN: this->info[servo].encoder_resolution=0; + this->info[servo].max_angle=0; + this->info[servo].center_angle=0; + this->info[servo].max_speed=0; + this->info[servo].model=SERVO_UNKNOWN; + this->info[servo].model_string="unknown"; + break; default: throw CMtnException(_HERE_,"Invalid servo type"); break; } @@ -377,7 +384,10 @@ double CRobotisMtn::get_angle(unsigned int servo_id, unsigned char page_id, unsi else { value=step.get_angle(servo_id); - return (value*this->info[servo_id].max_angle)/this->info[servo_id].encoder_resolution-this->info[servo_id].center_angle; + if(this->info[servo_id].model==SERVO_UNKNOWN) + return value; + else + return (value*this->info[servo_id].max_angle)/this->info[servo_id].encoder_resolution-this->info[servo_id].center_angle; } } diff --git a/src/robotis_mtn/robotis_mtn.h b/src/robotis_mtn/robotis_mtn.h index e50f377c4d86bc323ac577cd117715487f07f6c2..41c8aead034e6ee04b18279f4b5ab78b1bf7d01c 100644 --- a/src/robotis_mtn/robotis_mtn.h +++ b/src/robotis_mtn/robotis_mtn.h @@ -13,6 +13,7 @@ #define SERVO_MX64 0x0136 #define SERVO_EX106 0x006B #define SERVO_MX106 0x0140 +#define SERVO_UNKNOWN 0x0000 typedef struct { diff --git a/src/robotis_mtn_file/mtn_file.y b/src/robotis_mtn_file/mtn_file.y index c8dca07fc754a5ec8f107af9b3a8b494850a3f3f..6d4d3139db1fab0d54cf89b4f6aa875074d8fe9d 100644 --- a/src/robotis_mtn_file/mtn_file.y +++ b/src/robotis_mtn_file/mtn_file.y @@ -108,7 +108,7 @@ name: NAME EQUAL { page.clear(); page.set_name(std::string("")); } | NAME EQUAL strings { page.clear(); page.set_num_servos(num_servos); - page.set_name(motion_name); } + page.set_name(motion_name);} ; strings: strings STRING { motion_name+=" "+*($2); }