飞思卡尔K60dz电磁直立车程序 -

/*
* 底层库：基于野火5.0库开发
* 作者：北华航天工业学院秋明山车神队
* 注意：未经作者允许，请勿用于商业，可以任意转载，但请保留作者原创
*
*/
/***************************************************************/
#include "common.h"
#include "include.h"
/**************************************************************/
extern float Gyro_Now,g_fCarAngle;
extern float OutData[4]; //SCI示波器参数
extern float Gyro_Now,angle_offset_vertical; //陀螺仪转化后的角速度，转化后的加速度角度
extern float g_fCarAngle,g_fGyroscopeAngleIntegral; //融合后的角度
extern volatile int MMA7361 ,ENC03,real_angle; //加速度计AD ,陀螺仪AD，模块输出的角度
extern void PIT0_IRQHandler(void);
extern void OutPut_Data(void); //SCI采参数
void main()
{
DisableInterrupts; //禁止总中断
LCD_Init(); //显示屏初始化
GYRO_VAL=flash_read(255, 0, uint16);
GYRO2_VAL=flash_read(254, 0, uint16);
led_Init(); //LED初始化
adc_Init(); //ad采集初始化
QD_Init(); //编码器初始化
uart_init (UART1, 115200); //蓝牙初始化
Key_Init(); //按键初始化
//flash_init(); //FLASH不要初始化
gpio_init (PTA4, GPI,0); //PTA4设置下拉
port_init (PTA4, PULLDOWN );
FTM_PWM_init(FTM0, FTM_CH1,10000,0);
FTM_PWM_init(FTM0, FTM_CH2,10000,0);
FTM_PWM_init(FTM0, FTM_CH3,10000,0);
FTM_PWM_init(FTM0, FTM_CH4,10000,0);
FTM_PWM_Duty(FTM0,FTM_CH1,0);
FTM_PWM_Duty(FTM0,FTM_CH2,0);
FTM_PWM_Duty(FTM0,FTM_CH3,0);
FTM_PWM_Duty(FTM0,FTM_CH4,0);
pit_init_ms(PIT0, 1); //初始化PIT0，定时时间为： 5ms
set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler); //设置PIT0的中断复位函数为 PIT0_IRQHandler
enable_irq (PIT0_IRQn); //使能PIT0中断
DELAY_MS(1000);
EnableInterrupts; //中断允许
while(1)
{
Key();
/*#if 1
OutData[0] = g_fDirectionControlOut;
OutData[1] = g_fCarSpeednew;
OutData[2] = g_fCarSpeedleft;
OutData[3] = g_fCarSpeedright;*/
/* #if 1
OutData[0] = 1000*(AD[0]-AD[3])/(AD[0]+AD[3]);
OutData[1] = 1000*(AD[1]-AD[2])/(AD[1]+AD[2]);
OutData[2] = 1000*(AD[0]-AD[3]);
OutData[3] = 1000*(AD[0]+AD[3]);
OutPut_Data();
#endif
*/
/*
#if 1 //直立角度
OutData[0] = ENC03;
OutData[1] = MMA7361;
OutData[2] = angle_offset_vertical ;
OutData[3] = g_fCarAngle;
OutPut_Data();
#endif
*/
/* #if 1 //车速
OutData[0] = g_fSpeedControlIntegral;
OutData[1] = (g_fSpeedControlOutNew-g_fSpeedControlIntegral);
OutData[2] = g_fCarSpeednew ;
OutData[3] = 10*CAR_SPEED_SET;
OutPut_Data();
#endif
*/
/*#if 1 //车速
OutData[0] = g_fSpeedControlIntegral;
OutData[1] = bianhualeft;
OutData[2] = bianhuaright;
OutData[3] = g_fCarSpeednew;
OutPut_Data();
#endif*/
if(Star_flag==0&&g_nInterrupt_Count==2)
{
LCD_Fill(0x00); //初始清屏
LCD_PrintU16(0,0,AD_average[0]);
LCD_PrintU16(32,0,AD_average[1]);
LCD_PrintU16(64,0,AD_average[2]);
LCD_PrintU16(96,0,AD_average[3]);
LCD_PrintU16(0,2,AD[0]);
LCD_PrintU16(32,2,AD[1]);
LCD_PrintU16(64,2,AD[2]);
LCD_PrintU16(96,2,AD[3]);
LCD_PrintFloat(50,4,g_fCarAngle);
}
}
}
/**********************中断服务程序*******************/
void PIT0_IRQHandler(void)
{
DisableInterrupts;
if(jishu>=8000) //干簧管检测延时
{
jishu=8000;
led(LED5,LED_ON);
if(key_check(KEY_STOP) == KEY_DOWN) //干簧管停车
{
KEY_START_flag=0; //调参界面复位
Star_flag=50; //电机停转
led(LED4,LED_ON);
}
}
if(KEY_START_flag>=5) //调参界面5开始中断计数，干簧管延时
{jishu++;
}
g_nInterrupt_Count++; //一堆计数
g_nSpeedControlPeriod++;
SpeedControlOutput(); //一堆平滑输出
g_nDirectionControlPeriod++;
DirectionControlOutput() ;
if(g_nInterrupt_Count>=5) //三个环控制
{
GetMotorPulse();
g_nInterrupt_Count=0;
}
else
if(g_nInterrupt_Count==1)
{
Get_AD_data();
Rd_Ad_Value();
}
else
if(g_nInterrupt_Count==2)
{
AngleCalculate();
AngleControl(g_fCarAngle,Gyro_Now);
MotorOutput();
led_turn (LED1);
}
else
if(g_nInterrupt_Count==3)
{
g_nSpeedControlCount++;
if(g_nSpeedControlCount>=20)
{
led_turn (LED2);
SpeedControl();
g_nSpeedControlCount=0;
g_nSpeedControlPeriod=0;
}
}
else
if(g_nInterrupt_Count==4)
{
g_nDirectionControlCount++;
DirectionVoltageSigma();
if(g_nDirectionControlCount>=2)
{
DirectionControl();
g_nDirectionControlCount=0;
g_nDirectionControlPeriod=0;
}
}
PIT_Flag_Clear(PIT0); //清中断标志位
EnableInterrupts;
}

复制代码