移动机器人lissajous曲线轨迹跟踪

#define _CRT_SECURE_NO_WARNINGS 1
#define _USE_MATH_DEFINES
#include 
#include 
#include 

typedef double（*FUNC）（double*）;

#define	DIM	4
#define PAI M_PI
#define LEN 0.27
#define OME 0.1			/*  */
#define AMPX 1.0		/* x怳暆 */
#define AMPY 0.5		/* y怳暆 */

double	f0（double*） f1（double*） f2（double*） f3（double*） f4（double*）;
double  v1 v2;			/* 捈恑丒夞揮懍搙曄悢 */

void	initial（double* double* double*）;

static FUNC f[DIM + 1] = { f0 f1 f2 f3 f4 };

int main（void）
{
	static double	k[DIM + 1][4] q[DIM + 1][4] r[DIM + 1][4];
	static double	x[3][DIM + 1];
	static double	x_old[DIM + 1] x_new[DIM + 1];

	FILE	*fout;

	double	h t_max;
	long	i j n time;
	double	dx dy d2x d2y d3x d3y;
	double	dtheta d2theta dv1;

	fout = fopen（“lissajous.dat“ “w“）;

	initial（&t_max &h x_old）;
	n = （long）（t_max / h） + 1;

	for （i = 0; i < DIM + 1; i++）{
		fprintf（fout “%10.7lf\t“ x_old[i]）;
	}
	fprintf（fout “\n“）;

	for （i = 0; i < DIM + 1; i++）
		q[i][3] = 0.0;
	for （j = 1; j < n; j++） {

		for （i = 0; i < DIM + 1; i++） {
			k[i][0] = h * （*f[i]）（x_old）;
			r[i][0] = （k[i][0] - 2.0 * q[i][3]） / 2.0;
			x[0][i] = x_old[i] + r[i][0];
			q[i][0] = q[i][3] + 3.0 * r[i][0] - k[i][0] / 2.0;
		}
		for （i = 0; i < DIM + 1; i++） {
			k[i][1] = h * （*f[i]）（x[0]）;
			r[i][1] = （1.0 - sqrt（0.5）） * （k[i][1] - q[i][0]）;
			x[1][i] = x[0][i] + r[i][1];
			q[i][1] = q[i][0] + 3.0 * r[i][1] - （1.0 - sqrt（0.5）） * k[i][1];
		}
		for （i = 0; i < DIM + 1; i++） {
			k[i][2] = h * （*f[i]）（x[1]）;
			r[i][2] = （1.0 + sqrt（0.5）） * （k[i][2] - q[i][1]）;
			x[2][i] = x[1][i] + r[i][2];
			q[i][2] = q[i][1] + 3.0 * r[i][2] - （1.0 + sqrt（0.5）） * k[i][2];
		}
		for （i = 0; i < DIM + 1; i++） {
			k[i][3] = h * （*f[i]）（x[2]）;
			r[i][3] = （k[i][3] - 2.0 * q[i][2]） / 6.0;
			x_new[i] = x[2][i] + r[i][3];
			q[i][3] = q[i][2] + 3.0 * r[i][3] - k[i][3] / 2.0;
		}

		time = j / 10;
		time = j - time * 10;
		if （time == 0）{
			for （i = 0; i < DIM + 1; i++）{