main.h

飞思卡尔智能车程序

int g_nInputVoltage[6];   
float nInputVoltage[6];
#define VOLTAGE_GRAVITY_C				4
#define VOLTAGE_GYRO_C					3
#define VOLTAGE_LEFT_C					0
#define VOLTAGE_GYRO2_C				  2
#define VOLTAGE_RIGHT_C					1

                                                                 
#define VOLTAGE_GYRO					(g_nInputVoltage[VOLTAGE_GYRO_C])  //0加速度计
#define VOLTAGE_GRAVITY					(g_nInputVoltage[VOLTAGE_GRAVITY_C])//1陀螺仪
#define VOLTAGE_LEFT					(g_nInputVoltage[VOLTAGE_LEFT_C])//2左电感
#define DIR_CONTROL_D_VALUE         (g_nInputVoltage[VOLTAGE_GYRO2_C])//3转向陀螺仪
#define VOLTAGE_RIGHT					(g_nInputVoltage[VOLTAGE_RIGHT_C])//4右电感


#define GRAVITY_OFFSET			    1408   //1408      
float  GYROSCOPE_OFFSETGET;
#define GYROSCOPE_OFFSET		GYROSCOPE_OFFSETGET//1794////1575//GYROSCOPE_OFFSETGET//1010//设定陀螺仪初值

#define CAR_ANGLE_SET					0
#define CAR_ANGLE_SPEED_SET				0

#define ANGLE_CONTROL_P				   0.45   //0.2     //0.5    //1.2                                                  
#define ANGLE_CONTROL_D			     0.01  //0.0102	   //0.032	   //0.02                                              
                                                                             

#define MOTOR_OUT_DEAD_VAL				0	//Unit : Full output


float g_fAngleControlOut, g_fSpeedControlOut, g_fDirectionControlOut; //定义全局变量
float g_fCarAngle,g_fGravityAngle, g_fGyroscopeAngleSpeed,g_fGyroscopeAngleIntegral;//定义全局变量
/*g_fCarAngle 滤波计算后的角度 
g_fGravityAngle 加速度z轴减去零偏再乘以Rz得到*/

#define CAR_ANGLE_RANGE					180
#define GRAVITY_MAX						1850		// 3030
#define GRAVITY_MIN						538		// 1030
#define GRAVITY_ANGLE_RATIO				0.14//0005//((float)CAR_ANGLE_RANGE / (float)(GRAVITY_MAX - GRAVITY_MIN) )
                                
#define GYROSCOPE_ANGLE_RATIO	  0.8      //设置陀螺仪比例
#define CONTROL_PERIOD					5		     // Every 5 ms adjust all control
#define GYROSCOPE_ANGLE_SIGMA_FREQUENCY	(1000 / CONTROL_PERIOD) 
/*GYROSCOPE_ANGLE_SIGMA_FREQUENCY 是根据中断5ms调用AngleCalculate()来的  频率200*/
 
#define GRAVITY_ADJUST_TIME_CONSTANT	1  // unit : second


#define SPEED_CONTROL_P           0.135 //0.2//0.1    //D                                                  
#define SPEED_CONTROL_I           0.025 //0.015//0.03   //P                   0.05~0.2                                               
                                                                            
#define SPEED_CONTROL_PERIOD			(SPEED_CONTROL_COUNT * CONTROL_PERIOD) //unit: ms
#define OPTICAL_ENCODE_CONSTANT			100     //
#define SPEED_CONTROL_COUNT				20  	// 20 ms * 5 ms
#define CAR_SPEED_CONSTANT				1000.0/(SPEED_CONTROL_COUNT * CONTROL_PERIOD) / (float)OPTICAL_ENCODE_CONSTANT
float g_fCarSpeed,g_fCarSpeedset;
#define CAR_SPEED_SET		g_fCarSpeedset;		 //g_Argument.fCarSpeedSet;
float g_fSpeedControlIntegral;			// Keep the Speed Control Integral value
float g_fSpeedControlOutOld, g_fSpeedControlOutNew;
int g_nLeftMotorPulse, g_nRightMotorPulse,g_nLeftMotorPulseSigma, g_nRightMotorPulseSigma;
int g_nSpeedControlCount, g_nSpeedControlPeriod;
#define SpeedControlOutMAX   1.5
#define SpeedControlOutMIN   -1.5


float g_fLeftMotorOut, g_fRightMotorOut;
#define MOTOR_OUT_MAX					4
#define MOTOR_OUT_MIN					-4

#define MOTOR_LEFT_SPEED_POSITIVE		(g_fLeftMotorOut>0)
#define MOTOR_RIGHT_SPEED_POSITIVE		(g_fRightMotorOut>0)

unsigned int g_n1MSEventCount;


long g_lnInputVoltageSigma[6];
int g_nInputVoltageCount;

/*--------------方向控制函数变量定义-----------*/
int g_nDirectionControlFlag;
#define DIRECTION_CONTROL_STOP			g_nDirectionControlFlag = 0
#define DIRECTION_CONTROL_START			g_nDirectionControlFlag = 1

float g_fDirectionControlOutOld, g_fDirectionControlOutNew;
float g_fLeftVoltageSigma, g_fRightVoltageSigma;
int g_nDirectionControlCount, g_nDirectionControlPeriod;

#define LEFT_RIGHT_MINIMUM				10

#define DIRECTION_CONTROL_COUNT			2
#define DIRECTION_CONTROL_PERIOD		(DIRECTION_CONTROL_COUNT * CONTROL_PERIOD)

#define DIRECTION_CONTROL_OUT_MAX		MOTOR_OUT_MAX
#define DIRECTION_CONTROL_OUT_MIN		MOTOR_OUT_MIN

int LEFT_OFFSETGET,RIGHT_OFFSETGET;
#define DIR_LEFT_OFFSET			  LEFT_OFFSETGET
#define DIR_RIGHT_OFFSET		  RIGHT_OFFSETGET

#define DIR_CONTROL_P		      1.2
#define DIR_CONTROL_D         0//.0005

#define DIRECTION_CONTROL_DEADVALUE		0//.2
#define DIRECTION_OFFSET         1010




/*-------------函数声明------------*/
void PLL_Init(void);
void system_init(void);
void AD_init(void);
void AD_collect(void);
void GetMotorPulse(void);
void PWM_Init(void);
void AngleCalculate(void);
void AngleControl(void);
void SpeedControl(void);
void SpeedControlOutput(void);
void MotorOutput(void);
void MotorSpeedOut(void);
void SetMotorVoltage(float fLeftVoltage, float fRightVoltage);
void GetInputVoltageAverage(void);
void PIT_Init(void);
void DirectionControl(void);
void DirectionControlOutput(void);
void stopcar(void);