00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00024 #include <korebot/korebot.h>
00025
00026 #define RCVBUFSIZE 32
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];
00189 int recvMsgSize;
00190 int port;
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
00201
00202 kb_set_debug_level( 2 );
00203
00204
00205 if((rc = kb_init( argc , argv )) < 0 )
00206 return 1;
00207
00208
00209 if((rc = ksock_init('\n')) < 0 )
00210 return 1;
00211
00212
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
00240 kmot_GetFWVersion( motor1, &rc);
00241 printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(rc) , KMOT_REVISION(rc));
00242
00243
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
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
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
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);
00298
00299 return 0;
00300 }
00301