[Jde-dev] Parche driver naobody
Eduardo Perdices
edupergar en gmail.com
Mar Mayo 5 19:55:55 CEST 2009
Hola,
hemos estado juntando francisco y yo el código del svn con las
funcionalidades que él había añadido, aquí va el parche para los 5
archivos que usa el driver, ahora se utiliza ptmotors para mover el
cuello en todas direcciones, ptencoders para leer la posición del
cuello y MotionMotors para mover la posición del robot.
Saludos.
------------ próxima parte ------------
Se ha borrado un texto insertado con un juego de caracteres sin especificar...
Nombre: pathadapter.cpp.txt
Url: http://gsyc.escet.urjc.es/pipermail/jde-developers/attachments/20090505/6a1418c9/attachment-0001.txt
------------ próxima parte ------------
7a8
> #include "almotionproxy.h"
9c10,15
< #define PI 3.1415926
---
> const float PI=3.1415926;
> const float MAXY=60;
> const float MAXP=44;
> const float MAXVY=100;
> const float MAXV=100;
> const float CHANGE_RANGE=1;
45,46c51
< class Head{
<
---
> class motion{
48d52
<
51,54c55,59
< AL::ALProxy * headProxy;
< float lastValue;
<
< public:
---
> AL::ALMotionProxy * motionProxy;
> std::string name;
> float lastp;
> float lasty;
> int mysteps;
56c61,63
< Head();
---
> public:
>
> motion();
59c66,67
< void moveTo(float angle);
---
> int walk(float v, float w);
> int head(float y, float p,float *posy, float *posp,float vy, float vp, unsigned long int* clock);
73,78c81,90
< /*Head functions in C*/
< extern "C" Head* newHead();
< extern "C" int initHead(Head* h);
< extern "C" void terminateHead(Head* h);
< extern "C" void deleteHead(Head* h);
< extern "C" void moveToHead(Head* h, float angle);
---
> /*Motion functions in C*/
>
> extern "C" motion* newmotion();
> extern "C" int initmotion(motion* m);
> extern "C" void terminatemotion(motion* m);
> extern "C" int walkmotion(motion* m, float v, float w);
> extern "C" int headmotion(motion* m, float y, float p, float *posy, float *posp, float vy, float vp,unsigned long int* clock);
> extern "C" void deletemotion(motion* m);
> extern "C" int get_motionclock(motion* m);
>
------------ próxima parte ------------
39,40c39,40
< /** Head */
< Head * head = NULL;
---
> /** Motion */
> motion* m;
71,74c71,102
< /*Head parameters*/
< int serve_head;
< int head_active;
< int head_schema_id;
---
> /*Motion parameters*/
> /*pantiltencoers*/
> int serve_pantiltencoders;
> int pantiltencoders_active;
>
> /*pantiltmotors*/
> int serve_pantiltmotors;
> int pantiltmotors_active;
>
> int serve_motion;
> int motion_active;
> int motion_schema_id;
> int pantiltencoders_schema_id;
> int pantiltmotors_schema_id;
>
> /*API variables servidas*/
> float vnao;
> float wnao;
> float pnao;
> float ynao;
> float preal;
> float yreal;
> float vy;
> float vp;
> unsigned long int my_clock;
> float cycle;
> float max_longitude;
> float max_latitude;
> float min_longitude;
> float min_latitude;
> float max_y_speed;
> float max_p_speed;
78c106
< float headVal;
---
> //float headVal;
117,130c145,168
< /** head run function following jdec platform API schemas.
< * @param father Father id for this schema.
< * @param brothers Brothers for this schema.
< * @param fn arbitration function for this schema.
< * @return integer resuming result.*/
< int head_run(int father, int *brothers, arbitration fn){
< if((serve_head==1)&&(head_active==0)){
< head_active=1;
< printf("HeadMotors schema run (naobody driver)\n");
< all[head_schema_id].father = father;
< all[head_schema_id].fps = 0.0;
< all[head_schema_id].k =0;
< put_state(head_schema_id,winner);
< /* naobody thread goes winner */
---
> int motion_run(int father, int *brothers, arbitration fn){
> if((serve_motion==1)&&(motion_active==0)){
> motion_active=1;
> printf("MotionMotors schema run (naobody driver)\n");
> all[motion_schema_id].father = father;
> all[motion_schema_id].fps = 0.0;
> all[motion_schema_id].k =0;
> put_state(motion_schema_id,winner);
> /* naoqi thread goes winner */
> pthread_mutex_lock(&mymutex);
> state=winner;
> pthread_cond_signal(&condition);
> pthread_mutex_unlock(&mymutex);
> }
> return 0;
> }
>
>
> int motion_stop(){
> if((serve_motion==1)&&(motion_active==1)){
> motion_active=0;
> printf("MotionMotors schema stop (naobody driver)\n");
> put_state(motion_schema_id,slept);
> /* naoqi thread goes sleep */
132,133c170
< state=winner;
< pthread_cond_signal(&condition);
---
> state=slept;
139,146c176,230
< /** head stop function following jdec platform API schemas.
< * @return integer stopping result.*/
< int head_stop(){
< if((serve_head==1)&&(head_active==1)){
< head_active=0;
< printf("HeadMotors schema stop (naobody driver)\n");
< put_state(head_schema_id,slept);
< /* naobody thread goes sleep */
---
>
> int pantiltencoders_run(int father, int *brothers, arbitration fn){
> if((serve_pantiltencoders==1)&&(pantiltencoders_active==0)){
> pantiltencoders_active=1;
> printf("pantiltencoders schema run (naobody driver)\n");
> all[pantiltencoders_schema_id].father = father;
> all[pantiltencoders_schema_id].fps = 0.0;
> all[pantiltencoders_schema_id].k =0;
> put_state(pantiltencoders_schema_id,winner);
> /* naoqi thread goes winner */
> pthread_mutex_lock(&mymutex);
> state=winner;
> pthread_cond_signal(&condition);
> pthread_mutex_unlock(&mymutex);
> }
> return 0;
> }
>
> int pantiltencoders_stop(){
> if((serve_pantiltencoders==1)&&(pantiltencoders_active==1)){
> pantiltencoders_active=0;
> printf("pantiltencoders schema stop (naobody driver)\n");
> put_state(pantiltencoders_schema_id,slept);
> /* naoqi thread goes sleep */
> pthread_mutex_lock(&mymutex);
> state=slept;
> pthread_mutex_unlock(&mymutex);
> }
> return 0;
> }
>
>
> int pantiltmotors_run(int father, int *brothers, arbitration fn){
> if((serve_pantiltmotors==1)&&(pantiltmotors_active==0)){
> pantiltmotors_active=1;
> printf("pantiltmotors schema run (naobody driver)\n");
> all[pantiltmotors_schema_id].father = father;
> all[pantiltmotors_schema_id].fps = 0.0;
> all[pantiltmotors_schema_id].k =0;
> put_state(pantiltmotors_schema_id,winner);
> /* naoqi thread goes winner */
> pthread_mutex_lock(&mymutex);
> state=winner;
> pthread_cond_signal(&condition);
> pthread_mutex_unlock(&mymutex);
> }
> return 0;
> }
>
> int pantiltmotors_stop(){
> if((serve_pantiltmotors==1)&&(pantiltmotors_active==1)){
> pantiltmotors_active=0;
> printf("pantiltmotors schema stop (naobody driver)\n");
> put_state(pantiltmotors_schema_id,slept);
> /* naoqi thread goes sleep */
195,199c279,289
< /* Move head*/
< if(head_active) {
< speedcounter(head_schema_id);
< /*Actualizamos la posicion de la cabeza del robot*/
< moveToHead(head, headVal);
---
> /* Move body*/
> if(motion_active) {
> speedcounter(motion_schema_id);
> walkmotion(m,vnao,wnao);
> }
> if ((pantiltencoders_active)||(pantiltmotors_active)){
> if (pantiltencoders_active)
> speedcounter(pantiltencoders_schema_id);
> if (pantiltmotors_active)
> speedcounter(pantiltmotors_schema_id);
> headmotion(m,ynao,pnao,&yreal,&preal,vy,vp,&my_clock);
357,360c447,457
< } else if (words==2) {
< if(strcmp(word4,"head")==0){
< serve_head=1;
< } else
---
> } else if (words==2){
> if(strcmp(word4,"body")==0){
> serve_motion=1;
> }
> else if (strcmp(word4,"ptencoders")==0){
> serve_pantiltencoders=1;
> }
> else if (strcmp(word4,"ptmotors")==0){
> serve_pantiltmotors=1;
> }
> else
390c487
< if(serve_color==0 && serve_head==0)
---
> if(serve_color==0 /*&& serve_head==0*/)
399,402d495
< if(serve_head) {
< //Liberamos la cabeza motora
< deleteHead(head);
< }
404d496
< //Liberamos la camara de naobody
406a499,501
> if((serve_motion)||(serve_pantiltencoders)||serve_pantiltmotors) {
> deletemotion(m);
> }
427,431c522,525
< if(serve_head) {
< printf("inicializamos el head\n");
< head = newHead();
< if(head == NULL) {
< fprintf(stderr, "Cannot create head motors\n");
---
> if((serve_motion)||(serve_pantiltencoders)||(serve_pantiltmotors)) {
> m = newmotion();
> if(m == NULL) {
> fprintf(stderr, "Cannot create motion motors\n");
443,445d536
< width = DEFAULT_COLUMNS;
< height = DEFAULT_ROWS;
< fps = DEFAULT_FPS;
449,450c540,563
< serve_head=0;
< head_active=0;
---
> serve_motion=0;
> motion_active=0;
> serve_pantiltencoders=0;
> pantiltencoders_active=0;
> serve_pantiltmotors=0;
> pantiltmotors_active=0;
>
> width = DEFAULT_COLUMNS;
> height = DEFAULT_ROWS;
> fps = DEFAULT_FPS;
> pnao=0;
> ynao=0;
> preal=0;
> yreal=0;
> vp=0;
> vy=0;
> my_clock=0;
> cycle=0;
> max_longitude=MAXY;
> max_latitude=MAXP;
> min_longitude=-MAXY;
> min_latitude=-MAXP;
> max_y_speed=MAXVY;
> max_p_speed=MAXVY;
466c579
< if (serve_color || serve_head){
---
> if (serve_color || (serve_motion)||(serve_pantiltencoders)||(serve_pantiltmotors)){
498,503c611,616
< /*Creates a new schema for head motors*/
< if(serve_head==1) {
< all[num_schemas].id = (int *) &head_schema_id;
< strcpy(all[num_schemas].name,"HeadMotors");
< all[num_schemas].run = (runFn) head_run;
< all[num_schemas].stop = (stopFn) head_stop;
---
> /*Creates a new schema for motion*/
> if(serve_motion==1) {
> all[num_schemas].id = (int *) &motion_schema_id;
> strcpy(all[num_schemas].name,"MotionMotors");
> all[num_schemas].run = (runFn) motion_run;
> all[num_schemas].stop = (stopFn) motion_stop;
512,517c625,679
<
< headVal = 0.0;
< myexport("HeadMotors","value",&headVal);
< myexport("HeadMotors","id",&head_schema_id);
< myexport("HeadMotors","run",(void *)head_run);
< myexport("HeadMotors","stop",(void *)head_stop);
---
> vnao = 0.0;
> wnao= 0.0;
> myexport("MotionMotors","v",&vnao);
> myexport("MotionMotors","w",&wnao);
> myexport("MotionMotors","id",&motion_schema_id);
> myexport("MotionMotors","run",(void *)motion_run);
> myexport("MotionMotors","stop",(void *)motion_stop);
> }
> if (serve_pantiltencoders==1){
> all[num_schemas].id = (int *) &pantiltencoders_schema_id;
> strcpy(all[num_schemas].name,"ptencoders");
> all[num_schemas].run = (runFn) pantiltencoders_run;
> all[num_schemas].stop = (stopFn) pantiltencoders_stop;
> printf("%s schema loaded (id %d)\n",all[num_schemas].name,num_schemas);
> (*(all[num_schemas].id)) = num_schemas;
> all[num_schemas].fps = 0.;
> all[num_schemas].k =0;
> all[num_schemas].state=slept;
> all[num_schemas].terminate = NULL;
> all[num_schemas].handle = NULL;
> num_schemas++;
> myexport("ptencoders","pan_angle",&yreal);
> myexport("ptencoders","tilt_angle",&preal);
> myexport("ptencoders","clock",&my_clock);
> myexport("ptencoders","id",&pantiltencoders_schema_id);
> myexport("ptencoders","run",(void *)pantiltencoders_run);
> myexport("ptencoders","stop",(void *)pantiltencoders_stop);
> }
> if (serve_pantiltmotors==1){
> all[num_schemas].id = (int *) &pantiltmotors_schema_id;
> strcpy(all[num_schemas].name,"ptmotors");
> all[num_schemas].run = (runFn) pantiltmotors_run;
> all[num_schemas].stop = (stopFn) pantiltmotors_stop;
> printf("%s schema loaded (id %d)\n",all[num_schemas].name,num_schemas);
> (*(all[num_schemas].id)) = num_schemas;
> all[num_schemas].fps = 0.;
> all[num_schemas].k =0;
> all[num_schemas].state=slept;
> all[num_schemas].terminate = NULL;
> all[num_schemas].handle = NULL;
> num_schemas++;
> myexport("ptmotors","cycle",&cycle);
> myexport("ptmotors","longitude",&ynao);
> myexport("ptmotors","latitude",&pnao);
> myexport("ptmotors","max_longitude",&max_longitude);
> myexport("ptmotors","max_latitude",&max_latitude);
> myexport("ptmotors","min_longitude",&min_longitude);
> myexport("ptmotors","min_latitude",&min_latitude);
> myexport("ptmotors","longitude_speed",&vy);
> myexport("ptmotors","latitude_speed",&vp);
> myexport("ptmotors","max_longitud_speed",&max_y_speed);
> myexport("ptmotors","max_latitud_speed",&max_p_speed);
> myexport("ptmotors","id",&pantiltmotors_schema_id);
> myexport("ptmotors","run",(void *)pantiltmotors_run);
> myexport("ptmotors","stop",(void *)pantiltmotors_stop);
------------ próxima parte ------------
0a1,20
> /*
> * Copyright (C) 1997-2009 JDE Developers Team
> *
> * This program is free software: you can redistribute it and/or modify
> * it under the terms of the GNU General Public License as published by
> * the Free Software Foundation, either version 3 of the License, or
> * (at your option) any later version.
> *
> * This program is distributed in the hope that it will be useful,
> * but WITHOUT ANY WARRANTY; without even the implied warranty of
> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
> * GNU General Public License for more details.
> *
> * You should have received a copy of the GNU General Public License
> * along with this program. If not, see http://www.gnu.org/licenses/.
> *
> * Authors : Eduardo Perdices <edupergar en gmail.com>
> * Francisco Rivas <fm.rivas en alumnos.urjc.es>
> */
>
------------ próxima parte ------------
3c3,5
< provides head
---
> provides ptmotors
> provides ptencoders
> provides MotionMotors
More information about the Jde-developers
mailing list