robot.cpp

来自「FIRA 3对3 机器人足球PC端控制程序。」· C++ 代码 · 共 92 行

#include "stdafx.h"
#include "mainfrm.h"
//#include "mciclass.h"
#include "math.h" 
#include "robot.h"

#define WIN_SIZE		32                   
#define PIXEL_LIMIT		10  // Group Boundary	
#define PIXEL_NUM		5   // Minimum Number of Pixels
#define ANGLE_LIMIT_X	2 
#define ANGLE_LIMIT_Y	2

#define GAME_RIGHT		0

#define M_PI 			3.14159265358979323846
      
//Group g[GROUP_MAX]; 
//Group g1[GROUP_MAX];

//extern OMCIClass MCIObject;

//BYTE huge* lpYBuffer, huge* lpUBuffer, huge* lpVBuffer;
//HGLOBAL hYGlobal, hUGlobal, hVGlobal;
//BYTE huge* lpRGBBuffer;
//HGLOBAL hRGBGlobal;
//BYTE huge* lpLYBuffer, huge* lpLUBuffer, huge* lpLVBuffer;
//HGLOBAL hLYGlobal, hLUGlobal, hLVGlobal;
               

/* "Predictor" Structure Definition */
Predictor::Predictor()
{
	int i;

	flag = count = 0;
	a = b = sum_x = sum_y = sum_xy = sum_xx = 0.0;
	for(i = 0;i < PREDICTOR_NUM;i++){
		save_x[i] = 0.0;
		save_y[i] = 0.0;
	}
}

void Predictor::add(double x, double y)
{
	double den;

	if(flag == 0 && count > 0)
		flag = 1;
	if(flag == 1 && count == PREDICTOR_NUM) //PREDICTOR_NUM 30
		flag = 2;
	if(count >= PREDICTOR_NUM)     
		count -= PREDICTOR_NUM;
	sum_x += (x-save_x[count]);
	sum_y += (y-save_y[count]);
	sum_xy += (x*y-save_x[count]*save_y[count]);
	sum_xx += (x*x-save_x[count]*save_x[count]);
	cur_x = x;
	cur_y = y;
	save_x[count] = x;
	save_y[count] = y;
	count++;

	if(flag == 2){
		den = PREDICTOR_NUM*sum_xx-sum_x*sum_x;
		a = (PREDICTOR_NUM*sum_xy-sum_x*sum_y)/den;
		b = (sum_y*sum_xx-sum_xy*sum_x)/den;
	}
	else if(flag == 1){
		den = count*sum_xx-sum_x*sum_x;
		a = (count*sum_xy-sum_x*sum_y)/den;
		b = (sum_y*sum_xx-sum_xy*sum_x)/den;
	}
}

void Predictor::clear()
{
	int i;

	flag = count = 0;
	sum_x = sum_y = sum_xy = sum_xx = 0.0;
	for(i = 0;i < PREDICTOR_NUM;i++){
		save_x[i] = 0.0;
		save_y[i] = 0.0;
	}
}

double Predictor::predict(double x)
{
	return(a*(cur_x+x)+b);
}