第六届飞思卡尔电磁组程序

}
else
STEER_ANGLE=STEER_MID;
}
else
{
if(AD[0]>AD[2])
STEER_ANGLE=STEER_MAXR;
else if(AD[0]<AD[2])
goal_speed = Speed_P*error_speed +Speed_I*error_sum_speed+Speed_D*d_error;
//---------------------------------------------------------------//
#include "Init.h"
#include "Debug.h"
#define GO_AHEAD 0x20 //电机正转
#define BACK 0x80 //电机反转
#define M_MAX 3000 //电机最高加速
}
/*****************************************************
函数名称：Delay_1ms(uint i)
入口参数：i
*****************************************************/
void Delay_1ms(uint ms)
void Angle()
{
int ad;
Read_AD();
ad=AD[0]-AD[2];
if(AD[1]>500)
{
if(ad>0)
{
if((ad/8)>140)
{
;;
}
}
/*****************************************************
函数名称：TIMER1_ISR(void) 定时器中断
*****************************************************/
STEER_ANGLE=STEER_MAXL;
else
{
;;
}
}
}
/*****************************************************
函数名称：void PIDCalc(int speed_max)
入口参数：speed_max
电机PID调整
*****************************************************/
void PIDCalc(int speed)
{
error_speed = speed - count; //当前速度与目标速度的偏差
if( goal_speed >= M_MAX )
goal_speed = M_MAX;
if( goal_speed <= M_MIN )
goal_speed = M_MIN;
if(STOP_flag) //起始线停车
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 TIMER1_ISR(void) //timer1的中断向量号为66
{
count=PACNT; //读取外部计数值(编码器值)
ad=AD[2]-AD[0];
if((ad/8)>140)
STEER_ANGLE=STEER_MAXL;
else
STEER_ANGLE=STEER_MID-(ad/8);
函数名称：SCI0_ISR(void) 串口中断
*****************************************************/
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 20 SCI0_ISR(void) //SCI的中断向量号为20
入口参数：无
出口参数：无
说 明：无
修改日期：2011-3-6
*****************************************************/
void Read_AD(void)
{
uchar i;
AD[0]=0;
AD[1]=0;
MOTOR_SPEED=1700;//1200+10*(-error_speed);
STOP_flag=0;
}
else
{
PORTA=GO_AHEAD;
MOTOR_SPEED=goal_speed; //输出转速
{
//PORTA= BACK; //反转
//MOTOR_SPEED=1500;
//Delay_1ms(100);
MOTOR_SPEED=0;
}
else if(error_speed<-9)
{
//PORTA= BACK; //反转
PACNT=0; //寄存器清零
// PIDCalc(speed);
Angle();
PITTF_PTF0=1;
}
#pragma CODE_SEG DEFAULT
/*****************************************************
byte STOP_flag = 0; //电机停止运转标志位
uint count = 0; //编码器计数寄存器
int error_speed = 0; //当前速度与目标速度的偏差
int error_sum_speed = 0; //对每次的速度偏差进行累加
函数名称：main()
*****************************************************/
void main()
{
Init_Dev();
Delay_1ms(3000);
EnableInterrupts;
for(;;)
uint goal_speed = 0; //输出速度值
int speed = 58; //恒速跑，速度设置
/*****************************************************
函数名称：Read_AD(void)
error_sum_speed+=error_speed; //对每次的速度偏差进行累加
d_error=last_error-error_speed; //上次偏差与当前偏差的差值
last_error=error_speed;
if(error_sum_speed>3000)
{
int ii,jj;
if(ms==0)
ms=1;
for(ii=0;ii<ms;ii++)
for(jj=0;jj<2770;jj++);
}
/*****************************************************
error_sum_speed=3000;
if(error_sum_speed<0)
error_sum_speed=0;
if(error_speed<0)
error_speed=0;
if(d_error<0)
d_error=0;
//-----------------------------PI-------------------------------//
STOP_flag=0;
}
}
/*****************************************************
函数名称：Angle()
*****************************************************/
int last_error = 0; //error[-1]上次的当前速度与目标速度的偏差
int d_error = 0; //上次速度偏差与当前速度偏差的差值
//int pre_error = 0; //error[-2]
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include <MC9S12XS128.h>
#define M_MIN 1000 //电机最小加速
#define Speed_P 150 //速度PID比例P
#define Speed_I 6 //速度PID积分I
#define Speed_D 9 //速度PID微分D
AD[2]=0;
for(i=0;i<7;i++) //软件滤波
{
while(!ATD0STAT0_SCF);
AD[0]= ATD0DR0;
AD[1]= ATD0DR2;
AD[2]= ATD0DR4;
STEER_ANGLE=STER_MID+(ad/8);
}
else if(ad<0)
{