#include <hidef.h> /* common defines and macros */

#include <mc9s12dg128.h> /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"

#define CEN 694 //设定舵机转向中值
#define K1 -8//-11.6 //设定舵机转向系数
#define Umax 900 //设定速度的pwm最大输出
#define Umin 0 //设定速度的pwm最小输出
#define DEL1 10000
#define DEL2 10000
#define DEL3 9880
int num_sam;
int pos,pos_temp;
int outFlag;
int brake,brake1,ms;

double table[17]={8,7,6,5,4,2,0,0,0,0,0,-2,-4,-5,-6,-7,-8};

double angle,angle1,angle2,angle3,angle_last,delta_angle,u,du,u_1,u_2,out;
double counter0,counter1,counter2;
int num_filter;
int counterIC,counterIC_last;
int counter_stop;
double data[5],data_last,time,speed,speedr,speedL,speedM,speedH;
double e_speed,e_speed1,e_speed2,e_angle,e_angle1,e_angle2,Uc,duc,Kp,Kd,Ki;
unsigned int State[2];
int flagSt,flagSt_last,stCounter,lapCounter;


/******************************锁相环升频到24M***********************/
void init_PLL(void)
{
REFDV = 1;
SYNR =2;
while ((CRGFLG&0x08)==0);
CLKSEL|=0x80;
}
/**********************初始化定时器和输入捕捉***********************/
void init_Timer(void) // 定时器初始化
{
TSCR1 =0x80; // 使能主定时器
TSCR2 =0x83; // 使能主定时器溢出中断，8分频
TIOS =0x00; // channel 0 1 2 3 4 5 6 input capture;
TCTL3 =0x04; // rising edge trigle
TIE =0x20; // 捕捉中断5中断使能
}
/**********************************初始化端口***********************/
void init_PORT(void) //端口初始化
{
DDRB=0X00; //低八位 0X00 输入
DDRA=0X00; //高一位 0X00 输入

DDRH=0xF0; //给定速度
PTH_PTH6=0; //给33886 h7 h6输出
PTH_PTH7=1;
/*DDRE|=0X0c;//给33886 E2 E3输出
PORTE|=0X08;//3高
PORTE&=0XFB;//2低 */
}
/******************************PWM初始化*******************************/
void init_PWM(void)
{
PWMPRCLK =0x00; //预分频时不分频
PWMSCLA =24; //SA:24M/24/2
PWMSCLB =24; //SB:24M/24/2

PWMPOL =0xFF;

PWMCLK_PCLK1=1; //电机反转 Select clock SA for the PWM0 1
PWMCTL_CON01=1; //4 5 组合为十六通道
PWMPER01 =1000; //频率=500Hz
PWMDTY01 =0; //Duty rate
PWMCAE_CAE1 =0; //Select left aligned mode
PWME_PWME1 =1;

PWMCLK_PCLK5=1; //电机正转 Select clock