4a5
> 
192c193
< 										Head
---
> 										Motion
195,196c196
< 	/*Funciones de la clase head en C++*/
< 	Head::Head() {
---
> 	motion::motion() {
199c199,200
< 		this->lastValue = 1000;
---
> 		this->lasty=0;
> 		this->lastp=0;
202,210c203,213
< 	int Head::init() {
< 		try {
< 			/*Get proxy*/
< 			this->headProxy = new AL::ALProxy("ALMotion", IP, PORT);
< 			//this->headProxy->callVoid("gotoAngleWithSpeed","HeadYaw",1, 50, 0);
< 		} catch(AL::ALError& e) {
< 			std::cerr << "Excepción al conectar con naoqi: "<<e.toString()<< std::endl;
< 			return -1;
< 		}
---
> 
> 	int motion::init() {
> 
> 			try {
> 				this->motionProxy = new AL::ALMotionProxy(IP, PORT);
> 				//this->motionProxy = new AL::ALProxy("ALMotion", IP, PORT);
> 			} catch(AL::ALError& e) {
> 				std::cerr << "NaoBody: exception connecting to NaoQi: "<<e.toString()<< std::endl;
> 				return -1;
> 			}
> 			this->mysteps=0;
212,213c215,217
< 		return 0;
< 	}	
---
> 			return 0;	
> 	}
> 
215,216c219,220
< 	void Head::terminate() {
< 		/*Void*/
---
> 	void motion::terminate() {
> 	
219,220d222
< 	void Head::moveTo(float angle) {
< 		float value;
222,224c224,226
< 		/*No actualizamos si ya tenia ese valor*/
< 		if(angle == this->lastValue)
< 			return;
---
> 	int motion::walk(float v, float w) {
> 		float myv;
> 		int steps;
226,233c228,260
< 		this->lastValue = angle;
< 		
< 		value = angle*PI/180.0;
< 		/*Check limits*/
< 		if(value > 1.0) 
< 			value = 1.0;
< 		if(value < -1.0)
< 			value = -1.0;
---
> 		myv=-0.83*v+100;
> 	
> 		if ((w!=0)||(v!=0)){
> 				if ((v>0)&&(v<MAXV)){
> 					steps=this->motionProxy->getRemainingFootStepCount();
> 					if (steps!=this->mysteps){
> 						mysteps=steps;
> 						if (mysteps < 2){
> 							printf("aqui\n");
> 							if (w==0){
> 									this->motionProxy->post.walkStraight(0.08,myv);
> 							}
> 							else{
> 								this->motionProxy->post.walkArc(w,0.08, myv);
> 							}
> 						}
> 					}
> 					else if (steps==0){
> 						printf("empezamos\n");
> 						if (w==0){
> 									this->motionProxy->post.walkStraight(0.08,myv);
> 							}
> 							else{
> 								this->motionProxy->post.walkArc(w,0.08, myv);
> 							}
> 					}
> 				}
> 			/*else{
> 				printf("---%d\n",this->motionProxy->getRemainingFootStepCount());
> 			}*/
> 		}
> 		return 0;
> 	}
235,239c262,291
< 		try {
< 			this->headProxy->callVoid("gotoAngleWithSpeed","HeadYaw",value, 50, 0);		////#########Velocidad puede ser 100
< 		} catch(AL::ALError& e) {
< 			std::cerr << "Excepción al mover la cabeza: "<<e.toString()<< std::endl;
< 			return;
---
> 
> 	int motion::head(float y, float p, float *posy, float *posp, float vy, float vp, unsigned long int* clock){
> 		float ay;
> 		float ap;
> 		int clock_aux;
> 		float y_aux_real;
> 		float p_aux_real;
> 
> 	
> 		ay=this->motionProxy->getAngle("HeadYaw") * 180 /PI;
> 		ap=this->motionProxy->getAngle ("HeadPitch")* 180 /PI;
> 		
> 		y_aux_real=truncf(ay*10);
> 		p_aux_real=truncf(ap*10);
> 		if ((y_aux_real!=truncf(y*10))&&(y>(-MAXY))&&(y<=MAXY)&&(vy<=MAXV)){
> 			this->motionProxy->post.gotoAngleWithSpeed ("HeadYaw",-y*PI/180, vy, 0);
> 			//this->motionProxy->post.gotoAngleWithSpeed ("HeadYaw",-y*PI/180, 10, 0);
> 		}
> 		if ((p_aux_real!=truncf(p*10))&&(p>(-MAXY))&&(y<=MAXY)&&(vp<=MAXV)){
> 			this->motionProxy->post.gotoAngleWithSpeed ("HeadPitch",-p*PI/180, vp, 0);
> 			//this->motionProxy->post.gotoAngleWithSpeed ("HeadPitch",-p*PI/180, 10, 0);
> 		}
> 		if ((y_aux_real>this->lasty+CHANGE_RANGE)||(y_aux_real<this->lasty-CHANGE_RANGE)||(p_aux_real>this->lastp+CHANGE_RANGE)||(p_aux_real<this->lastp-CHANGE_RANGE)){
> 			*posy=-ay;
> 			*posp=-ap;
> 			clock_aux=*clock;
> 			clock_aux++;
> 			*clock=clock_aux;
> 			this->lasty=truncf(ay*10);
> 			this->lastp=truncf(ap*10);
240a293,294
> 	
> 		return 0;
243,247c297,301
< 	/*Funciones exportadas en C*/
< 	Head* newHead() {
< 		Head * h = new Head();
< 		if(initHead(h) < 0) {
< 			delete h;
---
> 
> 	motion* newmotion() {
> 		motion *m = new motion();
> 		if(initmotion(m) < 0) {
> 			delete m;
250c304
< 		return h;
---
> 		return m;
253,254c307,309
< 	int initHead(Head* h) {
< 		return h->init();
---
> 	int initmotion(motion* m) {
> 
> 		return m->init();
257,259c312,314
< 	void terminateHead(Head* h) {
< 		h->terminate();
< 	}	
---
> 	void terminatemotion(motion* m) {
> 		m->terminate();
> 	}
261,263c316,317
< 	void deleteHead(Head* h) {
< 		terminateHead(h);
< 		delete h;
---
> 	int walkmotion(motion* m, float v, float w){
> 		return m->walk(v,w); 
266,268c320,327
< 	void moveToHead(Head* h, float angle) {
< 		h->moveTo(angle);
< 	}	
---
> 	int headmotion(motion* m, float y, float p, float *posy, float *posp, float vy, float vp, unsigned long int* clock){
> 		return m->head(y,p,posy,posp,vy,vp,clock); 
> 	}
> 
> 	void deletemotion(motion* m) {
> 		terminatemotion(m);
> 		delete m;
> 	}
270d328
<