在线时间0 小时
UID338276
注册时间2012-1-10
NXP金币0
该用户从未签到
新手上路

- 积分
- 31
- 最后登录
- 1970-1-1
|
飞思卡尔XS128的程序
/****************************************************************
Code Warrior 5.9
Target : MC9S12XS128
Crystal: 16.000Mhz
Competition Group:Electromagnetic Group
Author:
Time:2012.03.26
******************************************************************/
#include /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include /* derivative information */
#include "SystemInit.h"
float g_fGravityAngle=0; //加速度计倾角
float g_fGyroscopeAngleSpeed=0; //陀螺仪倾角速度
float g_fCarAngle=0; //陀螺仪倾角
float g_fGyroscopeAngleIntegral=0; //陀螺仪倾角积分
#define g_fGyroscopeAngleIntegral_MAX 45
#define g_fGyroscopeAngleIntegral_MIX -45
float VOLTAGE_GYRO; //AD采集的陀螺仪AD值
float VOLTAGE_GRAVITY; //AD采集的加速度计AD值
float VOLTAGE_LEFT; //AD采集的左边电感AD值
float VOLTAGE_RIGHT; //AD采集的右边电感AD值
#define GRAVITY_ANGLE_RATIO 0.134 //加速度计Z轴:"归一化比例系数" 0.1345
#define GYROSCOPE_ANGLE_RATIO 0.24 //陀螺仪:"归一化比例系数" 0.1832
#define GRAVITY_ADJUST_TIME_CONSTANT 3 //加速度计时间补偿系数
#define GRAVITY_OFFSET 1400 //加速度计Z轴偏移量
#define GYROSCOPE_OFFSET 1110 //陀螺仪偏移量
#define CONTROL_PERIOD 5 // Every 5 ms adjust all control
#define GYROSCOPE_ANGLE_SIGMA_FREQUENCY (1000 / CONTROL_PERIOD) //陀螺仪积分时间 5ms
float g_fAngleControlOut; //角度控制输出量
#define CAR_ANGLE_SET 0 //宏定义恒等于0
#define CAR_ANGLE_SPEED_SET 0 //宏定义恒等于0
#define ANGLE_CONTROL_P 0.3 //角度控制比例参数 0.3
#define ANGLE_CONTROL_D 0.005 //角度控制微分参数 0.005
unsigned int AD_VAL_GYRO[20];
unsigned int AD_VAL_ACCE[20];
unsigned int AD_VAL_LEFT[20];
unsigned int AD_VAL_RIGHT[20];
unsigned int ADv0;
unsigned int ADv1;
unsigned int ADv2;
unsigned int ADv3;
float g_fLeftMotorOut;
float g_fRightMotorOut;
float AnglePWMControlOut;
#define MOTOR_OUT_DEAD_VAL 1
int g_nAngleControlFlag;
int g_nCarControlFlag;
#define ANGLE_CONTROL_STOP g_nAngleControlFlag = 0
#define ANGLE_CONTROL_START g_nAngleControlFlag = 1
#define CAR_CONTROL_SET g_nCarControlFlag = 1
#define CAR_CONTROL_CLEAR g_nCarControlFlag = 0
#define MOTOR_OUT_MAX 1.0
#define MOTOR_OUT_MIN -1.0
#define ANGLE_CONTROL_OUT_MAX MOTOR_OUT_MAX * 10
#define ANGLE_CONTROL_OUT_MIN MOTOR_OUT_MIN * 10
#define CAR_FAILURE_ANGLE_MAX 45.0
#define CAR_FAILURE_ANGLE_MIN -45.0
void SetMotorVoltage(float fLeftVoltage, float fRightVoltage) {
// Voltage : > 0 : Move forward;
// < 0 : Move backward
/* short nPeriod;
int nOut;
nPeriod = (short)getReg(PWM_PWMCM); */
//--------------------------------------------------------------------------
if(fLeftVoltage > 1.0) fLeftVoltage = 1.0;
else if(fLeftVoltage < -1.0) fLeftVoltage = -1.0;
if(fRightVoltage > 1.0) fRightVoltage = 1.0;
else if(fRightVoltage < -1.0) fRightVoltage = -1.0;
//--------------------------------------------------------------------------
if(fLeftVoltage > 0) {
//setReg(PWM_PWMVAL1, 0);
PWMDTY01=0;
//nOut = (int)(fLeftVoltage * nPeriod);
//setReg(PWM_PWMVAL0, nOut);
PWMDTY23=(int)(fLeftVoltage * 100);
} else {
//setReg(PWM_PWMVAL0, 0);
PWMDTY23=0;
fLeftVoltage = -fLeftVoltage;
//nOut = (int)(fLeftVoltage * nPeriod);
//setReg(PWM_PWMVAL1, nOut);
PWMDTY01=(int)(fLeftVoltage * 100);
}
//--------------------------------------------------------------------------
if(fRightVoltage > 0) {
//setReg(PWM_PWMVAL2, 0);
PWMDTY45=1;
//nOut = (int)(fRightVoltage * nPeriod);
//setReg(PWM_PWMVAL3, nOut);
PWMDTY67=(int)(fRightVoltage * 100);
} else {
//setReg(PWM_PWMVAL3, 0);
PWMDTY67=0;
fRightVoltage = -fRightVoltage;
//nOut = (int)(fRightVoltage * nPeriod);
//setReg(PWM_PWMVAL2, nOut);
PWMDTY45=(int)(fRightVoltage * 100);
}
}
void MotorSpeedOut(void)
{
float fLeftVal, fRightVal;
fLeftVal = g_fLeftMotorOut;
fRightVal = g_fRightMotorOut;
if(fLeftVal > 0) fLeftVal += MOTOR_OUT_DEAD_VAL;
else if(fLeftVal < 0) fLeftVal -= MOTOR_OUT_DEAD_VAL;
if(fRightVal > 0) fRightVal += MOTOR_OUT_DEAD_VAL;
else if(fRightVal < 0) fRightVal -= MOTOR_OUT_DEAD_VAL;
if(fLeftVal > MOTOR_OUT_MAX) fLeftVal = MOTOR_OUT_MAX;
if(fLeftVal < MOTOR_OUT_MIN) fLeftVal = MOTOR_OUT_MIN;
if(fRightVal > MOTOR_OUT_MAX) fRightVal = MOTOR_OUT_MAX;
if(fRightVal < MOTOR_OUT_MIN) fRightVal = MOTOR_OUT_MIN;
SetMotorVoltage(fLeftVal, fRightVal);
}
void MotorOutput(void)
{
float fLeft, fRight;
//fLeft = g_fAngleControlOut - g_fSpeedControlOut - g_fDirectionControlOut;
fLeft = g_fAngleControlOut;
//fRight = g_fAngleControlOut - g_fSpeedControlOut + g_fDirectionControlOut;
fRight = g_fAngleControlOut;
if(fLeft > MOTOR_OUT_MAX) fLeft = MOTOR_OUT_MAX;
if(fLeft < MOTOR_OUT_MIN) fLeft = MOTOR_OUT_MIN;
if(fRight > MOTOR_OUT_MAX) fRight = MOTOR_OUT_MAX;
if(fRight < MOTOR_OUT_MIN) fRight = MOTOR_OUT_MIN;
g_fLeftMotorOut = fLeft;
g_fRightMotorOut = fRight;
MotorSpeedOut();
}
void AngleCalculate(void)
{
float fDeltaValue;
// Define Angle
g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;
g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;
g_fCarAngle = g_fGyroscopeAngleIntegral;
fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
//#if SPEED_CONTROL_METHOD == 0
g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
//#else if SPEED_CONTROL_METHOD == 1
// g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + g_fAngleControlSpeedFeedback) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
//#endif // SPEED_CONTROL_METHOD
}
void AngleControl(void)
{
float fValue;
if(g_nAngleControlFlag == 0) {
g_fAngleControlOut = 0;
return;
}
//--------------------------------------------------------------------------
fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P + (CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D;
if(fValue > ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX;
else if(fValue < ANGLE_CONTROL_OUT_MIN) fValue = ANGLE_CONTROL_OUT_MIN;
g_fAngleControlOut = fValue;
}
void CarControlStart(void) {
CAR_CONTROL_SET;
ANGLE_CONTROL_START;
//SPEED_CONTROL_START;
//DIRECTION_CONTROL_START;
//TIME_TEST_ENA;
}
void CarControlStop(void)
{
SetMotorVoltage(0, 0);
CAR_CONTROL_CLEAR;
ANGLE_CONTROL_STOP;
//SPEED_CONTROL_STOP;
//DIRECTION_CONTROL_STOP;
//TIME_TEST_DIS;
g_fLeftMotorOut = g_fRightMotorOut = 0;
g_fAngleControlOut = 0;
//g_fSpeedControlOut = 0;
//g_fDirectionControlOut = 0;
//g_nWaitCarStandCount = 0;
}
/*void WaitCarStand(void) {
if(g_nCarControlFlag == 1) return;
if(g_nWaitCarStandCount < WAIT_CAR_STAND_COUNT) {
g_nWaitCarStandCount ++;
return;
}
if(g_fCarAngle > CAR_STAND_ANGLE_MIN && g_fCarAngle < CAR_STAND_ANGLE_MAX && g_fGravityAngle > CAR_STAND_ANGLE_MIN && g_fGravityAngle < CAR_STAND_ANGLE_MAX)
{
CarControlStart();
}
} */
void CheckCarStand(void) {
if(g_nCarControlFlag == 0) return;
if(g_fCarAngle >= CAR_FAILURE_ANGLE_MAX || g_fCarAngle
|
|