KoreBot Library: pantilt_demo.c Source File

00001 /*------------------------------------------------------------------------------- 00002 * Project: KoreBot Library 00003 * $Author: pbureau $ 00004 * $Date: 2005/01/20 08:42:05 $ 00005 * $Revision: 1.1 $ 00006 * 00007 * 00008 * $Header: /home/cvs/libkorebot/src/tests/pantilt_demo.c,v 1.1 2005/01/20 08:42:05 pbureau Exp $ 00009 */ 00010 00011 /*--------------------------------------------------------------------*/ 00024 #include <korebot/korebot.h> 00025 00026 #define RCVBUFSIZE 32 /* Size of receive buffer */ 00027 00028 00029 knet_dev_t *motor0, *motor1, *motor2, *motor3; 00030 int32_t minpos0,maxpos0,minpos1,maxpos1; 00031 int32_t minpos2,maxpos2,minpos3,maxpos3; 00032 00033 /****************************************************************/ 00037 void * koa_init_task(void * arg) 00038 { 00039 kmot_ResetError(motor0); 00040 kmot_ResetError(motor1); 00041 kmot_ResetError(motor2); 00042 kmot_ResetError(motor3); 00043 kmot_SearchLimits(motor0, 10, 3, &minpos0, &maxpos0,100000); 00044 printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0); 00045 kmot_SearchLimits(motor1, 10, 3, &minpos1, &maxpos1,100000); 00046 printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1); 00047 kmot_SearchLimits(motor2, 10, 3, &minpos2, &maxpos2,100000); 00048 printf("motor2: min:%ld max:%ld\n\r",minpos2, maxpos2); 00049 kmot_SearchLimits(motor3, 10, 3, &minpos3, &maxpos3,100000); 00050 printf("motor3: min:%ld max:%ld\n\r",minpos3, maxpos3); 00051 kmot_SetBlockedTime(motor0,5); 00052 kmot_SetBlockedTime(motor2,5); 00053 00054 return NULL; 00055 } 00056 00057 00058 /****************************************************************/ 00061 int koa_net_init(int argc, char * argv[]) 00062 { 00063 pthread_t init_task; 00064 int rc; 00065 00066 rc = pthread_create(&init_task, NULL, koa_init_task, NULL); 00067 if (rc){ 00068 KB_FATAL("koa_net_init", KB_ERROR_PTHREAD, "koa_init_task"); 00069 } 00070 } 00071 00072 int koa_net_camera(int argc, char * argv[]) 00073 { 00074 int i; 00075 int pos0,pos1; 00076 00077 pos0 = atoi(argv[1]); 00078 if(pos0 < 5) 00079 pos0 = 6; 00080 if(pos0 > 95) 00081 pos0 = 95; 00082 00083 pos1 = atoi(argv[2]); 00084 if(pos1 < 5) 00085 pos1 = 6; 00086 if(pos1 > 95) 00087 pos1 = 95; 00088 00089 pos0 = (pos0 * minpos0)/100; 00090 pos1 = (pos1 * minpos1)/100; 00091 #if 1 00092 kmot_SetPoint(motor0, kMotRegPos, pos0); 00093 kmot_SetPoint(motor1, kMotRegPos, pos1); 00094 #else 00095 kmot_SetPoint(motor0, kMotRegPosProfile, pos0); 00096 kmot_SetPoint(motor1, kMotRegPosProfile, pos1); 00097 #endif 00098 for(i=0;i<argc;i++) 00099 printf("%s ",argv[i]); 00100 printf("(%d,%d)\r\n",pos0,pos1); 00101 } 00102 00103 int koa_net_camera2(int argc, char * argv[]) 00104 { 00105 int i; 00106 int pos0,pos1; 00107 00108 pos0 = atoi(argv[1]); 00109 if(pos0 < 5) 00110 pos0 = 6; 00111 if(pos0 > 95) 00112 pos0 = 95; 00113 00114 pos1 = atoi(argv[2]); 00115 if(pos1 < 5) 00116 pos1 = 6; 00117 if(pos1 > 95) 00118 pos1 = 95; 00119 00120 pos0 = (pos0 * minpos2)/100; 00121 pos1 = (pos1 * minpos3)/100; 00122 kmot_SetPoint(motor2, kMotRegPos, pos0); 00123 kmot_SetPoint(motor3, kMotRegPos, pos1); 00124 for(i=0;i<argc;i++) 00125 printf("%s ",argv[i]); 00126 printf("(%d,%d)\r\n",pos0,pos1); 00127 } 00128 00129 int koa_net_bothcam(int argc, char * argv[]) 00130 { 00131 int i; 00132 int pos0,pos1; 00133 00134 pos0 = atoi(argv[1]); 00135 if(pos0 < 5) 00136 pos0 = 6; 00137 if(pos0 > 95) 00138 pos0 = 95; 00139 00140 pos1 = atoi(argv[2]); 00141 if(pos1 < 5) 00142 pos1 = 6; 00143 if(pos1 > 95) 00144 pos1 = 95; 00145 00146 kmot_SetPoint(motor0, kMotRegPos, (pos0 * minpos0)/100); 00147 kmot_SetPoint(motor1, kMotRegPos, (pos1 * minpos1)/100); 00148 kmot_SetPoint(motor2, kMotRegPos, (pos0 * minpos2)/100); 00149 kmot_SetPoint(motor3, kMotRegPos, (pos1 * minpos3)/100); 00150 for(i=0;i<argc;i++) 00151 printf("%s ",argv[i]); 00152 printf("(%d,%d)\r\n",pos0,pos1); 00153 } 00154 00155 int net_exit(int argc, char * argv[]) 00156 { 00157 exit(0); 00158 } 00159 00160 /****************************************************************/ 00163 void InitMotor(knet_dev_t * mot) 00164 { 00165 kmot_SetMode(mot,0); 00166 kmot_SetSampleTime(mot,1550); 00167 kmot_SetMargin(mot,20); 00168 kmot_SetOptions(mot,0,kMotSWOptWindup|kMotSWOptStopMotorBlk); 00169 kmot_ResetError(mot); 00170 00171 kmot_ConfigurePID(mot,kMotRegSpeed,1500,0,300); 00172 kmot_ConfigurePID(mot,kMotRegPos,70,50,5); 00173 kmot_SetSpeedProfile(mot,30,10); 00174 00175 kmot_SetBlockedTime(mot,5); 00176 kmot_SetLimits(mot,kMotRegCurrent,0,50); 00177 kmot_SetLimits(mot,kMotRegPos,-10000,10000); 00178 } 00179 00180 /****************************************************************/ 00181 int main( int argc , char * argv[] ) 00182 { 00183 ksock_t server; 00184 int rc; 00185 unsigned char buf[16]; 00186 unsigned char a,b,c,d; 00187 int clntSocket; 00188 char echoBuffer[RCVBUFSIZE]; /* Buffer for echo string */ 00189 int recvMsgSize; /* Size of received message */ 00190 int port; /* The server port to open */ 00191 00192 pthread_t sensor_task; 00193 pthread_t sound_task; 00194 00195 if(argc > 1) 00196 port = atoi(argv[1]); 00197 else 00198 port = 344; 00199 00200 /* Various initialization */ 00201 /* Set the libkorebot debug level - Highly recommended for development. */ 00202 kb_set_debug_level( 2 ); 00203 00204 /* Global libkorebot init */ 00205 if((rc = kb_init( argc , argv )) < 0 ) 00206 return 1; 00207 00208 /* Korebot socket module init */ 00209 if((rc = ksock_init('\n')) < 0 ) 00210 return 1; 00211 00212 /* Open the motor devices */ 00213 motor1 = knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL ); 00214 if(!motor1) 00215 { 00216 printf("Cannot open motor 0\r\n"); 00217 return 1; 00218 } 00219 motor0 = knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL ); 00220 if(!motor0) 00221 { 00222 printf("Cannot open motor 1\r\n"); 00223 return 1; 00224 } 00225 motor3 = knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL ); 00226 if(!motor3) 00227 { 00228 printf("Cannot open motor 2\r\n"); 00229 return 1; 00230 } 00231 motor2 = knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL ); 00232 if(!motor2) 00233 { 00234 printf("Cannot open motor 3\r\n"); 00235 return 1; 00236 } 00237 00238 00239 /* KoreMotor Comm test */ 00240 kmot_GetFWVersion( motor1, &rc); 00241 printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(rc) , KMOT_REVISION(rc)); 00242 00243 /* Intialize motor controller */ 00244 InitMotor(motor0); 00245 InitMotor(motor1); 00246 InitMotor(motor2); 00247 InitMotor(motor3); 00248 kmot_SearchLimits(motor0, 10, 3, &minpos0, &maxpos0,100000); 00249 printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0); 00250 kmot_SearchLimits(motor1, 10, 3, &minpos1, &maxpos1,100000); 00251 printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1); 00252 kmot_SearchLimits(motor2, 10, 3, &minpos2, &maxpos2,100000); 00253 printf("motor2: min:%ld max:%ld\n\r",minpos2, maxpos2); 00254 kmot_SearchLimits(motor3, 10, 3, &minpos3, &maxpos3,100000); 00255 printf("motor3: min:%ld max:%ld\n\r",minpos3, maxpos3); 00256 kmot_SetBlockedTime(motor0,5); 00257 kmot_SetBlockedTime(motor2,5); 00258 00259 /* Network setup - Create a ksock server */ 00260 ksock_server_open(&server, port); 00261 00262 ksock_add_command("movecamera",2,2,koa_net_camera); 00263 ksock_add_command("movecamera2",2,2,koa_net_camera2); 00264 ksock_add_command("moveboth",2,2,koa_net_bothcam); 00265 ksock_add_command("initcamera",0,0,koa_net_init); 00266 list_command(); 00267 00268 /* Network connection handling */ 00269 clntSocket = ksock_next_connection(&server); 00270 for(;;) 00271 { 00272 rc = ksock_get_command(clntSocket, echoBuffer, RCVBUFSIZE); 00273 00274 if(rc>0) 00275 { 00276 ksock_exec_command(echoBuffer); 00277 /* Echo message back to client */ 00278 if (send(clntSocket, echoBuffer, rc, 0) != rc) 00279 DieWithError("send() failed"); 00280 } 00281 else 00282 switch(rc) 00283 { 00284 case 0 : 00285 break; 00286 case -3 : 00287 printf("Client disconnected\r\n"); 00288 clntSocket = ksock_next_connection(&server); 00289 break; 00290 default : 00291 printf("Socket error: %d\r\n",rc); 00292 clntSocket = ksock_next_connection(&server); 00293 break; 00294 } 00295 } 00296 00297 close(clntSocket); /* Close client socket */ 00298 00299 return 0; 00300 } 00301