/*
* Predict the ball for Tpred seconds
* Using ballflight + bounce model.
*
* Note: Unlike in optim_task, here we do not need to check for ball's validity
* as we want to predict the ball repeatedly in MPC.
*
*/
static void predict_ball_path(SL_Cstate *ballPred) {
int N = TPRED/TSTEP;
ballPath = malloc((N+1)* sizeof(SL_Cstate));
int i,j;
for (j = 1; j <= N_CART; j++) {
ballPath[0].x[j] = ballPred->x[j];
ballPath[0].xd[j] = ballPred->xd[j];
}
// predict Tpred seconds into the future
int bounce = FALSE;
for (i = 1; i <= N; i++) {
integrate_ball_state(ballPred,TSTEP,&bounce);
for (j = 1; j <= N_CART; j++) {
ballPath[i].x[j] = ballPred->x[j];
ballPath[i].xd[j] = ballPred->xd[j];
}
}
/* revert back to initial ballpred */
for (j = 1; j <= N_CART; j++) {
ballPred->x[j] = ballPath[0].x[j];
ballPred->xd[j] = ballPath[0].xd[j];
}
}
/*
* Predict the ball for Tpred seconds
* Using ballflight + bounce model
*
*/
void predict_ball_state(double *b0, double *v0) {
int N = TPRED/TSTEP;
ballMat = my_matrix(1, N, 1, 2*CART);
SL_Cstate ballPred;
bzero((char *)&(ballPred), sizeof(ballPred));
int i,j;
for (j = 1; j <= CART; j++) {
ballMat[0][j] = ballPred.x[j] = b0[j-1];
ballMat[0][j+CART] = ballPred.xd[j] = v0[j-1];
}
// predict Tpred seconds into the future
int bounce = FALSE;
for (i = 1; i <= N; i++) {
integrate_ball_state(ballPred,&ballPred,TSTEP,&bounce);
for (j = 1; j <= CART; j++) {
ballMat[i][j] = ballPred.x[j];
ballMat[i][j+CART] = ballPred.xd[j];
}
}
//print_mat("Ball pred matrix: ", ballMat);
/*for (i = 1; i <= N; i++) {
for (j = 1; j <= 2*CART; j++)
printf("%.4f ", ballMat[i][j]);
printf("\n");
}*/
}
/*
* The lookup table is built up over the net [y-positions of the estimated balls]
* So when we want to lookup the corresponding parameters when the ball is estimated to be somewhere else
* then we have to predict forwards or backwards
*/
void adapt_lookup(SL_Cstate *ballPred, SL_DJstate target[], double *hitTime) {
static double lookup_fix_pos;
static double timeMax = 1.0; // only predict this much
static Vector ballvec;
static int firsttime = TRUE;
static double TSTEP = 0.002;
int j;
int bounce = FALSE;
double timePred = 0.0;
if (firsttime) {
ballvec = my_vector(1,2*N_CART);
lookup_fix_pos = dist_to_table - 0.5 * table_length; // net position
}
// copy ballpred state
for (j = 1; j <= N_CART; j++) {
ballvec[j] = ballPred->x[j];
ballvec[j + N_CART] = ballPred->xd[j];
}
// predict the ball till its around the net
while (ballPred->x[2] < lookup_fix_pos && timePred < timeMax) {
integrate_ball_state(ballPred,TSTEP,&bounce);
timePred += TSTEP;
}
// look up now over the net
lookup(target,hitTime,*ballPred,FALSE);
// revert back to initial ballpred
for (j = 1; j <= N_CART; j++) {
ballPred->x[j] = ballvec[j];
ballPred->xd[j] = ballvec[j + N_CART];
}
// adjust hitTime since the ball is not there yet at the net
*hitTime += timePred;
}