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 * AltMotorOpen(int motor)
00008 {
00009 switch(motor)
00010 {
00011 case 1 :
00012 return knet_open( "KoreMotorLE:AltMotor1", KNET_BUS_ANY, 0 , NULL );
00013 break;
00014 case 2 :
00015 return knet_open( "KoreMotorLE:AltMotor2", KNET_BUS_ANY, 0 , NULL );
00016 break;
00017 case 3 :
00018 return knet_open( "KoreMotorLE:AltMotor3", KNET_BUS_ANY, 0 , NULL );
00019 break;
00020 case 4 :
00021 return knet_open( "KoreMotorLE:AltMotor4", KNET_BUS_ANY, 0 , NULL );
00022 break;
00023 default:
00024 return NULL;
00025 break;
00026 }
00027 }
00028 knet_dev_t * MotorOpen(int motor)
00029 {
00030 switch(motor)
00031 {
00032 case 1 :
00033 return knet_open( "KoreMotorLE:PriMotor1", KNET_BUS_ANY, 0 , NULL );
00034 break;
00035 case 2 :
00036 return knet_open( "KoreMotorLE:PriMotor2", KNET_BUS_ANY, 0 , NULL );
00037 break;
00038 case 3 :
00039 return knet_open( "KoreMotorLE:PriMotor3", KNET_BUS_ANY, 0 , NULL );
00040 break;
00041 case 4 :
00042 return knet_open( "KoreMotorLE:PriMotor4", KNET_BUS_ANY, 0 , NULL );
00043 break;
00044 default:
00045 return NULL;
00046 break;
00047 }
00048 }
00049
00050 #define timercpy(a,b) \
00051 (a)->tv_sec = (b)->tv_sec; \
00052 (a)->tv_usec = (b)->tv_usec;
00053
00054 int main(int argc, char *argv[]) {
00055
00056 int ver,rc;
00057 int32_t position,speed,current;
00058 knet_dev_t * motor;
00059 unsigned char val,status,erreur;
00060
00061 struct timeval clock, clocksav, clockdif;
00062 struct timezone timez;
00063 char result;
00064
00065 if(argc < 2)
00066 {
00067 printf("motor number needed\n\r");
00068 exit(0);
00069 }
00070
00071
00072 kb_set_debug_level(2);
00073
00074 if((rc = kb_init( argc , argv )) < 0 )
00075 return 1;
00076
00077 printf("K-Team Monitor Program\r\n");
00078
00079 motor = MotorOpen(atoi(argv[1]));
00080 if(!motor)
00081 {
00082 printf("Cannot open motor %d\r\n",atoi(argv[1]));
00083 return 1;
00084 }
00085
00086
00087 kmot_GetFWVersion( motor, &ver );
00088 printf("Motor Firmware v%u.%u\n" , KMOT_VERSION(ver) , KMOT_REVISION(ver));
00089
00090
00091
00092
00093
00094 kmot_SetOptions(motor,0,kMotSWOptWindup|kMotSWOptStopMotorBlk);
00095
00096 gettimeofday(&clocksav,&timez);
00097
00098 while(1)
00099 {
00100 do{
00101 gettimeofday(&clock,&timez);
00102 timersub(&clock,&clocksav,&clockdif);
00103 }
00104
00105 while(clockdif.tv_usec < 5000);
00106
00107 timercpy(&clocksav,&clock);
00108
00109
00110 speed = kmot_GetMeasure(motor, kMotMesSpeed);
00111 printf("spd: %7ld ",speed);
00112 position = kmot_GetMeasure(motor, kMotMesPos);
00113 printf("pos: %7ld ",position);
00114 current = kmot_GetMeasure(motor, kMotMesCurrent);
00115 printf("cur: %7ld ",current);
00116
00117 kmot_GetStatus(motor,&status,&erreur);
00118 printf("err: 0x%x stat: 0x%x\r\n",erreur,status);
00119
00120
00121
00122 }
00123 }