// edited by matthew, May. 30, 2007

/* MCU2的程序
// 功能：
//     1. 接收上位機(jī)的數(shù)據(jù)：方向、速度、位移、Kp、Ki、Kd
//     2. 進(jìn)行閉環(huán)控制(PID控制)
//     3. 向MCU1發(fā)送閉環(huán)控制后的PWM占空比（脈寬）；向上位機(jī)回傳當(dāng)前速度、位移，用圖形顯示
*/

#include <iom8v.h>
#include <macros.h>

#include "v_pid.h" // 進(jìn)行PID控制的函數(shù)在此定義

//#define FORWARD    	 	0x01
//#define BACKWARD   	 	0x00

#define SendStart  	 	0x01	// 發(fā)送開始
#define SendOver   	 	0x00	// 發(fā)送結(jié)束

#define LENGTH     	 	7		// 下位機(jī)發(fā)送位數(shù)為7+1
#define MIDDISTANCE  	65536L	// 初始位移
#define DeadError       50L		// 死區(qū)范圍,當(dāng)目標(biāo)位置小于DeadError時(shí)，電機(jī)停轉(zhuǎn)。

unsigned char VelSendFlag=SendOver,		// 向下位機(jī)發(fā)送數(shù)據(jù)開始標(biāo)志
		      uart_rece_flag=0;			// 接收上位機(jī)數(shù)據(jù)完成標(biāo)志
unsigned char dir_flag= FORWARD;	    // 電機(jī)轉(zhuǎn)動(dòng)方向標(biāo)志
unsigned char UartSendData[10]={0},	    // 發(fā)送給下位機(jī)和PC的數(shù)據(jù)
		      Rec_data[10]={0};			// 從PC接收到的數(shù)據(jù)
signed 	 int  Num_Speed = 0;			//  記錄碼盤返回的當(dāng)前速度脈沖數(shù)
signed 	 int  temp_sint=0;				// 控制電機(jī)用的PWM值，對(duì)應(yīng)占空比
long     TargetPosition;				// 目標(biāo)位移
long     distance=MIDDISTANCE;		    // 已走位移

void port_init(void)
{
 PORTB = 0x00; //0000 0110
 DDRB  = 0x00; //0000 0110
 PORTC = 0x0C; //pc2,pc3--1,out
 DDRC  = 0x0C;
 PORTD = 0x20;
 DDRD  = 0x00;
}


//TIMER0 initialize - prescale:1024
// desired value: 20mSec
void timer0_init(void)
// timer0 20ms中斷初始化，采樣周期T=20ms
{
 TCCR0 = 0x00; //stop
 TCNT0 = 0x29; //set count
 TCCR0 = 0x05; //start timer
}


#pragma interrupt_handler timer0_ovf_isr:10
void timer0_ovf_isr(void)
// 名稱：20ms溢出中斷服務(wù)程序
// 實(shí)現(xiàn)功能：
//       1. 調(diào)用閉環(huán)控制函數(shù)
//       2. 向MCU1發(fā)送電機(jī)控制的PWM波占空比；同時(shí)，向上位機(jī)發(fā)送當(dāng)前的速度值和位移值
{
 TCNT0     = 0x29;       //reload counter value
 Num_Speed = TCNT1;  	 //得到T1反饋回的20ms內(nèi)的編碼器脈沖數(shù)
 TCNT1H    = 0x00;		 //計(jì)數(shù)器1清0
 TCNT1L    = 0x00;
 
 distance+=Num_Speed;		// 更新已走位移
 TargetPosition-=Num_Speed;		// 更新剩余位移

 // ************ 需要調(diào)整這里的移位數(shù)，因?yàn)樯胁恢离姍C(jī)最大轉(zhuǎn)速下，每個(gè)周期的脈沖數(shù) *************//
 // 測(cè)得電機(jī)在最大轉(zhuǎn)速下，該脈沖數(shù)字在350~450之間
 Num_Speed>>=1;		// 速度脈沖數(shù)/2， 折算到0~255

 sPID.vi_FeedBack = Num_Speed;		// 儲(chǔ)存當(dāng)前速度值,速度最大就是255，再大就不可能實(shí)現(xiàn)

 //************************ 在這里調(diào)用PID函數(shù)***********************//
 temp_sint = v_PIDCalc(&sPID);			// 計(jì)算得到控制的PWM值,在10到550之間,對(duì)應(yīng)的是占空比
 // 550 是由下位機(jī)中的ICR1設(shè)置的，對(duì)應(yīng)PWM方式14的TOP值

 if(TargetPosition<=DeadError)		// 處理進(jìn)入死區(qū)的情況
 {
  temp_sint=1;
 }
 
 //更新要發(fā)送的數(shù)據(jù)
 if(dir_flag==BACKWARD)
 {
 UartSendData[1]=temp_sint>>8;
 UartSendData[1]+=1<<7;//置方向位，為1為反轉(zhuǎn)
 UartSendData[2]=temp_sint;
 }
 else if(dir_flag==FORWARD)
 {
 UartSendData[1]=temp_sint>>8;
 UartSendData[2]=temp_sint;
 }
 else 
 {
 UartSendData[1]=UartSendData[2]=0;
 }
 
 UartSendData[3]=Num_Speed>>8;
 UartSendData[4]=Num_Speed;

 UartSendData[5]=distance>>16;
 UartSendData[6]=distance>>8;
 UartSendData[7]=distance;
 
 //置發(fā)送標(biāo)志位
 VelSendFlag=SendStart;
  
}

