void compute_state_imu(){
compute_position_acceleration();
compute_position_velocity();
compute_position();
compute_angle(); //use gyro + mag
compute_angular_velocity(); // differentiate above
compute_angular_acceleration(); // differentiate above
}
FragmentSeparatorCanvas::FragmentSeparatorCanvas(UmlCanvas * canvas, FragmentCanvas * f,
double vp)
: DiagramCanvas(0, canvas, 0, 0, LIFE_LINE_HEIGHT, 0, -1),
fragment(f), vpos(vp) {
if (canvas->paste())
// must not be selected else the move done after if wrong
canvas->unselect(this);
else
compute_position();
show();
}
/**
* uint8_t request_rssi(unsigned char *p) - attain rssi value from the received packet
* and calls the related functions to do the calculation
*/
float request_rssi(unsigned char *p)
{
char node=p[1];
switch(node)
{ case 'B':
rssi_store.current_node=0;
break;
case 'C':
rssi_store.current_node=1;
break;
case 'E':
rssi_store.current_node=2;
break;
default:
return 0.0;
}
int j=8;
for(int i=0;i<10;i++)
{
rssi_store.rssi[rssi_store.current_node][i]=(p[j+1]-'0')*10+(p[j+2]-'0');
if(!(p[j]==' '))
rssi_store.rssi[rssi_store.current_node][i]=(p[j]-'0')*100;
j+=4;
}
// if(!(rssi_store.ready[1][0]||rssi_store.ready[1][1]||rssi_store.ready[1][2]))
// {
// if(rtc_get_ticks()-time<60)
// {
// for(int i=0;i<10;i++)
// {
// rssi_store.pre_rssi[rssi_store.current_node][i]=rssi_store.rssi[rssi_store.current_node][i];
// }
// }
// time=rtc_get_ticks();
// }
// else
// {
rssi_store.ready[1][rssi_store.current_node]=1;
// }
if(rssi_store.ready[1][0]&&rssi_store.ready[1][1]&&rssi_store.ready[1][2])
{
if(!(rssi_store.ready[0][0]||rssi_store.ready[0][1]||rssi_store.ready[0][2]))
{
res_store();
}
compute_position();
// sprintf(str,"%20f",rssi_store.radius);
// strcpy((char*)p, p1);
return rssi_store.radius;
}
return 0.0;
}
uint16_t compute_y_position(uint16_t sourcevalue)
{
if(hasYAnchor)
{
if(sourcevalue >= slaveYAnchorMin)
{
if(sourcevalue <= slaveYAnchorMax)
{
// main (central) area
return compute_position(sourcevalue,
slaveYAnchorMin, slaveYAnchorMax,
usbYAnchorMin, usbYAnchorMax);
}
else
{
// max area
return compute_position(sourcevalue,
slaveYAnchorMax, slaveYMax,
usbYAnchorMax, usbYMax);
}
}
else
{
// min area
return compute_position(sourcevalue,
slaveYMin, slaveYAnchorMin,
usbYMin, usbYAnchorMin);
}
}
else
{
// no anchor processing
return compute_position(sourcevalue,
slaveYMin, slaveYMax,
usbYMin, usbYMax);
}
}
int SurfaceEdgeCollapse::compute_newpoint(int v1, int v2, Star &star, pair *link, int num_link, float &x_0, float &y_0, float &z_0)
{ // This procedure choses the strategy according to which the position of
// the new point is to be computed.
int okay;
switch (new_point)
{
case 1:
okay = compute_midpoint(v1, v2, star, link, num_link, x_0, y_0, z_0);
break;
case 2:
okay = compute_position(v1, v2, star, link, num_link, x_0, y_0, z_0);
break;
case 3:
okay = compute_endpoint(v1, v2, star, link, num_link, x_0, y_0, z_0);
break;
default:
okay = compute_midpoint(v1, v2, star, link, num_link, x_0, y_0, z_0);
break;
};
return (okay);
}
/*Algoritmo Principal*/
int main(int argc, char *argv[])
{
char name[20];// = "_kita_1";
char archiveName[20] = "archive";
char fitputName[20] = "fitput";
char varputName[20] = "varput";
char hvName[20]= "hv";
unsigned int i, j, t;
unsigned int fun, gen;
clock_t startTime, endTime;
double duration, clocktime;
FILE *fitfile,*varfile;
/*Parametros iniciales*/
strcpy(name,argv[1]);
fun = atoi(argv[2]);
gen = atoi(argv[3]);
printf("%s %d %d \n",name,fun,gen);
initialize_data(fun,gen);
/*Iniciar variables*/
initialize_memory();
/*
sprintf(name,"kita_1_");
sprintf(archiveName,strcat(name,"archive.out"));
sprintf(fitputName, strcat(name,"fitput.out"));
sprintf(varputName, strcat(name,"varput.out"));
sprintf(hvName,strcat(name,"hv.out"));
*/
//fitfile = fopen(strcat(strcat(fitputName,name),".out"),"w");
//varfile = fopen(strcat(strcat(varputName,name),".out"),"w");
//hv = fopen(strcat(strcat(hvName,name),".out"),"w");
/* Iniciar generador de aleatorios*/
initialize_rand();
startTime = clock();
/* Iniciar contador de generaciones*/
t = 0;
/* Iniciar valores de la poblacion de manera aleatoria*/
initialize_pop();
/* Calcular velocidad inicial*/
initialize_vel();
/* Evaluar las particulas de la poblacion*/
evaluate();
/* Almacenar el pBest inicial (variables and valor de aptitud) de las particulas*/
store_pbests();
/* Insertar las particulas no domindas de la poblacion en el archivo*/
insert_nondom();
//printf("\n%d",nondomCtr);
/*Ciclo Principal*/
while(t <= maxgen)
{
//clocktime = (clock() - startTime)/(double)CLOCKS_PER_SEC;
/*if(verbose > 0 && t%printevery==0 || t == maxgen)
{
fprintf(stdout,"Generation %d Time: %.2f sec\n",t,clocktime);
fflush(stdout);
}*/
/*if(t%printevery==0 || t == maxgen)
{
fprintf(outfile,"Generation %d Time: %.2f sec\n",t,clocktime);
}*/
/* Calcular la nueva velocidad de cada particula en la pooblacion*/
//printf("\n 1");
compute_velocity();
/* Calcular la nueva posicion de cada particula en la poblacion*/
//printf("\n 2");
compute_position();
/* Mantener las particulas en la poblacion de la poblacion en el espacio de busqueda*/
//printf("\n 3");
maintain_particles();
/* Pertubar las particulas en la poblacion*/
if(t < maxgen * pMut)
mutate(t);
/* Evaluar las particulas en la poblacion*/
//printf("\n 4");
evaluate();
/* Insertar nuevas particulas no domindas en el archivo*/
//printf("\n 5");
update_archive();
/* Actualizar el pBest de las particulas en la poblacion*/
//printf("\n 6");
update_pbests();
/* Escribir resultados del mejor hasta ahora*/
//verbose > 0 && t%printevery==0 || t == maxgen
/*if(t%printevery==0 || t == maxgen)
{
//fprintf(outfile, "Size of Pareto Set: %d\n", nondomCtr);
fprintf(fitfile, "%d\n",t);
fprintf(varfile, "%d\n",t);
for(i = 0; i < nondomCtr; i++)
{
for(j = 0; j < maxfun; j++)
fprintf(fitfile, "%f ", archiveFit[i][j]);
fprintf(fitfile, "\n");
}
fprintf(fitfile, "\n\n");
fflush(fitfile);
for(i = 0; i < nondomCtr; i++)
{
for(j = 0; j < maxfun; j++)
fprintf(varfile, "%f ", archiveVar[i][j]);
fprintf(varfile, "\n");
}
fprintf(varfile, "\n\n");
fflush(varfile);
}*/
/* Incrementar contador de generaciones*/
t++;
//printf("%d\n",t);
}
/* Escribir resultados en el archivo */
save_results(strcat(strcat(archiveName,name),".out"));
endTime = clock();
duration = ( endTime - startTime ) / (double)CLOCKS_PER_SEC;
fprintf(stdout, "%lf sec\n", duration);
//fclose(fitfile);
//fclose(varfile);
free_memory();
return EXIT_SUCCESS;
}
void FragmentSeparatorCanvas::update() {
hide();
compute_position();
show();
canvas()->update();
}
void lzs_renderer_draw(lzs_renderer_t* self)
{
assert(self);
LOGD("debug");
double t0 = a3d_utime();
// stretch screen to 800x480
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrthof(0.0f, SCREEN_W, SCREEN_H, 0.0f, 0.0f, 2.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// draw camera
glEnable(GL_TEXTURE_EXTERNAL_OES);
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, self->texid);
glVertexPointer(3, GL_FLOAT, 0, VERTEX);
glTexCoordPointer(2, GL_FLOAT, 0, COORDS);
glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
glDisable(GL_TEXTURE_EXTERNAL_OES);
utime_update("setup", &t0);
// capture buffers
GLint format = TEXGZ_BGRA;
GLint type = GL_UNSIGNED_BYTE;
glGetIntegerv(GL_IMPLEMENTATION_COLOR_READ_FORMAT_OES, &format);
glGetIntegerv(GL_IMPLEMENTATION_COLOR_READ_TYPE_OES, &type);
if((format == TEXGZ_BGRA) && (type == GL_UNSIGNED_BYTE))
{
LOGD("readpixels format=0x%X, type=0x%X", format, type);
// TODO - check for texgz errors
// process buffers
texgz_tex_t* bc = self->buffer_color;
texgz_tex_t* bg = self->buffer_gray;
texgz_tex_t* bsx = self->buffer_sx;
texgz_tex_t* bsy = self->buffer_sy;
glReadPixels(self->sphero_x - RADIUS_BALL, (SCREEN_H - self->sphero_y - 1) - RADIUS_BALL,
bc->width, bc->height, bc->format, bc->type, (void*) bc->pixels);
#ifdef DEBUG_BUFFERS
texgz_tex_export(bc, "/sdcard/laser-shark/color.texgz");
#endif
utime_update("readpixels", &t0);
texgz_tex_computegray(bc, bg);
utime_update("computegray", &t0);
texgz_tex_computeedges3x3(bg, bsx, bsy);
utime_update("computeedges", &t0);
// compute peak
{
int x;
int y;
int peak_x = 0;
int peak_y = 0;
float peak = 0.0f;
float* gpixels = (float*) bg->pixels;
float* xpixels = (float*) bsx->pixels;
float* ypixels = (float*) bsy->pixels;
for(x = 0; x < bg->width; ++x)
{
for(y = 0; y < bg->height; ++y)
{
int idx = bg->width*y + x;
// compute magnitude squared
float magsq = xpixels[idx]*xpixels[idx] + ypixels[idx]*ypixels[idx];
gpixels[idx] = magsq;
if(magsq > peak)
{
peak_x = x;
peak_y = y;
peak = magsq;
}
}
}
LOGD("peak=%f, peak_x=%i, peak_y=%i", peak, peak_x, peak_y);
utime_update("computepeak", &t0);
#ifdef DEBUG_BUFFERS
texgz_tex_export(bg, "/sdcard/laser-shark/peak.texgz");
#endif
// move sphero center to match peak
self->sphero_x += (float) peak_x - (float) bg->width / 2.0f;
self->sphero_y -= (float) peak_y - (float) bg->height / 2.0f;
}
}
else
{
LOGE("unsupported format=0x%X, type=0x%X", format, type);
}
// compute phone X, Y center
compute_position(self, SCREEN_CX, SCREEN_CY, &self->phone_X, &self->phone_Y);
// compute sphero X, Y
compute_position(self, self->sphero_x, self->sphero_y, &self->sphero_X, &self->sphero_Y);
utime_update("computeposition", &t0);
// compute goal
float dx = self->phone_X - self->sphero_X;
float dy = self->phone_Y - self->sphero_Y;
float a = fix_angle(atan2f(dx, dy) * 180.0f / M_PI);
self->sphero_goal = a - self->sphero_heading_offset;
// compute speed
float dotp = cosf((a - self->sphero_heading) * M_PI / 180.0f);
if(dotp > 0.0f)
{
// linearly interpolate speed based on the turning angle
self->sphero_speed = SPEED_MAX*dotp + SPEED_MIN*(1.0f - dotp);
}
else
{
// go slow to turn around
self->sphero_speed = SPEED_MIN;
}
// draw camera cross-hair
{
float x = SCREEN_CX;
float y = SCREEN_CY;
float r = RADIUS_CROSS;
limit_position(r, &x, &y);
lzs_renderer_crosshair(y - r, x - r, y + r, x + r, 1.0f, 0.0f, 0.0f);
}
// draw sphero search box
{
float r = RADIUS_BALL;
limit_position(r, &self->sphero_x, &self->sphero_y);
float x = self->sphero_x;
float y = self->sphero_y;
lzs_renderer_drawbox(y - r, x - r, y + r, x + r, 0.0f, 1.0f, 0.0f, 0);
}
lzs_renderer_step(self);
// draw string
a3d_texstring_printf(self->string_sphero, "sphero: head=%i, x=%0.1f, y=%0.1f, spd=%0.2f, goal=%i", (int) fix_angle(self->sphero_heading + self->sphero_heading_offset), self->sphero_X, self->sphero_Y, self->sphero_speed, (int) fix_angle(self->sphero_goal));
a3d_texstring_printf(self->string_phone, "phone: heading=%i, slope=%i, x=%0.1f, y=%0.1f", (int) fix_angle(self->phone_heading), (int) fix_angle(self->phone_slope), self->phone_X, self->phone_Y);
a3d_texstring_draw(self->string_sphero, 400.0f, 16.0f, 800, 480);
a3d_texstring_draw(self->string_phone, 400.0f, 16.0f + self->string_sphero->size, 800, 480);
a3d_texstring_draw(self->string_fps, (float) SCREEN_W - 16.0f, (float) SCREEN_H - 16.0f, SCREEN_W, SCREEN_H);
utime_update("draw", &t0);
//texgz_tex_t* screen = texgz_tex_new(SCREEN_W, SCREEN_H, SCREEN_W, SCREEN_H, TEXGZ_UNSIGNED_BYTE, TEXGZ_BGRA, NULL);
//glReadPixels(0, 0, screen->width, screen->height, screen->format, screen->type, (void*) screen->pixels);
//texgz_tex_export(screen, "/sdcard/laser-shark/screen.texgz");
//texgz_tex_delete(&screen);
A3D_GL_GETERROR();
}
