00001 #include <korebot/korebot.h>
00002 #include <signal.h>
00003 #include <sys/time.h>
00004
00005 #define MOT_SetPointLL 0x2F
00006
00007 knet_dev_t * MotorOpen(int motor)
00008 {
00009 switch(motor)
00010 {
00011 case 1 :
00012 return knet_open( "Khepera3:Mot1", KNET_BUS_ANY, 0 , NULL );
00013 break;
00014 case 2 :
00015 return knet_open( "Khepera3:Mot2", KNET_BUS_ANY, 0 , NULL );
00016 break;
00017 default:
00018 return NULL;
00019 break;
00020 }
00021 }
00022
00023 #define timercpy(a,b) \
00024 (a)->tv_sec = (b)->tv_sec; \
00025 (a)->tv_usec = (b)->tv_usec;
00026
00027 int main(int argc, char *argv[]) {
00028
00029 int ver,rc;
00030 int32_t position,speed,current;
00031 knet_dev_t * motor;
00032 unsigned char val,status,erreur;
00033
00034 struct timeval clock, clocksav, clockdif;
00035 struct timezone timez;
00036 char result;
00037
00038 if(argc < 2)
00039 {
00040 printf("motor number needed\n\r");
00041 exit(0);
00042 }
00043
00044
00045 kb_set_debug_level(2);
00046
00047 if((rc = kb_init( argc , argv )) < 0 )
00048 return 1;
00049
00050 printf("K-Team Monitor Program\r\n");
00051
00052 motor = MotorOpen(atoi(argv[1]));
00053 if(!motor)
00054 {
00055 printf("Cannot open motor %d\r\n",atoi(argv[1]));
00056 return 1;
00057 }
00058
00059
00060 kmot_GetFWVersion( motor, &ver );
00061 printf("Motor Firmware v%u.%u\n" , KMOT_VERSION(ver) , KMOT_REVISION(ver));
00062
00063
00064
00065
00066
00067 kmot_SetOptions(motor,0,kMotSWOptWindup|kMotSWOptStopMotorBlk);
00068
00069 gettimeofday(&clocksav,&timez);
00070
00071 while(1)
00072 {
00073 do{
00074 gettimeofday(&clock,&timez);
00075 timersub(&clock,&clocksav,&clockdif);
00076 }
00077
00078 while(clockdif.tv_usec < 5000);
00079
00080 timercpy(&clocksav,&clock);
00081
00082
00083 speed = kmot_GetMeasure(motor, kMotMesSpeed);
00084 printf("spd: %7ld ",speed);
00085 position = kmot_GetMeasure(motor, kMotMesPos);
00086 printf("pos: %7ld ",position);
00087 current = kmot_GetMeasure(motor, kMotMesCurrent);
00088 printf("cur: %7ld ",current);
00089
00090 kmot_GetStatus(motor,&status,&erreur);
00091 printf("err: 0x%x stat: 0x%x\r\n",erreur,status);
00092
00093
00094
00095 }
00096 }