diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 4d10d163828c1f7031ded7d0e28b838e42a71b07..9e86a4cc1594fc0d44274018e7ba09ac533e093e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,9 +1,8 @@
-ADD_SUBDIRECTORY(parser)
+ADD_SUBDIRECTORY(mtn)
# locate the necessary dependencies
# add the necessary include directories
-INCLUDE_DIRECTORIES(. ./parser)
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}/parser)
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR})
+INCLUDE_DIRECTORIES(. ./mtn)
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}/mtn)
# application source files
SET(sources bioloid_motion_pages.cpp)
# application header files
diff --git a/src/bin/bin_file_parser.cpp b/src/bin/bin_file_parser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..015cedb387ea0e36bd88947b8053f7f3f9b84be1
--- /dev/null
+++ b/src/bin/bin_file_parser.cpp
@@ -0,0 +1,75 @@
+#include "bin_file_parser.h"
+#include <cctype>
+#include <fstream>
+
+CBinFileParser::CBinFileParser()
+{
+ memset(this->pages,0x00,sizeof(this->pages));
+}
+
+void CBinFileParser::parse(const char *filename)
+{
+ std::vector<unsigned short int> angles;
+ std::ifstream in_file(filename);
+ std::vector<int> compliance;
+ int length=0,i=0,j=0;
+ CMtnPage *page;
+ CMtnStep *step;
+
+ if(!in_file.good())
+ {
+ std::cout << " File << " << filename << " not found" << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ in_file.seekg (0, in_file.end);
+ length = in_file.tellg();
+ in_file.seekg (0, in_file.beg);
+ if(length!=sizeof(this->pages))
+ {
+ std::cout << "Invalid binary motion file" << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ in_file.read((char *)this->pages,length);
+ in_file.close();
+ // initialize the CMtnFile class
+ this->motions.set_type("motion");
+ this->motions.set_version(1.0);
+ this->motions.set_num_servos(PAGE_MAX_NUM_SERVOS);
+ for(i=0;i<MAX_NUM_PAGES;i++)
+ {
+ page=new CMtnPage(PAGE_MAX_NUM_SERVOS);
+ page->set_name(this->pages[i].header.name);
+ compliance.resize(PAGE_MAX_NUM_SERVOS);
+ for(j=0;j<PAGE_MAX_NUM_SERVOS;j++)
+ compliance[j]=this->pages[i].header.slope[j];
+ page->set_compliances(compliance);
+ page->set_next_page(this->pages[i].header.next);
+ page->set_exit_page(this->pages[i].header.exit);
+ page->set_repetitions(this->pages[i].header.repeat);
+ page->set_speed_rate(this->pages[i].header.speed);
+ page->set_inertial(this->pages[i].header.accel);
+ for(j=0;j<this->pages[i].header.num_steps;j++)
+ {
+ step=new CMtnStep(PAGE_MAX_NUM_SERVOS);
+ step->set_pause_time(this->pages[i].steps[j].pause);
+ step->set_step_time(this->pages[i].steps[j].time);
+ angles.resize(PAGE_MAX_NUM_SERVOS);
+ for(k=0;k<PAGE_MAX_NUM_SERVOS;k++)
+ angles[k]=this->pages[i].steps[j].position[k];
+ step->set_angles(angles);
+ page->add_step(step);
+ }
+ this->motions->add_page(page);
+ }
+}
+
+CMtnFile *CBinFileParser::get_motions(void)
+{
+ return this->motions;
+}
+
+CBinFileParser::~CBinFileParser()
+{
+ memset(this->pages,0x00,sizeof(this->pages));
+}
+
diff --git a/src/bin/bin_file_parser.h b/src/bin/bin_file_parser.h
new file mode 100644
index 0000000000000000000000000000000000000000..ba2e6735bf6b1dc5fdbc8eb63844649c220bdb62
--- /dev/null
+++ b/src/bin/bin_file_parser.h
@@ -0,0 +1,55 @@
+#ifndef _BIN_FILE_PARSER_H
+#define _BIN_FILE_PARSER_H
+
+#include "mtn_file.h"
+
+#define MAX_NUM_PAGES 256
+#define PAGE_MAX_NUM_SERVOS 31
+#define POSE_NUMBER_OF_POSES_PER_PAGE 7
+
+typedef struct // Header Structure (total 64unsigned char)
+{
+ unsigned char name[13]; // Name 0~13
+ unsigned char reserved1; // Reserved1 14
+ unsigned char repeat; // Repeat count 15
+ unsigned char schedule; // schedule 16
+ unsigned char reserved2[3]; // reserved2 17~19
+ unsigned char stepnum; // Number of step 20
+ unsigned char reserved3; // reserved3 21
+ unsigned char speed; // Speed 22
+ unsigned char reserved4; // reserved4 23
+ unsigned char accel; // Acceleration time 24
+ unsigned char next; // Link to next 25
+ unsigned char exit; // Link to exit 26
+ unsigned char reserved5[4]; // reserved5 27~30
+ unsigned char checksum; // checksum 31
+ unsigned char slope[PAGE_MAX_NUM_SERVOS]; // CW/CCW compliance slope 32~62
+ unsigned char reserved6; // reserved6 63
+} TPageHeader;
+
+typedef struct // Step Structure (total 64unsigned char)
+{
+ unsigned short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
+ unsigned char pause; // Pause time 62
+ unsigned char time; // Time 63
+} TStep;
+
+typedef struct // Page Structure (total 512unsigned char)
+{
+ TPageHeader header; // Page header 0~64
+ TStep steps[POSE_NUMBER_OF_POSES_PER_PAGE]; // Page step 65~511
+} TPage;
+
+class CBinFileParser
+{
+ private:
+ CMtnFile motions;
+ TPage pages[MAX_NUM_PAGES];
+ public:
+ CBinFileParser();
+ void parse(const char *filename);
+ CMtnFile *get_motions(void);
+ ~CBinFileParser();
+};
+
+#endif
diff --git a/src/bioloid_motion_pages.cpp b/src/bioloid_motion_pages.cpp
index b374f6f3ce547b2149385249cdc6d6d064934f36..27fa5098ac654da96fc2374b7ab46c5bcbfcb5d0 100755
--- a/src/bioloid_motion_pages.cpp
+++ b/src/bioloid_motion_pages.cpp
@@ -1,5 +1,6 @@
#include <iostream>
#include <cstdlib>
+#include <string.h>
#include "mtn_file_parser.hpp"
@@ -12,7 +13,24 @@ int main(const int argc,char *argv[])
CMtnStep *step;
if(argc!=2)
+ {
+ std::cout << "A motion file must be provided (either .mtn or .bin)" << std::endl;
return (EXIT_FAILURE);
+ }
+ else
+ {
+ if(strstr(argv[1],".mtn")!=NULL)
+ {
+ }
+ else if(strstr(argv[1],".bin")!=NULL)
+ {
+ }
+ else
+ {
+ std::cout << "Invalid file format. Supported formats are .mtn and .bin." << std::endl;
+ return (EXIT_FAILURE);
+ }
+ }
MTN::MTN_File_Parser mtn_parser;
mtn_parser.parse(argv[1]);
motions=mtn_parser.get_motions();
diff --git a/src/mtn/CMakeLists.txt b/src/mtn/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2c36ec55b20bcd0dc9ba1eea2e3c04e3a5758774
--- /dev/null
+++ b/src/mtn/CMakeLists.txt
@@ -0,0 +1,33 @@
+# Create target for the parser
+FIND_PACKAGE(BISON REQUIRED)
+FIND_PACKAGE(FLEX REQUIRED)
+
+SET(BisonOutput mtn_parser.cpp)
+IF(BISON_FOUND)
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${BisonOutput}
+ COMMAND ${BISON_EXECUTABLE}
+ --output=${BisonOutput}
+ ${CMAKE_CURRENT_SOURCE_DIR}/mtn_file.y
+ COMMENT "Generating mtn_parser.cpp"
+ )
+ENDIF()
+
+FIND_PACKAGE(FLEX REQUIRED)
+SET(FlexOutput mtn_scanner.cpp)
+IF(FLEX_FOUND)
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${FlexOutput}
+ COMMAND ${FLEX_EXECUTABLE}
+ --outfile=${FlexOutput}
+ ${CMAKE_CURRENT_SOURCE_DIR}/mtn_file.l
+ COMMENT "Generating mtn_scanner.cpp"
+ )
+ENDIF()
+
+SET(mtn_SRCS mtn_file_parser.cpp mtn_file.cpp mtn_page.cpp mtn_step.cpp)
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+add_library(mtn_parser SHARED ${BisonOutput} ${FlexOutput} ${mtn_SRCS})
+
diff --git a/src/mtn/mtn_file.cpp b/src/mtn/mtn_file.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7fe8e0d96c3c0b733dcb6a27d7df5de31d500485
--- /dev/null
+++ b/src/mtn/mtn_file.cpp
@@ -0,0 +1,154 @@
+#include "mtn_file.h"
+
+CMtnFile::CMtnFile()
+{
+ this->type="";
+ this->version=0.0;
+ this->enable.clear();
+ this->motor_type.clear();
+ this->pages.clear();
+}
+
+void CMtnFile::set_type(std::string &type)
+{
+ this->type=type;
+}
+
+void CMtnFile::set_version(double version)
+{
+ this->version=version;
+}
+
+void CMtnFile::set_num_servos(unsigned int num_servos)
+{
+ this->enable.resize(num_servos);
+ this->motor_type.resize(num_servos);
+}
+
+void CMtnFile::set_enable(unsigned int servo,bool enable)
+{
+ if(servo>this->enable.size())
+ {
+ /* handle errors */
+ }
+ else
+ this->enable[servo]=enable;
+}
+
+void CMtnFile::set_enables(std::vector<bool> &enables)
+{
+ unsigned int i=0;
+
+ if(this->enable.size()!=enables.size())
+ {
+ /* handle errors */
+ }
+ else
+ for(i=0;i<this->enable.size();i++)
+ this->enable[i]=enables[i];
+}
+
+void CMtnFile::set_motor_type(unsigned int servo, int motor_type)
+{
+ if(servo>this->motor_type.size())
+ {
+ /* handle errors */
+ }
+ else
+ this->motor_type[servo]=motor_type;
+}
+
+void CMtnFile::set_motor_types(std::vector<int> &motor_types)
+{
+ unsigned int i=0;
+
+ if(this->motor_type.size()!=motor_types.size())
+ {
+ /* handle errors */
+ }
+ else
+ for(i=0;i<this->motor_type.size();i++)
+ this->motor_type[i]=motor_types[i];
+}
+
+void CMtnFile::add_page(CMtnPage *page)
+{
+ this->pages.push_back(page);
+}
+
+std::string CMtnFile::get_type(void)
+{
+ return this->type;
+}
+
+double CMtnFile::get_version(void)
+{
+ return this->version;
+}
+
+unsigned int CMtnFile::get_num_servos(void)
+{
+ return this->enable.size();
+}
+
+bool CMtnFile::get_enable(unsigned int servo)
+{
+ if(servo>this->enable.size())
+ {
+ /* handle errors */
+ }
+ else
+ return this->enable[servo];
+}
+
+std::vector<bool> CMtnFile::get_enables(void)
+{
+ return this->enable;
+}
+
+int CMtnFile::get_motor_type(unsigned int servo)
+{
+ if(servo>this->motor_type.size())
+ {
+ /* handle errors */
+ }
+ else
+ return this->motor_type[servo];
+}
+
+std::vector<int> CMtnFile::get_motor_types(void)
+{
+ return this->motor_type;
+}
+
+unsigned int CMtnFile::get_num_pages(void)
+{
+ return this->pages.size();
+}
+
+CMtnPage *CMtnFile::get_page(unsigned int page)
+{
+ if(page>this->pages.size())
+ {
+ /* handle errors */
+ }
+ else
+ return this->pages[page];
+}
+
+CMtnFile::~CMtnFile()
+{
+ unsigned int i=0;
+
+ this->type="";
+ this->version=0.0;
+ this->enable.clear();
+ this->motor_type.clear();
+ for(i=0;i<this->pages.size();i++)
+ {
+ delete this->pages[i];
+ this->pages[i]=NULL;
+ }
+ this->pages.clear();
+}
+
diff --git a/src/mtn/mtn_file.h b/src/mtn/mtn_file.h
new file mode 100644
index 0000000000000000000000000000000000000000..edd2f5591c5bbd5102f290c9f112a1fc515b012b
--- /dev/null
+++ b/src/mtn/mtn_file.h
@@ -0,0 +1,36 @@
+#ifndef _MTN_FILE_H
+#define _MTN_FILE_H
+
+#include "mtn_page.h"
+
+class CMtnFile
+{
+ private:
+ std::string type;
+ double version;
+ std::vector<bool> enable;
+ std::vector<int> motor_type;
+ std::vector<CMtnPage *> pages;
+ public:
+ CMtnFile();
+ void set_type(std::string &type);
+ void set_version(double version);
+ void set_num_servos(unsigned int num_servos);
+ void set_enable(unsigned int servo,bool enable);
+ void set_enables(std::vector<bool> &enables);
+ void set_motor_type(unsigned int servo, int motor_type);
+ void set_motor_types(std::vector<int> &motor_types);
+ void add_page(CMtnPage *page);
+ std::string get_type(void);
+ double get_version(void);
+ unsigned int get_num_servos(void);
+ bool get_enable(unsigned int servo);
+ std::vector<bool> get_enables(void);
+ int get_motor_type(unsigned int servo);
+ std::vector<int> get_motor_types(void);
+ unsigned int get_num_pages(void);
+ CMtnPage *get_page(unsigned int page);
+ ~CMtnFile();
+};
+
+#endif
diff --git a/src/mtn/mtn_file.l b/src/mtn/mtn_file.l
new file mode 100644
index 0000000000000000000000000000000000000000..3527fd851ed162497b797718e20b18c0eda2a229
--- /dev/null
+++ b/src/mtn/mtn_file.l
@@ -0,0 +1,37 @@
+%{
+/* Implementation of yyFlexScanner */
+#include "mtn_file_scanner.hpp"
+#include "mtn_parser.hpp"
+
+/* typedef to make the returns for the tokens shorter */
+typedef MTN::MTN_Parser::token token;
+
+%}
+
+%option debug
+%option nodefault
+%option yyclass="MTN_File_Scanner"
+%option noyywrap
+%option c++
+%option yylineno
+
+%%
+[ \t\n] ;
+"type" { return token::TYPE; }
+"version" { return token::VERSION; }
+"enable" { return token::ENABLE; }
+"motor_type" { return token::MOTOR_TYPE; }
+"page_begin" { return token::BEGIN_PAGE; }
+"name" { return token::NAME; }
+"compliance" { return token::COMPLIANCE; }
+"play_param" { return token::PLAY_PARAMS; }
+"step" { return token::STEP; }
+"page_end" { return token::END_PAGE; }
+"=" { return token::EQUAL; }
+[0-9]+"."[0-9]+ { yylval->fval=atof(yytext); return token::FLOAT; }
+[0-9]+ { yylval->ival=atoi(yytext); return token::INT; }
+[a-zA-Z0-9]+|_ { yylval->sval=new std::string(yytext);
+ return token::STRING; }
+. { std::cout << "bad input character " << yytext << " at line " << yylineno << std::endl; }
+%%
+
diff --git a/src/mtn/mtn_file.y b/src/mtn/mtn_file.y
new file mode 100644
index 0000000000000000000000000000000000000000..020d8f9be1016b1132758416580453ab8a0ad0bc
--- /dev/null
+++ b/src/mtn/mtn_file.y
@@ -0,0 +1,157 @@
+%skeleton "lalr1.cc"
+%require "2.5"
+%debug
+%defines
+%define namespace "MTN"
+%define parser_class_name "MTN_Parser"
+
+%code requires
+{
+ namespace MTN
+ {
+ class MTN_File_Parser;
+ class MTN_File_Scanner;
+ }
+}
+
+%lex-param { MTN_File_Scanner &scanner }
+%parse-param { MTN_File_Scanner &scanner }
+
+%lex-param { MTN_File_Parser &parser }
+%parse-param { MTN_File_Parser &parser }
+
+%code
+{
+ #include <iostream>
+ #include <cstdlib>
+ #include <fstream>
+ /* include for all driver functions */
+ #include "mtn_file_parser.hpp"
+
+ /* motion files variables */
+ int num_servos=0;
+ std::vector<bool> enables;
+ std::vector<int> motor_types;
+ CMtnPage *page;
+ std::vector<unsigned int> compliances;
+ std::string motion_name;
+ CMtnStep *step;
+ std::vector<unsigned short int> angles;
+
+ /* this is silly, but I can't figure out a way around */
+ static int yylex(MTN::MTN_Parser::semantic_type *yylval,MTN::MTN_File_Scanner &scanner,MTN::MTN_File_Parser &parser);
+}
+
+/* token types */
+%union {
+ int ival;
+ float fval;
+ std::string *sval;
+}
+
+%token TYPE VERSION ENABLE MOTOR_TYPE BEGIN_PAGE END_PAGE
+%token NAME COMPLIANCE PLAY_PARAMS STEP
+%token EQUAL
+%token <ival> INT
+%token <fval> FLOAT
+%token <sval> STRING
+
+/* destructor rule for <sval> objects */
+%destructor { if ($$) { delete ($$); ($$) = NULL; } } <sval>
+
+%%
+motion_file: type version enables motor_types pages
+ ;
+
+type: TYPE EQUAL STRING { parser.set_type(*($3)); }
+ ;
+
+version: VERSION EQUAL FLOAT { parser.set_version($3); }
+ ;
+
+enables: ENABLE EQUAL enable { parser.set_num_servos(num_servos);
+ parser.set_enables(enables);
+ std::cout << num_servos << std::endl;}
+ ;
+
+enable: enable INT { num_servos++;
+ enables.push_back($2); }
+ | INT { num_servos++;
+ enables.push_back($1); }
+ ;
+
+motor_types:
+ | MOTOR_TYPE EQUAL motor_type { parser.set_motor_types(motor_types); }
+ ;
+
+motor_type: motor_type INT { motor_types.push_back($2); }
+ | INT { motor_types.push_back($1); }
+ ;
+
+pages: pages page
+ | page
+ ;
+
+page: BEGIN_PAGE name compliances params steps END_PAGE { parser.add_page(page);
+ motion_name="";
+ compliances.clear(); }
+ | BEGIN_PAGE name compliances params END_PAGE { parser.add_page(page);
+ motion_name="";
+ compliances.clear(); }
+ ;
+
+name: NAME EQUAL { page=new CMtnPage(num_servos);
+ motion_name="";
+ page->set_name(motion_name); }
+ | NAME EQUAL strings { page=new CMtnPage(num_servos);
+ page->set_name(motion_name); }
+ ;
+
+strings: strings STRING { motion_name+=" "+*($2); }
+ | STRING { motion_name+=*($1); }
+ ;
+
+compliances: COMPLIANCE EQUAL compliance { page->set_compliances(compliances); }
+ ;
+
+compliance: compliance INT { compliances.push_back($2); }
+ | INT { compliances.push_back($1); }
+ ;
+
+params: PLAY_PARAMS EQUAL INT INT INT FLOAT INT { page->set_next_page($3);
+ page->set_exit_page($4);
+ page->set_repetitions($5);
+ page->set_speed_rate($6);
+ page->set_inertial($7); }
+ ;
+
+steps: steps step
+ | step
+ ;
+
+
+step: STEP EQUAL angles FLOAT FLOAT { step=new CMtnStep(num_servos);
+ step->set_angles(angles);
+ angles.clear();
+ step->set_pause_time($4);
+ step->set_step_time($5);
+ page->add_step(step); }
+ ;
+
+angles: angles INT { angles.push_back($2); }
+ | INT { angles.push_back($1); }
+ ;
+%%
+void
+MTN::MTN_Parser::error(const MTN::MTN_Parser::location_type &l,const std::string &err_message )
+{
+ std::cerr << "Error: " << err_message << "\n";
+}
+
+/* include for access to scanner.yylex */
+#include "mtn_file_scanner.hpp"
+
+static int yylex(MTN::MTN_Parser::semantic_type *yylval,MTN::MTN_File_Scanner &scanner,MTN::MTN_File_Parser &parser)
+{
+ return(scanner.yylex(yylval));
+}
diff --git a/src/mtn/mtn_file_parser.cpp b/src/mtn/mtn_file_parser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d3301996697be95a0a3a4fa78c70e478aa3bea4
--- /dev/null
+++ b/src/mtn/mtn_file_parser.cpp
@@ -0,0 +1,92 @@
+#include <cctype>
+#include <fstream>
+#include <cassert>
+
+#include "mtn_file_parser.hpp"
+#include "mtn_parser.hpp"
+
+MTN::MTN_File_Parser::MTN_File_Parser()
+{
+ this->scanner=NULL;
+ this->parser=NULL;
+}
+
+MTN::MTN_File_Parser::~MTN_File_Parser()
+{
+ if(this->scanner!=NULL)
+ {
+ delete this->scanner;
+ this->scanner=NULL;
+ }
+ if(this->parser!=NULL)
+ {
+ delete this->parser;
+ this->parser=NULL;
+ }
+}
+
+void MTN::MTN_File_Parser::parse(const char *filename)
+{
+ std::ifstream in_file(filename);
+ if(!in_file.good())
+ exit( EXIT_FAILURE );
+ if(this->scanner!=NULL)
+ delete this->scanner;
+ try
+ {
+ scanner=new MTN::MTN_File_Scanner(&in_file);
+ }catch(std::bad_alloc &ba){
+ std::cerr << "Failed to allocate scanner: (" << ba.what() << "), exiting!!\n";
+ exit( EXIT_FAILURE );
+ }
+ if(this->parser!=NULL)
+ delete this->parser;
+ try
+ {
+ parser = new MTN::MTN_Parser((*scanner),(*this));
+ }catch(std::bad_alloc &ba){
+ std::cerr << "Failed to allocate parser: (" << ba.what() << "), exiting!!\n";
+ exit( EXIT_FAILURE );
+ }
+
+ const int accept(0);
+ if(parser->parse()!=accept)
+ {
+ std::cerr << "Parse failed!!\n";
+ }
+}
+
+void MTN::MTN_File_Parser::set_type(std::string &type)
+{
+ this->motions.set_type(type);
+}
+
+void MTN::MTN_File_Parser::set_version(double version)
+{
+ this->motions.set_version(version);
+}
+
+void MTN::MTN_File_Parser::set_num_servos(unsigned int num_servos)
+{
+ this->motions.set_num_servos(num_servos);
+}
+
+void MTN::MTN_File_Parser::set_enables(std::vector<bool> &enables)
+{
+ this->motions.set_enables(enables);
+}
+
+void MTN::MTN_File_Parser::set_motor_types(std::vector<int> &motor_types)
+{
+ this->motions.set_motor_types(motor_types);
+}
+
+void MTN::MTN_File_Parser::add_page(CMtnPage *page)
+{
+ this->motions.add_page(page);
+}
+
+CMtnFile *MTN::MTN_File_Parser::get_motions(void)
+{
+ return &(this->motions);
+}
diff --git a/src/mtn/mtn_file_parser.hpp b/src/mtn/mtn_file_parser.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3c2df7dd43999a9ee502eb96a32299b9e355d465
--- /dev/null
+++ b/src/mtn/mtn_file_parser.hpp
@@ -0,0 +1,33 @@
+#ifndef _MTN_FILE_PARSER_HPP
+#define _MTN_FILE_PARSER_HPP
+
+#include <string>
+#include "mtn_file_scanner.hpp"
+#include "mtn_file.h"
+
+namespace MTN
+{
+ class MTN_File_Parser
+ {
+ friend class MTN_Parser;
+ private:
+ MTN::MTN_Parser *parser;
+ MTN::MTN_File_Scanner *scanner;
+ CMtnFile motions;
+ protected:
+ // motion files functions
+ void set_type(std::string &type);
+ void set_version(double version);
+ void set_num_servos(unsigned int num_servos);
+ void set_enables(std::vector<bool> &enables);
+ void set_motor_types(std::vector<int> &motor_types);
+ void add_page(CMtnPage *page);
+ public:
+ MTN_File_Parser();
+ void parse(const char *filename);
+ virtual ~MTN_File_Parser();
+ CMtnFile *get_motions(void);
+ };
+}
+
+#endif
diff --git a/src/mtn/mtn_file_scanner.hpp b/src/mtn/mtn_file_scanner.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb944ed2558184c57193a21dec6c4010607eca3c
--- /dev/null
+++ b/src/mtn/mtn_file_scanner.hpp
@@ -0,0 +1,35 @@
+#ifndef _MTN_FILE_SCANNER_HPP
+#define _MTN_FILE_SCANNER_HPP
+
+#if !defined(yyFlexLexerOnce)
+#include <FlexLexer.h>
+#endif
+
+#undef YY_DECL
+#define YY_DECL int MTN::MTN_File_Scanner::yylex()
+
+#include "mtn_parser.hpp"
+
+namespace MTN
+{
+ class MTN_File_Scanner : public yyFlexLexer
+ {
+ public:
+ MTN_File_Scanner(std::istream *in) : yyFlexLexer(in), yylval( NULL )
+ {
+ };
+ int yylex(MTN::MTN_Parser::semantic_type *lval)
+ {
+ yylval = lval;
+ return( yylex() );
+ }
+
+ private:
+ /* hide this one from public view */
+ int yylex();
+ /* yyval ptr */
+ MTN::MTN_Parser::semantic_type *yylval;
+ };
+} /* end namespace MC */
+
+#endif
diff --git a/src/mtn/mtn_page.cpp b/src/mtn/mtn_page.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ea95321bb365edc54209971ef7d2223573f7c830
--- /dev/null
+++ b/src/mtn/mtn_page.cpp
@@ -0,0 +1,178 @@
+#include "mtn_page.h"
+
+CMtnPage::CMtnPage(unsigned int num_servos)
+{
+ this->name="";
+ this->compliance.resize(num_servos);
+ this->next_page=-1;
+ this->exit_page=-1;
+ this->repetitions=0;
+ this->speed_rate=0;
+ this->inertial=-1;
+ this->steps.clear();
+}
+
+void CMtnPage::set_name(std::string &name)
+{
+ this->name=name;
+}
+
+void CMtnPage::set_compliance(unsigned int servo,unsigned int compliance)
+{
+ if(servo>this->compliance.size())
+ {
+ /* handle errors */
+ }
+ else
+ this->compliance[servo]=compliance;
+}
+
+void CMtnPage::set_compliances(std::vector<unsigned int> compliances)
+{
+ unsigned int i=0;
+
+ if(this->compliance.size()!=compliances.size())
+ {
+ /* handle errors */
+ }
+ else
+ {
+ for(i=0;i<this->compliance.size();i++)
+ this->compliance[i]=compliances[i];
+ }
+}
+
+void CMtnPage::set_next_page(unsigned int page)
+{
+ if(page>=256)
+ {
+ /* handle errors */
+ }
+ else
+ this->next_page=page;
+}
+
+void CMtnPage::set_exit_page(unsigned int page)
+{
+ if(page>=256)
+ {
+ /* handle errors */
+ }
+ else
+ this->exit_page=page;
+}
+
+void CMtnPage::set_repetitions(unsigned int num)
+{
+ if(num>=256)
+ {
+ /* handle errors */
+ }
+ else
+ this->repetitions=num;
+}
+
+void CMtnPage::set_speed_rate(double rate)
+{
+ this->speed_rate=rate;
+}
+
+void CMtnPage::set_inertial(unsigned int inertia)
+{
+ if(inertia>=256)
+ {
+ /* handle errors */
+ }
+ else
+ this->inertial=inertia;
+}
+
+void CMtnPage::add_step(CMtnStep *step)
+{
+ if(this->steps.size()>=7)
+ {
+ /* handle errors */
+ }
+ else
+ this->steps.push_back(step);
+}
+
+std::string CMtnPage::get_name(void)
+{
+ return this->name;
+}
+
+unsigned int CMtnPage::get_compliance(unsigned int servo)
+{
+ if(servo>this->compliance.size())
+ {
+ /* handle errors */
+ }
+ else
+ return this->compliance[servo];
+}
+
+std::vector<unsigned int> CMtnPage::get_compliances(void)
+{
+ return this->compliance;
+}
+
+unsigned int CMtnPage::get_next_page(void)
+{
+ return this->next_page;
+}
+
+unsigned int CMtnPage::get_exit_page(void)
+{
+ return this->exit_page;
+}
+
+unsigned int CMtnPage::get_repetitions(void)
+{
+ return this->repetitions;
+}
+
+double CMtnPage::get_speed_rate(void)
+{
+ return this->speed_rate;
+}
+
+unsigned int CMtnPage::get_inertial(void)
+{
+ return this->inertial;
+}
+
+unsigned int CMtnPage::get_num_steps(void)
+{
+ return this->steps.size();
+}
+
+CMtnStep *CMtnPage::get_step(unsigned int step)
+{
+ if(step>this->steps.size())
+ {
+ /* handle error */
+ }
+ else
+ return this->steps[step];
+}
+
+CMtnPage::~CMtnPage()
+{
+ unsigned int i=0;
+
+ this->name.clear();
+ this->compliance.clear();
+ this->next_page=-1;
+ this->exit_page=-1;
+ this->repetitions=0;
+ this->speed_rate=0;
+ this->inertial=-1;
+ for(i=0;i<this->steps.size();i++)
+ {
+ delete this->steps[i];
+ this->steps[i]=NULL;
+ }
+ this->steps.clear();
+}
+
diff --git a/src/mtn/mtn_page.h b/src/mtn/mtn_page.h
new file mode 100644
index 0000000000000000000000000000000000000000..22549e10671451bf8bbe8c4323d554962b4c45fc
--- /dev/null
+++ b/src/mtn/mtn_page.h
@@ -0,0 +1,42 @@
+#ifndef _MTN_PAGE_H
+#define _MTN_PAGE_H
+
+#include "mtn_step.h"
+#include <string>
+
+class CMtnPage
+{
+ private:
+ std::string name;
+ std::vector<unsigned int> compliance;
+ unsigned int next_page;
+ unsigned int exit_page;
+ unsigned int repetitions;
+ double speed_rate;
+ unsigned int inertial;
+ std::vector<CMtnStep *> steps;
+ public:
+ CMtnPage(unsigned int num_servos);
+ void set_name(std::string &name);
+ void set_compliance(unsigned int servo,unsigned int compliance);
+ void set_compliances(std::vector<unsigned int> compliances);
+ void set_next_page(unsigned int page);
+ void set_exit_page(unsigned int page);
+ void set_repetitions(unsigned int num);
+ void set_speed_rate(double rate);
+ void set_inertial(unsigned int inertia);
+ void add_step(CMtnStep *step);
+ std::string get_name(void);
+ unsigned int get_compliance(unsigned int servo);
+ std::vector<unsigned int> get_compliances(void);
+ unsigned int get_next_page(void);
+ unsigned int get_exit_page(void);
+ unsigned int get_repetitions(void);
+ double get_speed_rate(void);
+ unsigned int get_inertial(void);
+ unsigned int get_num_steps(void);
+ CMtnStep *get_step(unsigned int step);
+ ~CMtnPage();
+};
+
+#endif
diff --git a/src/mtn/mtn_step.cpp b/src/mtn/mtn_step.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..377cf35835a0af30611320a60ccd936c3904df3b
--- /dev/null
+++ b/src/mtn/mtn_step.cpp
@@ -0,0 +1,86 @@
+#include "mtn_step.h"
+
+CMtnStep::CMtnStep(unsigned int num_servos)
+{
+ this->angles.resize(num_servos);
+ this->pause_time=0.0;
+ this->step_time=0.0;
+}
+
+void CMtnStep::set_angle(unsigned int servo,unsigned short int angle)
+{
+ if(servo>this->angles.size())
+ {
+ /* handle errors */
+ }
+ else
+ this->angles[servo]=angle;
+}
+
+void CMtnStep::set_angles(std::vector<unsigned short int> &angles)
+{
+ unsigned int i=0;
+
+ if(angles.size()!=this->angles.size())
+ {
+ /* handle errors */
+ }
+ else
+ {
+ for(i=0;i<angles.size();i++)
+ this->angles[i]=angles[i];
+ }
+}
+
+void CMtnStep::set_pause_time(double time)
+{
+ if(time<0.0)
+ {
+ /* handle errors */
+ }
+ else
+ this->pause_time=time;
+}
+
+void CMtnStep::set_step_time(double time)
+{
+ if(time<0.0)
+ {
+ /* handle errors */
+ }
+ else
+ this->step_time=time;
+}
+
+unsigned short int CMtnStep::get_angle(unsigned int servo)
+{
+ if(servo>this->angles.size())
+ {
+ /* handler errors */
+ }
+ else
+ return this->angles[servo];
+}
+
+std::vector<unsigned short int> CMtnStep::get_angles(void)
+{
+ return this->angles;
+}
+
+double CMtnStep::get_pause_time(void)
+{
+ return this->pause_time;
+}
+
+double CMtnStep::get_step_time(void)
+{
+ return this->step_time;
+}
+
+CMtnStep::~CMtnStep()
+{
+ this->angles.clear();
+ this->pause_time=0.0;
+ this->step_time=0.0;
+}
+
diff --git a/src/mtn/mtn_step.h b/src/mtn/mtn_step.h
new file mode 100644
index 0000000000000000000000000000000000000000..f85b4f17977e572f11441ebd74068ed6f680b6f8
--- /dev/null
+++ b/src/mtn/mtn_step.h
@@ -0,0 +1,25 @@
+#ifndef _MTN_STEP_H
+#define _MTN_STEP_H
+
+#include <vector>
+
+class CMtnStep
+{
+ private:
+ std::vector<unsigned short int> angles;
+ double pause_time;
+ double step_time;
+ public:
+ CMtnStep(unsigned int num_servos);
+ void set_angle(unsigned int servo,unsigned short int angle);
+ void set_angles(std::vector<unsigned short int> &angles);
+ void set_pause_time(double time);
+ void set_step_time(double time);
+ unsigned short int get_angle(unsigned int servo);
+ std::vector<unsigned short int> get_angles(void);
+ double get_pause_time(void);
+ double get_step_time(void);
+ ~CMtnStep();
+};
+
+#endif