00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00023 #include <signal.h>
00024 #include <korebot/korebot.h>
00025
00026 #define MARGIN 20
00027 #define TGT_MARGIN 100
00028
00029 void InitMotor(knet_dev_t * mot)
00030 {
00031 kmot_SetMode(mot,0);
00032 kmot_SetSampleTime(mot,1550);
00033 kmot_SetMargin(mot,20);
00034 kmot_SetOptions(mot,0,kMotSWOptWindup|kMotSWOptStopMotorBlk);
00035 kmot_ResetError(mot);
00036
00037 kmot_ConfigurePID(mot,kMotRegSpeed,1500,0,300);
00038 kmot_ConfigurePID(mot,kMotRegPos,100,30,3);
00039 kmot_SetSpeedProfile(mot,30,10);
00040
00041 kmot_SetBlockedTime(mot,5);
00042 kmot_SetLimits(mot,kMotRegCurrent,0,50);
00043 kmot_SetLimits(mot,kMotRegPos,-10000,10000);
00044 }
00045
00046 knet_dev_t * PantiltOpen(int motor)
00047 {
00048 switch(motor)
00049 {
00050 case 1 :
00051 return knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL );
00052 break;
00053 case 2 :
00054 return knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL );
00055 break;
00056 case 3 :
00057 return knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL );
00058 break;
00059 case 4 :
00060 return knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL );
00061 break;
00062 default:
00063 return NULL;
00064 break;
00065 }
00066 }
00067
00068 int main( int argc , char * argv[] )
00069 {
00070 unsigned int ver;
00071 int rc;
00072 char * name;
00073 int32_t position,speed,current;
00074 knet_dev_t *motor0, *motor1;
00075 int32_t minpos0,maxpos0,minpos1,maxpos1;
00076 int32_t tgtmin0,tgtmax0,tgtmax1,tgtmin1;
00077 unsigned char status0,erreur0,status1,erreur1;
00078 int32_t pos0,pos1;
00079 unsigned counter = 10, addr0, addr1;
00080
00082 if(argc < 3)
00083 {
00084
00085 printf("Usage: pantilt motor0 motor1 [nb cycle]\r\n");
00086
00087 printf("\tmotor number is 1,2,3 or 4\r\n");
00088 printf("\tnb cycle is 10 by default\r\n");
00089 return 0;
00090 }
00091
00092 if(argc > 3)
00093 counter = atoi(argv[3]);
00094 else
00095 counter = 10;
00096
00097
00098 kb_set_debug_level(2);
00099
00100 if((rc = kb_init( argc , argv )) < 0 )
00101 return 1;
00102
00103 printf("K-Team Pantilt Test Program\r\n");
00104
00105
00106 motor0 = PantiltOpen(atoi(argv[1]));
00107 if(!motor0)
00108 {
00109 printf("Cannot open motor %d\r\n",atoi(argv[1]));
00110 return 1;
00111 }
00112
00113 motor1 = PantiltOpen(atoi(argv[2]));
00114 if(!motor1)
00115 {
00116 printf("Cannot open motor %d\r\n",atoi(argv[2]));
00117 return 1;
00118 }
00119
00120
00121 kmot_GetFWVersion( motor1, &ver );
00122 printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(ver) , KMOT_REVISION(ver));
00123
00124
00125 InitMotor(motor0);
00126 InitMotor(motor1);
00127 kmot_SearchLimits(motor0, 5, 3, &minpos0, &maxpos0,100000);
00128 tgtmin0 = minpos0 + TGT_MARGIN;
00129 tgtmax0 = maxpos0 - TGT_MARGIN;
00130 printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0);
00131
00132 kmot_SearchLimits(motor1, 5, 3, &minpos1, &maxpos1,100000);
00133 tgtmin1 = minpos1 + TGT_MARGIN;
00134 tgtmax1 = maxpos1 - TGT_MARGIN;
00135 printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1);
00136
00137 printf("%d: set pos: %ld,%ld\n\r",counter,tgtmin0,tgtmin1);
00138 #if 0
00139 kmot_SetPoint(motor0, kMotRegPosProfile, tgtmin0);
00140 kmot_SetPoint(motor1, kMotRegPosProfile, tgtmin1);
00141 #else
00142 kmot_SetPoint(motor0, kMotRegPos, tgtmin0);
00143 kmot_SetPoint(motor1, kMotRegPos, tgtmin1);
00144 #endif
00145
00146 while(counter)
00147 {
00148 usleep(100000);
00149 printf("%d: set pos: %ld,%ld\n\r",counter,tgtmin0,tgtmin1);
00150 #if 0
00151 kmot_SetPoint(motor0, kMotRegPosProfile, tgtmin0);
00152 kmot_SetPoint(motor1, kMotRegPosProfile, tgtmin1);
00153 #else
00154 kmot_SetPoint(motor0, kMotRegPos, tgtmin0);
00155 kmot_SetPoint(motor1, kMotRegPos, tgtmin1);
00156 #endif
00157
00158 #if 0
00159 kmot_GetStatus(motor0,&status0,&erreur0);
00160 kmot_GetStatus(motor1,&status1,&erreur1);
00161 while( ! ((status0 & 0x8) && (status1 & 0x8)))
00162 {
00163 kmot_GetStatus(motor0,&status0,&erreur0);
00164 kmot_GetStatus(motor1,&status1,&erreur1);
00165 }
00166 #else
00167 pos0 = kmot_GetMeasure(motor0,kMotMesPos);
00168 pos1 = kmot_GetMeasure(motor1,kMotMesPos);
00169 while( abs(pos0 - tgtmin0) > MARGIN || abs(pos1 - tgtmin1) > MARGIN)
00170 {
00171 pos0 = kmot_GetMeasure(motor0,kMotMesPos);
00172 pos1 = kmot_GetMeasure(motor1,kMotMesPos);
00173 }
00174 #endif
00175 kmot_SetMode(motor0,2);
00176 kmot_SetMode(motor1,2);
00177
00178 usleep(100000);
00179 printf("%d: set pos: %ld,%ld\n\r",counter, tgtmax0,tgtmax1);
00180 #if 0
00181 kmot_SetPoint(motor0, kMotRegPosProfile, tgtmax0);
00182 kmot_SetPoint(motor1, kMotRegPosProfile, tgtmax1);
00183 #else
00184 kmot_SetPoint(motor0, kMotRegPos, tgtmax0);
00185 kmot_SetPoint(motor1, kMotRegPos, tgtmax1);
00186 #endif
00187
00188 #if 0
00189 kmot_GetStatus(motor0,&status0,&erreur0);
00190 kmot_GetStatus(motor1,&status1,&erreur1);
00191 while( ! ((status0 & 0x8) && (status1 & 0x8)))
00192 {
00193 kmot_GetStatus(motor0,&status0,&erreur0);
00194 kmot_GetStatus(motor1,&status1,&erreur1);
00195 }
00196 #else
00197 pos0 = kmot_GetMeasure(motor0,kMotMesPos);
00198 pos1 = kmot_GetMeasure(motor1,kMotMesPos);
00199 while( abs(pos0 - tgtmax0) > MARGIN || abs(pos1 - tgtmax1) > MARGIN)
00200 {
00201 pos0 = kmot_GetMeasure(motor0,kMotMesPos);
00202 pos1 = kmot_GetMeasure(motor1,kMotMesPos);
00203 }
00204 #endif
00205
00206 kmot_SetMode(motor0,2);
00207 kmot_SetMode(motor1,2);
00208
00209 counter--;
00210 }
00211
00212 }
00213