//TIMER1 initialize - prescale:Rising edge
// WGM: 0) Normal, TOP=0xFFFF
// desired value: 1Hz
// actual value: Out of range
void timer1_init(void)
// timer1用來記錄一個(gè)20ms周期內(nèi)，從編碼盤得到的脈沖數(shù),采用計(jì)數(shù)方式
{
 TCCR1B = 0x00; //stop
 TCNT1H = 0x00 /*INVALID SETTING*/; //setup
 TCNT1L = 0x00 /*INVALID SETTING*/;
 TCCR1B = 0x07; //start Timer
}


//UART0 initialize
// desired baud rate: 9600
// actual: baud rate:9600 (0.0%)
// char size: 8 bit
// parity: Disabled
void uart0_init(void)
{
 UCSRB = 0x00; //disable while setting baud rate
 UCSRA = 0x00;
 UCSRC = BIT(URSEL) | 0x06;
 UBRRL = 0x47; //set baud rate lo
 UBRRH = 0x00; //set baud rate hi
 UCSRB = 0x98;
}


#pragma interrupt_handler uart0_rx_isr:12
void uart0_rx_isr(void)
// 從上位機(jī)接收數(shù)據(jù)的中斷
{
 //uart has received a character in UDR
 static unsigned char data,rece_num=0,i=0;
 data=UDR;
 if((data==0xAA)&&(rece_num==0))//判斷第1字節(jié)，0XAA起始位判斷
     {
	 TIMSK = 0x00; //timer interrupt sources
	 Rec_data[rece_num]=data;
     rece_num++;
     } 
 else if(rece_num>0)//第2字節(jié)到第8字節(jié)寄存到接收數(shù)據(jù)中
   {
	 Rec_data[rece_num]=data;
     rece_num++;
	 if(rece_num==8)//接收完畢，標(biāo)志位置1
      {
	    rece_num=0; //準(zhǔn)備接收下一楨
	    uart_rece_flag=1;
       }
    }
}

#pragma interrupt_handler int0_isr:2
void int0_isr(void)
{
 //external interupt on INT0
}

#pragma interrupt_handler int1_isr:3
void int1_isr(void)
{
 //external interupt on INT1
}

//call this routine to initialize all peripherals
void init_devices(void)
{
 //stop errant interrupts until set up
 CLI(); //disable all interrupts
 port_init();
 timer0_init();
 timer1_init();
 uart0_init();
 MCUCR = 0x0A;
 GICR  = 0xC0;
 TIMSK = 0x05; //timer interrupt sources
 SEI(); //re-enable interrupts
 		//all peripherals are now initialized
}


// *************** main ****************//
void main(void)
{
 char i=0;
 
 init_devices();
 PIDInit();	// 調(diào)用PID參數(shù)初始化函數(shù)
 TargetPosition=0x0000;
 UartSendData[0]=0xAA;		// 發(fā)送到下位機(jī)或PC的起始位
 
 while(1)
 {
  
 //---------------start  if( uart_rece_flag==1)
  if( uart_rece_flag==1)	// 接收完數(shù)據(jù)后進(jìn)行數(shù)據(jù)處理
 	{
	 uart_rece_flag=0;		// 清接收完成標(biāo)志

	 PIDInit();  // 防止每次運(yùn)行時(shí)出現(xiàn)超調(diào)現(xiàn)象, 
	 			 // 因?yàn)樯洗芜\(yùn)行結(jié)束后, sPID.vi_PreU 不為0，各變量均應(yīng)該清0 
	 
	 dir_flag=Rec_data[1];   				   //電機(jī)方向
	 sPID.vi_Ref =Rec_data[2];				   //給定速度大小

	 TargetPosition=Rec_data[3];  			   //目標(biāo)位置
	 TargetPosition<<=12; 
	 distance=MIDDISTANCE;					   //位移 			  

	 sPID.v_Ka=Rec_data[4];					   //參數(shù)Ka = Kp
	 sPID.v_Kb=Rec_data[5];					   //參數(shù)Kb =  T / Ti 
	 sPID.v_Kb=Rec_data[6];					   //參數(shù)Kc = ( Td / T )

	 TIMSK = 0x01; //timer interrupt sources
	 
/*********************目標(biāo)位置處理原理*****************************
設(shè)定值為絲杠轉(zhuǎn)過的轉(zhuǎn)數(shù)，則電機(jī)需要轉(zhuǎn)過的轉(zhuǎn)數(shù)為：設(shè)定轉(zhuǎn)數(shù)×25
則編碼器輸出的脈沖為：設(shè)定轉(zhuǎn)數(shù)×25×500＝設(shè)定轉(zhuǎn)數(shù)×12500
在程序中的處理為：假高設(shè)定轉(zhuǎn)數(shù)為temp，則temp=<<10，即temp=temp×1024。
TargetPosition＝temp<<2;//4倍
TargetPosition+=temp<<3;//8倍
這樣處理之后，TargetPosition的值為：設(shè)定轉(zhuǎn)數(shù)×12×1024＝設(shè)定轉(zhuǎn)數(shù)×12288
（12500－12288）/12500=0.01696=%1.696，誤差非常小。
實(shí)際測(cè)試時(shí)設(shè)定的轉(zhuǎn)數(shù)也能非常準(zhǔn)確的到達(dá)，表明上述誤差可忽略不計(jì)。
*******************************************************************/

    }
 //---------------end  if( uart_rece_flag==1)
 
 
 //---------------start  if(VelSendFlag==SendStart)
 if(VelSendFlag==SendStart)		// 開始發(fā)送數(shù)據(jù)
 {
  VelSendFlag=SendOver;				// 清開始發(fā)送標(biāo)志
  for(i=0;i<=LENGTH;i++)
  {
   while (!(UCSRA&(1<<UDRE)));		// 等待發(fā)送寄存器為空
   UDR=UartSendData[i];						 // 為空時(shí)，發(fā)送下一字節(jié)
  }
 }
 //---------------end  if(VelSendFlag==SendStart)
 }
}