00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00025 #include <korebot/korebot.h>
00026
00027 #define RCVBUFSIZE 32
00028 #define DEFAULTPORT 344
00029 #define NMOTOR 4
00030 #define SENDBUFFERSIZE 64
00031 #define ERRORSTRING "kmot_error"
00032
00033 knet_dev_t * motorList[NMOTOR];
00034 char sendBuffer[SENDBUFFERSIZE];
00035
00043 int kmot_ipFirmware(int argc, char * argv[],void * data)
00044 {
00045 unsigned int version,motor;
00046
00047 motor = atoi(argv[2]);
00048
00049 if(motor >= NMOTOR)
00050 {
00051 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00052 ksock_send_answer((int*)data,sendBuffer);
00053 }
00054 else
00055 {
00056 kmot_GetFWVersion(motorList[motor],&version);
00057
00058 sprintf(sendBuffer,"%s %s %u.%u",argv[0], argv[1],KMOT_VERSION(version) , KMOT_REVISION(version));
00059
00060 printf("%s\r\n",sendBuffer);
00061 ksock_send_answer((int*)data,sendBuffer);
00062 }
00063
00064 return 0;
00065 }
00066
00074 int kmot_ipStatus(int argc, char * argv[],void * data)
00075 {
00076 unsigned int motor;
00077 unsigned char status,error;
00078
00079 motor = atoi(argv[2]);
00080
00081 if(motor >= NMOTOR)
00082 {
00083 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00084 ksock_send_answer((int*)data,sendBuffer);
00085 }
00086 else
00087 {
00088 kmot_GetStatus( motorList[motor], &status , &error );
00089
00090 sprintf(sendBuffer,"%s %s %u %u",argv[0], argv[1], status, error);
00091
00092 printf("%s\r\n",sendBuffer);
00093 ksock_send_answer((int*)data,sendBuffer);
00094 }
00095
00096 return 0;
00097 }
00098
00106 int kmot_ipConfigPID(int argc, char * argv[],void * data)
00107 {
00108 unsigned int motor;
00109
00110 motor = atoi(argv[2]);
00111
00112 if(motor >= NMOTOR)
00113 {
00114 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00115 ksock_send_answer((int*)data,sendBuffer);
00116 }
00117 else
00118 {
00119 kmot_ConfigurePID( motorList[motor], atoi(argv[3]) , atoi(argv[4]), atoi(argv[5]), atoi(argv[6]));
00120
00121 sprintf(sendBuffer,"%s %s",argv[0], argv[1]);
00122
00123 printf("%s\r\n",sendBuffer);
00124 ksock_send_answer((int*)data,sendBuffer);
00125 }
00126
00127 return 0;
00128 }
00129
00137 int kmot_ipSetPointSource(int argc, char * argv[],void * data)
00138 {
00139 unsigned int motor;
00140
00141 motor = atoi(argv[2]);
00142
00143 if(motor >= NMOTOR)
00144 {
00145 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00146 ksock_send_answer((int*)data,sendBuffer);
00147 }
00148 else
00149 {
00150 kmot_SetPointSource( motorList[motor], atoi(argv[3]),atoi(argv[4]),atoi(argv[5]),atoi(argv[6]),atoi(argv[7]));
00151
00152 sprintf(sendBuffer,"%s %s",argv[0], argv[1]);
00153
00154 printf("%s\r\n",sendBuffer);
00155 ksock_send_answer((int*)data,sendBuffer);
00156 }
00157
00158 return 0;
00159 }
00160
00161
00169 int kmot_ipMeasure(int argc, char * argv[],void * data)
00170 {
00171 unsigned int motor;
00172 long int measure;
00173
00174 motor = atoi(argv[2]);
00175
00176 if(motor >= NMOTOR)
00177 {
00178 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00179 ksock_send_answer((int*)data,sendBuffer);
00180 }
00181 else
00182 {
00183 measure = kmot_GetMeasure( motorList[motor], atoi(argv[3]));
00184
00185 sprintf(sendBuffer,"%s %s %ld",argv[0], argv[1],measure);
00186
00187 printf("%s\r\n",sendBuffer);
00188 ksock_send_answer((int*)data,sendBuffer);
00189 }
00190
00191 return 0;
00192 }
00193
00201 int kmot_ipResetError(int argc, char * argv[],void * data)
00202 {
00203 unsigned int motor;
00204
00205 motor = atoi(argv[2]);
00206
00207 if(motor >= NMOTOR)
00208 {
00209 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00210 ksock_send_answer((int*)data,sendBuffer);
00211 }
00212 else
00213 {
00214 kmot_ResetError( motorList[motor] );
00215
00216 sprintf(sendBuffer,"%s %s",argv[0], argv[1]);
00217
00218 printf("%s\r\n",sendBuffer);
00219 ksock_send_answer((int*)data,sendBuffer);
00220 }
00221
00222 return 0;
00223 }
00224
00232 int kmot_ipSetSpeed(int argc, char * argv[],void * data)
00233 {
00234 unsigned int motor;
00235
00236 motor = atoi(argv[2]);
00237
00238 if(motor >= NMOTOR)
00239 {
00240 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00241 ksock_send_answer((int*)data,sendBuffer);
00242 }
00243 else
00244 {
00245 kmot_SetPoint( motorList[motor] , kMotRegSpeed , atoi(argv[3]));
00246
00247 sprintf(sendBuffer,"%s %s",argv[0], argv[1]);
00248
00249 printf("%s\r\n",sendBuffer);
00250 ksock_send_answer((int*)data,sendBuffer);
00251 }
00252
00253 return 0;
00254 }
00255
00263 int kmot_ipSetPos(int argc, char * argv[],void * data)
00264 {
00265 unsigned int motor;
00266
00267 motor = atoi(argv[2]);
00268
00269 if(motor >= NMOTOR)
00270 {
00271 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00272 ksock_send_answer((int*)data,sendBuffer);
00273 }
00274 else
00275 {
00276 kmot_SetPoint( motorList[motor] , kMotRegPos, atoi(argv[3]));
00277
00278 sprintf(sendBuffer,"%s %s",argv[0], argv[1]);
00279
00280 printf("%s\r\n",sendBuffer);
00281 ksock_send_answer((int*)data,sendBuffer);
00282 }
00283
00284 return 0;
00285 }
00286
00287
00288
00296 int kmot_ipInitMotor(int argc, char * argv[],void * data)
00297 {
00298 unsigned int nmotor;
00299 knet_dev_t * motor;
00300
00301 nmotor = atoi(argv[2]);
00302
00303 if(nmotor >= NMOTOR)
00304 {
00305 sprintf(sendBuffer,"%s %s %s",argv[0], argv[1],ERRORSTRING);
00306 ksock_send_answer((int*)data,sendBuffer);
00307 }
00308 else
00309 {
00310 motor = motorList[nmotor];
00311
00312 kmot_SetMode( motor , kMotModeIdle );
00313 kmot_SetSampleTime( motor , 1550 );
00314 kmot_SetMargin( motor , 6 );
00315 kmot_SetOptions( motor , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
00316
00317 kmot_ResetError( motor );
00318 kmot_SetBlockedTime( motor , 10 );
00319 kmot_SetLimits( motor , kMotRegCurrent , 0 , 500 );
00320 kmot_SetLimits( motor , kMotRegPos , -10000 , 10000 );
00321
00322
00323 kmot_ConfigurePID( motor , kMotRegSpeed , 1500 , 0 , 300 );
00324 kmot_ConfigurePID(motor,kMotRegPos,70,50,10);
00325
00326 kmot_SetSpeedProfile(motor,30,10);
00327
00328 sprintf(sendBuffer,"%s %s",argv[0], argv[1]);
00329
00330 printf("%s\r\n",sendBuffer);
00331 ksock_send_answer((int*)data,sendBuffer);
00332 }
00333
00334 return 0;
00335 }
00336
00337
00338 int main( int argc , char * argv[] )
00339 {
00340 ksock_t server;
00341 int rc;
00342 unsigned char buf[16];
00343 unsigned char a,b,c,d;
00344 int clntSocket;
00345 char echoBuffer[RCVBUFSIZE];
00346 int recvMsgSize;
00347 int port;
00348
00349 pthread_t sensor_task;
00350 pthread_t sound_task;
00351
00352 if(argc > 1)
00353 port = atoi(argv[1]);
00354 else
00355 port = DEFAULTPORT;
00356
00357
00358
00359 kb_set_debug_level( 2 );
00360
00361
00362 if((rc = kb_init( argc , argv )) < 0 )
00363 return 1;
00364
00365
00366 if((rc = ksock_init('\n')) < 0 )
00367 return 1;
00368
00369
00370 motorList[0] = knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL );
00371 if(!motorList[0])
00372 {
00373 printf("Cannot open motor 0\r\n");
00374 return 1;
00375 }
00376 motorList[1] = knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL );
00377 if(!motorList[1])
00378 {
00379 printf("Cannot open motor 1\r\n");
00380 return 1;
00381 }
00382 motorList[2] = knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL );
00383 if(!motorList[2])
00384 {
00385 printf("Cannot open motor 2\r\n");
00386 return 1;
00387 }
00388 motorList[3] = knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL );
00389 if(!motorList[3])
00390 {
00391 printf("Cannot open motor 3\r\n");
00392 return 1;
00393 }
00394
00395
00396
00397 kmot_GetFWVersion( motorList[0], &rc);
00398 printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(rc) , KMOT_REVISION(rc));
00399
00400
00401 ksock_server_open(&server, port);
00402
00403
00404
00405 ksock_add_command("ipFirmware",2,2,kmot_ipFirmware);
00406 ksock_add_command("ipInit",2,2,kmot_ipInitMotor);
00407 ksock_add_command("ipMeasure",3,3,kmot_ipMeasure);
00408 ksock_add_command("ipSetSpeed",3,3,kmot_ipSetSpeed);
00409 ksock_add_command("ipSetPos",3,3,kmot_ipSetPos);
00410 ksock_add_command("ipStatus",2,2,kmot_ipStatus);
00411 ksock_add_command("ipConfigPID",6,6,kmot_ipConfigPID);
00412 ksock_add_command("ipSetPointSource",7,7,kmot_ipSetPointSource);
00413 ksock_add_command("ipResError",2,2,kmot_ipResetError);
00414 list_command();
00415
00416
00417 clntSocket = ksock_next_connection(&server);
00418 for(;;)
00419 {
00420 rc = ksock_get_command(clntSocket, echoBuffer, RCVBUFSIZE);
00421
00422 if(rc>0)
00423 {
00424 ksock_exec_command_pending(clntSocket,echoBuffer);
00425 #if 0
00426
00427 if (send(clntSocket, echoBuffer, rc, 0) != rc)
00428 DieWithError("send() failed");
00429 #endif
00430 }
00431 else
00432 switch(rc)
00433 {
00434 case 0 :
00435 break;
00436 case -3 :
00437 printf("Client disconnected\r\n");
00438 clntSocket = ksock_next_connection(&server);
00439 break;
00440 case -1 :
00441
00442 break;
00443 default :
00444 printf("Socket error: %d\r\n",rc);
00445 clntSocket = ksock_next_connection(&server);
00446 break;
00447 }
00448 }
00449
00450 close(clntSocket);
00451
00452 return 0;
00453 }
00454