/*--------------------------------------------------------------------*/
int initKH3( void )
{
/* This is required */
kh3_init();
/* open various socket and store the handle in their respective pointers */
dsPic = knet_open( "Khepera3:dsPic" , KNET_BUS_I2C , 0 , NULL );
mot1 = knet_open( "Khepera3:mot1" , KNET_BUS_I2C , 0 , NULL );
mot2 = knet_open( "Khepera3:mot2" , KNET_BUS_I2C , 0 , NULL );
if(dsPic!=0)
{
if((mot1!=0)&&(mot2!=0))
{
initMot(mot1);
initMot(mot2);
return 0;
}
else
return -1;
}
return -2;
}
char *download_from_remote(const char *url)
{
const int buf_size = 1 * 1024 * 1024;
char *fn;
FILE *fp;
uint8_t *buf;
knetFile *fp_remote;
int l;
l = strlen(url);
for (fn = (char*)url + l - 1; fn >= url; --fn)
if (*fn == '/') break;
++fn; // fn now points to the file name
if(fexists(fn))
return fn;
fprintf(stderr, "attempting to download the remote index file: %s\n",url);
extern std::vector <char *> dumpedFiles;
dumpedFiles.push_back(strdup(fn));
fp_remote = knet_open(url, "r");
if (fp_remote == 0) {
fprintf(stderr, "Fail to open remote file:%s\n",url);
exit(0);
}
if ((fp = fopen(fn, "wb")) == 0) {
fprintf(stderr, "Fail to save remote file:%s in working dir\n",url);
exit(0);
knet_close(fp_remote);
}
buf = (uint8_t*)calloc(buf_size, 1);
while ((l = knet_read(fp_remote, buf, buf_size)) != 0)
fwrite(buf, 1, l, fp);
free(buf);
fclose(fp);
knet_close(fp_remote);
return fn;
}
//{{{struct vid_file *open_vid_file(char *file_name)
struct vid_file *open_vid_file(char *file_name)
{
struct vid_file *v = (struct vid_file *) malloc(sizeof(struct vid_file));
v->file_name = strdup(file_name);
v->type = VID_REMOTE;
v->file.remote = knet_open(file_name,"rb+");
if (!(v->file.remote))
err(EX_NOINPUT, "Cannot open VID file '%s", file_name);
v->gqt_header = read_remote_gqt_file_header(v->file_name, v->file.remote);
if ( !((v->gqt_header->marker[0] == 'G') &&
(v->gqt_header->marker[1] == 'Q') &&
(v->gqt_header->marker[2] == 'T')) )
errx(EX_NOINPUT, "File '%s' is not a GQT file.", file_name);
if (v->gqt_header->type != 'v')
errx(EX_NOINPUT, "File '%s' is not a VID file.", file_name);
v->vids = NULL;
return v;
}
int main( int argc , char * argv[] )
{
int rc,ver;
/* Set the libkorebot debug level - Highly recommended for development. */
kb_set_debug_level(2);
if((rc = kb_init( argc , argv )) < 0 )
return 1;
signal( SIGINT , ctrlc_handler );
printf("K-Team KoreIO Test Program\r\n");
koreio = knet_open( "KoreIOLE:Board", KNET_BUS_ANY, 0 , NULL );
if(!koreio)
{
printf("Cannot open KoreIO device trying alternate address\r\n");
koreio = knet_open( "KoreIOLE:AltBoard", KNET_BUS_ANY, 0 , NULL );
if(!koreio)
{
printf("Cannot open KoreIO device\r\n");
return 1;
}
}
/* Get and display the koreio firmware version */
kio_GetFWVersion(koreio,&ver);
printf("KoreIO firmware %d.%d\r\n", (ver&0x000000F0)>>4, (ver&0x0000000F));
/* parse commands */
while (!quitReq) {
printf("\n> ");
if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
buf[strlen(buf)-1] = '\0';
kb_parse_command( buf , cmds , NULL);
}
}
knet_close( koreio );
}
knet_dev_t * PantiltOpen(int motor)
{
switch(motor)
{
case 1 :
return knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL );
break;
case 2 :
return knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL );
break;
case 3 :
return knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL );
break;
case 4 :
return knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL );
break;
default:
return NULL;
break;
}
}
int main(void)
{
char *buf;
knetFile *fp;
int type = 4, l;
#ifdef _WIN32
knet_win32_init();
#endif
buf = calloc(0x100000, 1);
if (type == 0) {
fp = knet_open("knetfile.c", "r");
knet_seek(fp, 1000, SEEK_SET);
} else if (type == 1) { // NCBI FTP, large file
fp = knet_open("ftp://ftp.ncbi.nih.gov/1000genomes/ftp/data/NA12878/alignment/NA12878.chrom6.SLX.SRP000032.2009_06.bam", "r");
knet_seek(fp, 2500000000ll, SEEK_SET);
l = knet_read(fp, buf, 255);
} else if (type == 2) {
fp = knet_open("ftp://ftp.sanger.ac.uk/pub4/treefam/tmp/index.shtml", "r");
knet_seek(fp, 1000, SEEK_SET);
} else if (type == 3) {
fp = knet_open("http://www.sanger.ac.uk/Users/lh3/index.shtml", "r");
knet_seek(fp, 1000, SEEK_SET);
} else if (type == 4) {
fp = knet_open("http://www.sanger.ac.uk/Users/lh3/ex1.bam", "r");
knet_read(fp, buf, 10000);
knet_seek(fp, 20000, SEEK_SET);
knet_seek(fp, 10000, SEEK_SET);
l = knet_read(fp, buf+10000, 10000000) + 10000;
}
if (type != 4 && type != 1) {
knet_read(fp, buf, 255);
buf[255] = 0;
printf("%s\n", buf);
} else write(fileno(stdout), buf, l);
knet_close(fp);
free(buf);
return 0;
}
hFILE *hopen_net(const char *filename, const char *mode)
{
hFILE_net *fp;
// Do any networking initialisation if this is the first use.
if (! net_inited) { if (net_init() < 0) return NULL; }
fp = (hFILE_net *) hfile_init(sizeof (hFILE_net), mode, 0);
if (fp == NULL) return NULL;
fp->netfp = knet_open(filename, mode);
if (fp->netfp == NULL) { hfile_destroy((hFILE *) fp); return NULL; }
fp->base.backend = &net_backend;
return &fp->base;
}
bool PhoneHome::connect()
{
if(ourNumber == -1)
{
srand (time(NULL));
ourNumber = rand();
String numString;
numString = ourNumber;
String thinningString;
thinningString = allThinning;
add("uniqNum", numString);
add("thinning", thinningString);
}
if((ourNumber % 100) >= allThinning)
{
// Skip phoneHome.
return(true);
}
// std::cerr << "url = " << ourURL << std::endl;
ourReturnString.Clear();
// return(true);
#ifndef _NO_PHONEHOME
knet_silent(1);
knetFile *file = knet_open(ourURL.c_str(), "r");
if (file == 0) return(false);
const int BUF_SIZE = 100;
char buf[BUF_SIZE];
ssize_t readLen = BUF_SIZE-1;
ssize_t numRead = readLen;
while(numRead == readLen)
{
numRead = knet_read(file, buf, readLen);
buf[numRead] = '\0';
ourReturnString += buf;
}
// std::cerr << ourReturnString.c_str() << std::endl;
knet_close(file);
knet_silent(0);
ourReturnString = buf;
#endif
return(true);
}
FILE *download_and_open(const char *fn)
{
const int buf_size = 1 * 1024 * 1024;
uint8_t *buf;
FILE *fp;
knetFile *fp_remote;
const char *url = fn;
const char *p;
int l = strlen(fn);
for (p = fn + l - 1; p >= fn; --p)
if (*p == '/') break;
fn = p + 1;
// First try to open a local copy
fp = fopen(fn, "r");
if (fp)
return fp;
// If failed, download from remote and open
fp_remote = knet_open(url, "rb");
if (fp_remote == 0) {
fprintf(stderr, "[download_from_remote] fail to open remote file %s\n",url);
return NULL;
}
if ((fp = fopen(fn, "wb")) == 0) {
fprintf(stderr, "[download_from_remote] fail to create file in the working directory %s\n",fn);
knet_close(fp_remote);
return NULL;
}
buf = (uint8_t*)calloc(buf_size, 1);
while ((l = knet_read(fp_remote, buf, buf_size)) != 0)
fwrite(buf, 1, l, fp);
free(buf);
fclose(fp);
knet_close(fp_remote);
return fopen(fn, "r");
}
int main()
{
char Buffer[100],bar[11][IR_BAR_LEN+5];
int sensors[11],i,n;
float pulsestomm=PULSE_TO_MM_FIRMWARE_S_3 ;
float mmstospeed=MM_S_TO_SPEED_FIRMWARE_S_3 ;
long motspeed = (long)(40.0*mmstospeed);
char fileName[] = "test.txt", searchStr[13]= "Signal level=", searchStr2[6]="ESSID:", measurement[4]
, essid[12], path[1035], ch;
char measurements [200][4], essids[200][12];
int j, y, iterator=0;
kh3_init();
dsPic = knet_open( "Khepera3:dsPic", KNET_BUS_I2C , 0 , NULL );
mot1 = knet_open( "Khepera3:mot1", KNET_BUS_I2C , 0 , NULL );
mot2 = knet_open( "Khepera3:mot2", KNET_BUS_I2C , 0 , NULL );
kmot_SetMode( mot1 , kMotModeIdle );
kmot_SetSampleTime( mot1 , 1550 );
kmot_SetMargin( mot1 , 6 );
kmot_SetOptions( mot1 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk
| kMotSWOptDirectionInv );
kmot_ResetError( mot1 );
kmot_SetBlockedTime( mot1 , 10);
kmot_ConfigurePID( mot1 , kMotRegSpeed , 620 , 3 , 10 );
kmot_ConfigurePID( mot1 ,kMotRegPos,600,20,30);
kmot_SetSpeedProfile(mot1 ,15000,30);
kmot_SetMode( mot2 , kMotModeIdle );
kmot_SetSampleTime( mot2 , 1550 );
kmot_SetMargin( mot2 , 6 );
kmot_SetOptions( mot2 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
kmot_ResetError( mot2 );
kmot_SetBlockedTime( mot2 , 10);
kmot_ConfigurePID( mot2 , kMotRegSpeed , 620 , 3 , 10 );
kmot_ConfigurePID( mot2 ,kMotRegPos,600,20,30);
kmot_SetSpeedProfile(mot2 ,15000,30);
while(iterator<23)
{
FILE *f = fopen(fileName, "w");
FILE *fp = popen("iwlist eth0 scan", "r");
while (fgets(path, sizeof(path)-1, fp) != NULL)
fprintf(f, "%s", path);
fclose(f);
pclose(fp);
fp = fopen(fileName,"r");
while(!feof(fp))
{
for(j=0; j<13; j++)
{
ch = fgetc(fp);
if(ch != searchStr[j])
break;
}
if(j == 13)
{
for(y =0; y<12; y++)
feof(fp);
measurement[0] = fgetc(fp);
feof(fp);
measurement[1] = fgetc(fp);
feof(fp);
measurement[2] = fgetc(fp);
feof(fp);
measurement[3]='\0';
}
}
pclose(fp);
fp = fopen(fileName,"r");
while(!feof(fp))
{
for(j=0; j<6; j++)
{
ch = fgetc(fp);
if(ch != searchStr2[j])
break;
}
if(j == 6)
{
for(y =0; y<5; y++)
feof(fp);
essid[0] = fgetc(fp);
feof(fp);
essid[1] = fgetc(fp);
feof(fp);
essid[2] = fgetc(fp);
feof(fp);
essid[3] = fgetc(fp);
feof(fp);
essid[4] = fgetc(fp);
feof(fp);
essid[5] = fgetc(fp);
feof(fp);
essid[6] = fgetc(fp);
feof(fp);
essid[7] = fgetc(fp);
feof(fp);
essid[8] = fgetc(fp);
feof(fp);
essid[9] = fgetc(fp);
feof(fp);
essid[10] = fgetc(fp);
feof(fp);
essid[11]='\0';
}
}
measurements[iterator][0] = measurement[0];
measurements[iterator][1]= measurement[1];
measurements[iterator][2] = measurement[2];
measurements[iterator][3] = measurement[3];
essids[iterator][0]= essid[0];
essids[iterator][1]= essid[1];
essids[iterator][2]= essid[2];
essids[iterator][3]= essid[3];
essids[iterator][4]= essid[4];
essids[iterator][5]= essid[5];
essids[iterator][6]= essid[6];
essids[iterator][7]= essid[7];
essids[iterator][8]= essid[8];
essids[iterator][9]= essid[9];
essids[iterator][10]= essid[10];
iterator++;
}
FILE *f = fopen(fileName, "w");
for(j =0; j<100; j++)
{
fprintf(f, "%s is %s\n", essids[j], measurements[j]);
fprintf(f, "==============================\n\n\n");
}
fclose(f);
}
int main( int argc , char * argv[] )
{
int rc,ver,io,ad,pw = 0, freq;
char newpage = 12, state;
uint16_t val;
uint32_t time;
/* Set the libkorebot debug level - Highly recommended for development. */
kb_set_debug_level(2);
if((rc = kb_init( argc , argv )) < 0 )
return 1;
signal( SIGINT , ctrlc_handler );
printf("K-Team KoreIO Test Program\r\n");
koreio = knet_open( "KoreIOLE:Board", KNET_BUS_ANY, 0 , NULL );
if(!koreio)
{
printf("Cannot open KoreIOLE device trying alternate address\r\n");
koreio = knet_open( "KoreIOLE:AltBoard", KNET_BUS_ANY, 0 , NULL );
if(!koreio)
{
printf("Cannot open KoreIOLE device\r\n");
return 1;
}
}
/* Get and display the koreio firmware version */
kio_GetFWVersion(koreio,&ver);
printf("KoreIOLE firmware %d.%d\r\n", (ver&0x000000F0)>>4, (ver&0x0000000F));
/* config all the IO as a output */
io = 0;
while(io < 16)
while(io < 16)
{
kio_ConfigIO(koreio,io,1);
io++;
usleep(1000);
}
/* set one of two Output */
io = 0;
while(io < 16)
{
kio_SetIO(koreio,io);
io+= 2;
}
/* clear one of two Output */
io = 1;
while(io < 16)
{
kio_ClearIO(koreio,io);
io+= 2;
}
/* parse commands */
while (!quitReq) {
printf("\n> ");
/* read all the analog input value */
ad = 0;
while(ad < 12)
{
kio_ReadAnalog(koreio,ad,&val,&time);
printf("read ad %d: %u at %lu mS\r\n",ad,val,time);
ad++;
}
/* blink the output */
io = 0;
while(io < 16)
{
kio_ChangeIO(koreio,io);
io++;
}
/* blink the power output */
kio_ClearPW(koreio,pw);
if(++pw > 5)
pw = 0;
kio_SetPW(koreio,pw);
/*if ( fgets( buf , sizeof(buf) , stdin ) != NULL ) {
buf[strlen(buf)-1] = '\0';
kb_parse_command( buf , cmds , NULL);
}*/
/* clear the current page */
usleep(200000);
printf("%c", newpage);
}
knet_close( koreio );
}
int main()
{
char Buffer[100],bar[11][IR_BAR_LEN+5];
int sensors[11],i,n, j=0, y=0, k=0;
float pulsestomm=PULSE_TO_MM_FIRMWARE_S_3 ;
float mmstospeed=MM_S_TO_SPEED_FIRMWARE_S_3 ;
long motspeed = (long)(30.0*mmstospeed),iterator =0, lpos, rpos, lastLpos=0, lastRpos=0;
char *fileName ="results.txt", ch, path[1035];
FILE *f = fopen(fileName, "w");
fprintf(f, "\n=================================================\n\n\n");
kh3_init();
dsPic = knet_open( "Khepera3:dsPic", KNET_BUS_I2C , 0 , NULL );
mot1 = knet_open( "Khepera3:mot1", KNET_BUS_I2C , 0 , NULL );
mot2 = knet_open( "Khepera3:mot2", KNET_BUS_I2C , 0 , NULL );
kmot_SetMode( mot1 , kMotModeIdle );
kmot_SetSampleTime( mot1 , 1550 );
kmot_SetMargin( mot1 , 6 );
kmot_SetOptions( mot1 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk
| kMotSWOptDirectionInv );
kmot_ResetError( mot1 );
kmot_SetBlockedTime( mot1 , 10);
kmot_ConfigurePID( mot1 , kMotRegSpeed , 620 , 3 , 10 );
kmot_ConfigurePID( mot1 ,kMotRegPos,600,20,30);
kmot_SetSpeedProfile(mot1 ,15000,30);
kmot_SetMode( mot2 , kMotModeIdle );
kmot_SetSampleTime( mot2 , 1550 );
kmot_SetMargin( mot2 , 6 );
kmot_SetOptions( mot2 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
kmot_ResetError( mot2 );
kmot_SetBlockedTime( mot2 , 10);
kmot_ConfigurePID( mot2 , kMotRegSpeed , 620 , 3 , 10 );
kmot_ConfigurePID( mot2 ,kMotRegPos,600,20,30);
kmot_SetSpeedProfile(mot2 ,15000,30);
while(iterator<9)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
if(iterator!=4&&iterator!=6&&iterator!=8)
{
kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
sleep(0.5);
FILE *fp = popen("iwlist eth0 scan", "r");
while (fgets(path, sizeof(path)-1, fp) != NULL)
fprintf(f, "%s", path);
lpos = kmot_GetMeasure(mot1, kMotRegPos);
rpos = kmot_GetMeasure(mot2, kMotRegPos);
fprintf(f,"\ncontrol %ld\n",(((lpos-lastLpos)+(rpos-lastRpos))/2));
lastLpos = lpos;
lastRpos = rpos;
fprintf(f, "\n=================================================\n\n\n");
pclose(fp);
sleep(0.5);
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
while(iterator<1)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
while(iterator<1)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(2.8);
iterator=0;
while(iterator<7)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
if(iterator!=6)
{
kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
sleep(0.5);
FILE *fp = popen("iwlist eth0 scan", "r");
while (fgets(path, sizeof(path)-1, fp) != NULL)
fprintf(f, "%s", path);
lpos = kmot_GetMeasure(mot1, kMotRegPos);
rpos = kmot_GetMeasure(mot2, kMotRegPos);
fprintf(f,"\ncontrol %ld\n",(((lpos-lastLpos)+(rpos-lastRpos))/2));
lastLpos = lpos;
lastRpos = rpos;
fprintf(f, "\n=================================================\n\n\n");
pclose(fp);
sleep(0.5);
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
while(iterator<2)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
if(iterator!=1)
{
kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
sleep(0.5);
FILE *fp = popen("iwlist eth0 scan", "r");
while (fgets(path, sizeof(path)-1, fp) != NULL)
fprintf(f, "%s", path);
lpos = kmot_GetMeasure(mot1, kMotRegPos);
rpos = kmot_GetMeasure(mot2, kMotRegPos);
fprintf(f,"\ncontrol %ld\n",(((lpos-lastLpos)+(rpos-lastRpos))/2));
lastLpos = lpos;
lastRpos = rpos;
fprintf(f, "\n=================================================\n\n\n");
pclose(fp);
sleep(0.5);
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
while(iterator<9)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
if(iterator!=8)
{
kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
sleep(0.5);
FILE *fp = popen("iwlist eth0 scan", "r");
while (fgets(path, sizeof(path)-1, fp) != NULL)
fprintf(f, "%s", path);
lpos = kmot_GetMeasure(mot1, kMotRegPos);
rpos = kmot_GetMeasure(mot2, kMotRegPos);
fprintf(f,"\ncontrol %ld\n",(((lpos-lastLpos)+(rpos-lastRpos))/2));
lastLpos = lpos;
lastRpos = rpos;
fprintf(f, "\n=================================================\n\n\n");
pclose(fp);
sleep(0.5);
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
while(iterator<1)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
while(iterator<1)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(2.8);
iterator=0;
while(iterator<12)
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
if(kh3_proximity_ir((char *)Buffer, dsPic))
{
for (i=0; i<11; i++)
{
sensors[i]=(Buffer[i*2+1] | Buffer[i*2+2]<<8);
n=(int)(sensors[i]/4096.0*IR_BAR_LEN);
if (n==0)
sprintf(bar[i],"|\33[%dC>|",IR_BAR_LEN-1);
else if (n>=IR_BAR_LEN-1)
sprintf(bar[i],"|>\33[%dC|",IR_BAR_LEN-1);
else
sprintf(bar[i],"|\33[%dC>\33[%dC|",IR_BAR_LEN-1-n,n);
}
if(sensors[9]>3000&& sensors[10]<3000)
{
kmot_SetPoint( mot1 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]>3000&& sensors[9]<3000)
{
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
}
else if(sensors[10]<3000&&sensors[9]<3000)
{
if(iterator!=1&&iterator!=2&&iterator!=11)
{
kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
sleep(0.5);
FILE *fp = popen("iwlist eth0 scan", "r");
while (fgets(path, sizeof(path)-1, fp) != NULL)
fprintf(f, "%s", path);
lpos = kmot_GetMeasure(mot1, kMotRegPos);
rpos = kmot_GetMeasure(mot2, kMotRegPos);
fprintf(f,"\ncontrol %ld\n",(((lpos-lastLpos)+(rpos-lastRpos))/2));
lastLpos = lpos;
lastRpos = rpos;
fprintf(f, "\n=================================================\n\n\n");
pclose(fp);
sleep(0.5);
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
sleep(1);
iterator++;
}
}
}
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2.8);
iterator=0;
kmot_SetPoint( mot1 , kMotRegSpeed , 0 );
kmot_SetPoint( mot2 , kMotRegSpeed , 0 );
}
int main( int argc , char * argv[] )
{
ksock_t server;
int rc;
unsigned char buf[16];
unsigned char a,b,c,d;
int clntSocket;
char echoBuffer[RCVBUFSIZE]; /* Buffer for echo string */
int recvMsgSize; /* Size of received message */
int port; /* The server port to open */
pthread_t sensor_task;
pthread_t sound_task;
if(argc > 1)
port = atoi(argv[1]);
else
port = 344;
/* Various initialization */
/* Set the libkorebot debug level - Highly recommended for development. */
kb_set_debug_level( 2 );
/* Global libkorebot init */
if((rc = kb_init( argc , argv )) < 0 )
return 1;
/* Korebot socket module init */
if((rc = ksock_init('\n')) < 0 )
return 1;
/* Open the motor devices */
motor1 = knet_open( "KoreMotor:PriMotor1", KNET_BUS_ANY, 0 , NULL );
if(!motor1)
{
printf("Cannot open motor 0\r\n");
return 1;
}
motor0 = knet_open( "KoreMotor:PriMotor2", KNET_BUS_ANY, 0 , NULL );
if(!motor0)
{
printf("Cannot open motor 1\r\n");
return 1;
}
motor3 = knet_open( "KoreMotor:PriMotor3", KNET_BUS_ANY, 0 , NULL );
if(!motor3)
{
printf("Cannot open motor 2\r\n");
return 1;
}
motor2 = knet_open( "KoreMotor:PriMotor4", KNET_BUS_ANY, 0 , NULL );
if(!motor2)
{
printf("Cannot open motor 3\r\n");
return 1;
}
/* KoreMotor Comm test */
kmot_GetFWVersion( motor1, &rc);
printf("Motor 1 Firmware v%u.%u\n" , KMOT_VERSION(rc) , KMOT_REVISION(rc));
/* Intialize motor controller */
InitMotor(motor0);
InitMotor(motor1);
InitMotor(motor2);
InitMotor(motor3);
kmot_SearchLimits(motor0, 10, 3, &minpos0, &maxpos0,100000);
printf("motor0: min:%ld max:%ld\n\r",minpos0, maxpos0);
kmot_SearchLimits(motor1, 10, 3, &minpos1, &maxpos1,100000);
printf("motor1: min:%ld max:%ld\n\r",minpos1, maxpos1);
kmot_SearchLimits(motor2, 10, 3, &minpos2, &maxpos2,100000);
printf("motor2: min:%ld max:%ld\n\r",minpos2, maxpos2);
kmot_SearchLimits(motor3, 10, 3, &minpos3, &maxpos3,100000);
printf("motor3: min:%ld max:%ld\n\r",minpos3, maxpos3);
kmot_SetBlockedTime(motor0,5);
kmot_SetBlockedTime(motor2,5);
/* Network setup - Create a ksock server */
ksock_server_open(&server, port);
ksock_add_command("movecamera",2,2,koa_net_camera);
ksock_add_command("movecamera2",2,2,koa_net_camera2);
ksock_add_command("moveboth",2,2,koa_net_bothcam);
ksock_add_command("initcamera",0,0,koa_net_init);
list_command();
/* Network connection handling */
clntSocket = ksock_next_connection(&server);
for(;;)
{
rc = ksock_get_command(clntSocket, echoBuffer, RCVBUFSIZE);
if(rc>0)
{
ksock_exec_command(echoBuffer);
/* Echo message back to client */
if (send(clntSocket, echoBuffer, rc, 0) != rc)
DieWithError("send() failed");
}
else
switch(rc)
{
case 0 :
break;
case -3 :
printf("Client disconnected\r\n");
clntSocket = ksock_next_connection(&server);
break;
default :
printf("Socket error: %d\r\n",rc);
clntSocket = ksock_next_connection(&server);
break;
}
}
close(clntSocket); /* Close client socket */
return 0;
}
int main()
{
float fpos;
long lpos,rpos;
char Buffer[100],bar[11][IR_BAR_LEN+5];
int sensors[11],i,n,revision,version;
short index, value;
char usvalues[5];
long motspeed;
float pulsestomm=PULSE_TO_MM_FIRMWARE_S_3 ;
float mmstospeed=MM_S_TO_SPEED_FIRMWARE_S_3 ;
kh3_init();
dsPic = knet_open( "Khepera3:dsPic", KNET_BUS_I2C , 0 , NULL );
mot1 = knet_open( "Khepera3:mot1", KNET_BUS_I2C , 0 , NULL );
mot2 = knet_open( "Khepera3:mot2", KNET_BUS_I2C , 0 , NULL );
kmot_SetMode( mot1 , kMotModeIdle );
kmot_SetSampleTime( mot1 , 1550 );
kmot_SetMargin( mot1 , 6 );
kmot_SetOptions( mot1 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk
| kMotSWOptDirectionInv );
kmot_ResetError( mot1 );
kmot_SetBlockedTime( mot1 , 10);
kmot_ConfigurePID( mot1 , kMotRegSpeed , 620 , 3 , 10 );
kmot_ConfigurePID( mot1 ,kMotRegPos,600,20,30);
kmot_SetSpeedProfile(mot1 ,15000,30);
kmot_SetMode( mot2 , kMotModeIdle );
kmot_SetSampleTime( mot2 , 1550 );
kmot_SetMargin( mot2 , 6 );
kmot_SetOptions( mot2 , 0x0 , kMotSWOptWindup | kMotSWOptStopMotorBlk );
kmot_ResetError( mot2 );
kmot_SetBlockedTime( mot2 , 10);
kmot_ConfigurePID( mot2 , kMotRegSpeed , 620 , 3 , 10 );
kmot_ConfigurePID( mot2 ,kMotRegPos,600,20,30);
kmot_SetSpeedProfile(mot2 ,15000,30);
if(kh3_revision((char *)Buffer, dsPic))
{
version=(Buffer[1] | Buffer[2]<<8);
revision=(Buffer[3] | Buffer[4]<<8);
printf("\r\n%c,%4.4u,%4.4u => Version = %u, Revision = %u\r\n",
Buffer[0],version ,revision,version ,revision);
if (version <3)
{
pulsestomm=PULSE_TO_MM_FIRMWARE_S_3;
mmstospeed=MM_S_TO_SPEED_FIRMWARE_S_3;
}
else
{
pulsestomm=PULSE_TO_MM_FIRMWARE_BE_3;
mmstospeed=MM_S_TO_SPEED_FIRMWARE_BE_3;
}
}
if(kh3_battery_voltage((char *)Buffer, 0, dsPic))
{
printf("\r\n%c,%3.3u,%3.3u => battery voltage = %u.%uV\r\n",
Buffer[0], (Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8),
(Buffer[1] | Buffer[2]<<8), (Buffer[3] | Buffer[4]<<8));
}
motspeed= (long)(40.0*mmstospeed);
while(1)
{
index=0;
value=31;
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
for (i=0;i<5;i++)
{
if(kh3_measure_us((char *)Buffer, i+1, dsPic))
{
usvalues[i] = (Buffer[0*8+3] | Buffer[0*8+4]<<8);
}
}
printf("%d\n", usvalues[2]);
if((usvalues[1]<10&&usvalues[1]!=0)||(usvalues[2]<20&&usvalues[2]!=0)||(usvalues[3]<10&&usvalues[3]!=0))
{
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , -motspeed );
sleep(2);
kmot_SetPoint( mot1 , kMotRegSpeed , motspeed );
kmot_SetPoint( mot2 , kMotRegSpeed , motspeed );
}
}
}
int main( int argc , char * argv[] )
{
int rc;
unsigned char a,b,c,d;
int clntSocket;
ksock_t server;
char echoBuffer[RCVBUFSIZE]; /* Buffer for echo string */
int port; /* The server port to open */
pthread_t lrf_task;
/* Set signal handler */
signal(SIGINT, handle_kill);
if(argc > 1)
port = atoi(argv[1]);
else
port = 344;
/* Set the libkorebot debug level - Highly recommended for development. */
kb_set_debug_level(2);
if((rc = kb_init( argc , argv )) < 0 )
return 1;
printf("Koala Demo Program\r\n");
/* Korebot socket module init */
if((rc = ksock_init('\n')) < 0 )
return 1;
/* Open a device for the koala robot */
koala = knet_open( "Koala:Robot", KNET_BUS_ANY, 0 , NULL );
if(!koala)
{
printf("Open Koala device failed\r\n");
return 1;
}
/* Koala Comm test */
koa_getOSVersion(koala,&a,&b,&c,&d);
printf("OS revision %d.%d protocol %d.%d\r\n",a,b,c,d);
koa_setSpeed(koala,0,0);
/* Network setup - Create a ksock server */
ksock_server_open(&server, port);
ksock_add_command("setspeed",2,2,koa_net_setspeed);
ksock_add_command("stop",0,0,koa_net_stop);
list_command();
/* Task creation */
rc = pthread_create(&lrf_task, NULL, koa_lrf_task, NULL);
if (rc) {
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
/* Network connection handling */
clntSocket = ksock_next_connection(&server);
for(;;)
{
#if 1
rc = ksock_get_command(clntSocket, echoBuffer, RCVBUFSIZE);
if(rc>0)
{
cmd_received = 1;
ksock_exec_command(echoBuffer);
/* Echo message back to client */
if (send(clntSocket, echoBuffer, rc, 0) != rc)
DieWithError("send() failed");
}
else
switch(rc)
{
case 0 :
break;
case -3 :
printf("Client disconnected\r\n");
clntSocket = ksock_next_connection(&server);
break;
default :
printf("Socket error: %d\r\n",rc);
clntSocket = ksock_next_connection(&server);
break;
}
#endif
}
close(clntSocket); /* Close client socket */
}
