Beispiel #1
0
/*
 * 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];
	}
}
Beispiel #2
0
/*
 * 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");
	}*/

}
Beispiel #3
0
/*
 * 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;
}