/*
Uses all of the information calculated so far to set up the various qpDUNES
datastructures in preparation for the feedback step.
This is really inefficient right now – there's heaps of probably unnecessary
copying going on.
*/
void OptimalControlProblem::initialise_qp() {
uint32_t i;
real_t Q[NMPC_DELTA_DIM*NMPC_DELTA_DIM];
Eigen::Map<StateWeightMatrix> Q_map(Q);
real_t R[NMPC_CONTROL_DIM*NMPC_CONTROL_DIM];
Eigen::Map<ControlWeightMatrix> R_map(R);
real_t P[NMPC_DELTA_DIM*NMPC_DELTA_DIM];
Eigen::Map<StateWeightMatrix> P_map(P);
real_t g[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> g_map(g);
real_t C[(NMPC_STATE_DIM-1)*NMPC_GRADIENT_DIM];
Eigen::Map<ContinuityConstraintMatrix> C_map(C);
real_t c[NMPC_DELTA_DIM];
Eigen::Map<DeltaVector> c_map(c);
real_t zLow[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> zLow_map(zLow);
real_t zUpp[NMPC_GRADIENT_DIM];
Eigen::Map<GradientVector> zUpp_map(zUpp);
zLow_map.segment<NMPC_DELTA_DIM>(0) = lower_state_bound;
zUpp_map.segment<NMPC_DELTA_DIM>(0) = upper_state_bound;
/* Set up problem dimensions. */
/* TODO: Determine number of affine constraints (D), and add them. */
qpDUNES_setup(
&qp_data,
OCP_HORIZON_LENGTH,
NMPC_DELTA_DIM,
NMPC_CONTROL_DIM,
0,
&qp_options);
return_t status_flag;
/* Gradient vector fixed to zero. */
g_map = GradientVector::Zero();
/* Continuity constraint constant term fixed to zero. */
c_map = DeltaVector::Zero();
/* Zero Jacobians for now */
C_map = ContinuityConstraintMatrix::Zero();
Q_map = state_weights;
R_map = control_weights;
/* Copy the relevant data into the qpDUNES arrays. */
zLow_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) = lower_control_bound;
zUpp_map.segment<NMPC_CONTROL_DIM>(NMPC_DELTA_DIM) = upper_control_bound;
for(i = 0; i < OCP_HORIZON_LENGTH; i++) {
status_flag = qpDUNES_setupRegularInterval(
&qp_data, qp_data.intervals[i],
0, Q, R, 0, g, C, 0, 0, c, zLow, zUpp, 0, 0, 0, 0, 0, 0, 0);
AssertOK(status_flag);
}
/* Set up final interval. */
P_map = terminal_weights;
status_flag = qpDUNES_setupFinalInterval(&qp_data, qp_data.intervals[i],
P, g, zLow, zUpp, 0, 0, 0);
AssertOK(status_flag);
qpDUNES_setupAllLocalQPs(&qp_data, QPDUNES_FALSE);
qpDUNES_indicateDataChange(&qp_data);
}
int main() {
init();
BITMAP *Buffer = create_bitmap(SCREEN_WIDTH ,SCREEN_HEIGHT);
BITMAP *Tile_Set0 = load_bitmap("tiles0.bmp", NULL);
BITMAP *p = load_bitmap("p1.bmp", NULL);
ALFONT_FONT *fnt = alfont_load_font("dr.ttf");
alfont_set_font_size(fnt, 40);
Play P1;
Fill_Mapa d,f;
BITMAP *till_0[400];//imagens do tileset_0
P_map(-48,0);//posicao do mapa na tela quando e desenhado
T_Tile();//tamanho do tile padrao(16x16)
d.C_Tiles(till_0,20,20,400,Tile_Set0,Buffer);//carega o tile_set
while (!key[KEY_ESC]) {
//----------------------------------------------------------------------
Longit_Lat();//latitude e longitude do mapa em relacao ao ponto 0x0 da tela
d.Imp_Map(till_0,400,0,0,60,32,Buffer,map_1f,X_MAP*0.2,Y_MAP*0.2);//plano de fundo//'X_MAP*0.2' e 'Y_MAP*0.2' taixa de delocamento do mapa
d.Imp_Map(till_0,400,0,0,60,32,Buffer,map_1,X_MAP,Y_MAP);//mapa dos objetos a tras do pesonagem
P1.Personagen(Buffer,p);
P1.Mov_Play();
//gravidade(4);
//d.Imp_Map(till_0,400,16,16,0,0,60,32,Buffer,map_1,X_MAP,Y_MAP);//mapa dos objetos a frente do pesonagem
d.Scoll_Map(320,240,58,32); //tacha de movimento do mapa
if(key[KEY_T])//testa o tile_set caregado
d.Tile_test(till_0,20,20,Buffer);
//----------------------------------------------------------------------
EnqPlay_Scrull(Buffer,makecol(255,0,0),208,0,480,448,0,480,0,160,640,0,448,640,1);
textprintf_ex(Buffer,font,100,100,makecol(0,255,0),-1,"x_Mi: %d /y_Mi: %d",X_MAP,Y_MAP);
textprintf_ex(Buffer,font,100,120,makecol(0,255,0),-1,"Lon_Xi: %d /Lon_Yi: %d",Lon_Xi_MAP,Lon_Yi_MAP);
textprintf_ex(Buffer,font,100,140,makecol(0,255,0),-1,"Lon_Xf: %d /Lon_Yf: %d",Lon_Xf_MAP,Lon_Yf_MAP);
//textprintf_ex(Buffer,font,100,140,makecol(0,255,0),-1,"PM_X: %d /PM_Y: %d", mouse_x,mouse_y);
//alfont_textprintf_ex(Buffer, fnt, 300, 90, makecol(255,0,0), -1, "Mario!");
//alfont_textprintf_centre_ex(Buffer, fnt, 300, 150, makecol(255,0,0), -1, "Mario!");
//show_mouse(screen);//mostra o malse na tela
//position_mouse(300,100);
//rotate_sprite(Buffer,Buffer,0,0,DEGREES(450));
//muda a escala da a imagem
//stretch_blit(Buffer,screen,0,0,SCREEN_WIDTH ,SCREEN_HEIGHT,0,0,SCREEN_WIDTH_Z ,SCREEN_HEIGHT_Z);
rest(60);//paralisa o programa por 'x' mile segundos
blit(Buffer,screen,0,0,0,0,SCREEN_WIDTH,SCREEN_HEIGHT);//fixa o Buffer no screen
clear(Buffer);
}
alfont_destroy_font(fnt);
destroy_bitmap(Tile_Set0);
for(int i=0;i<400;i++)
destroy_bitmap(till_0[i]);
destroy_bitmap(Buffer);
deinit();
return 0;
}
