main.c

车载硬盘的减振代码

#include <iom88.h>
#include <inavr.h>

//The global varibles define
double Integrator0,Integrator1;
//float CtrlOut00,CtrlOut01,CtrlOut02;
float CtrlOut10,CtrlOut11,CtrlOut12;
float Pos00,Pos11;
float Kp0,Ki0,K01,K02,TarVel0;
float Kp1,Ki1,K11,K12,TarVel1;
float Ts  = 0.002;//The sample time
float k0x,k1x,k0b,k1b;
char Status   =0x01;//1 = Front Moving 2 = Back Moving 3 = Move toward Origin   
char flag	  =0x01;//1 = Above Moving 2 = below moving 3 = Move toward Origin
int k3x = -40,k3b = 1;
int k4x = 40,k4b  = 1;
static float Pos1,Vel1,Pos0,Vel0;
static float Velocity1,Velocity0;
float a1 = 0,a2 = 0;

char index = 0x0;

/* UART Buffer Defines */
#define UART_RX_BUFFER_SIZE 8 /* 1,2,4,8,16,32,64,128 or 256 bytes */
#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1 )
#define UART_TX_BUFFER_SIZE 8 /* 1,2,4,8,16,32,64,128 or 256 bytes */
#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1 )

#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK )
#error RX buffer size is not a power of 2
#endif

/* Static Variables */
static unsigned char UART_RxBuf[UART_RX_BUFFER_SIZE];
static volatile unsigned char UART_RxHead;
static volatile unsigned char UART_RxTail;
static unsigned char UART_TxBuf[UART_TX_BUFFER_SIZE];
static volatile unsigned char UART_TxHead;
static volatile unsigned char UART_TxTail;



//Call this routine to initialize all port 
void port_init(void)
{
 PORTB = 0x00;
 DDRB  = 0x00;
 PORTC = 0x00; //m103 output only
 DDRC  = 0x00;
 PORTD = 0x00;
 DDRD  = 0x00;
}

//call this routine to initialize all peripherals
void init_devices(void)
{
 //stop errant interrupts until set up
 __disable_interrupt(); //disable all interrupts
 port_init();

 MCUCR = 0x00;
 EICRA = 0x00; //extended ext ints
 EIMSK = 0x00;
 
 TIMSK0 = 0x00; //timer 0 interrupt sources
 TIMSK1 = 0x00; //timer 1 interrupt sources
 TIMSK2 = 0x00; //timer 2 interrupt sources
 
 PCMSK0 = 0x00; //pin change mask 0 
 PCMSK1 = 0x00; //pin change mask 1 
 PCMSK2 = 0x00; //pin change mask 2
 PCICR = 0x00; //pin change enable 
 PRR = 0x00; //power controller
 __enable_interrupt(); //re-enable interrupts
 //all peripherals are now initialized
}

/* initialize UART */
void InitUART( unsigned char baudrate )
{
/****************
	unsigned char x;
	UBRR = baudrate;	
	UCR = ( (1<<RXCIE) | (1<<RXEN) | (1<<TXEN) );
	x = 0; 
	UART_RxTail = x;
	UART_RxHead = x;
	UART_TxTail = x;
	UART_TxHead = x;
****************/
}

//Time0 init 
void Timer0Init(int timsk,int tccr0,int tccr1)
{
 unsigned char sreg;
 TIMSK0 = TIMSK0|timsk;
 sreg = SREG;
 OCR0A = tccr0;
 OCR0B = tccr1;
 SREG = sreg;
}

//Get the position 0 
float GetPos0(void)
{
  float ActualPos0;
  //ActualPos0 = ...
  return ActualPos0;
}

//Get the position 1 
float GetPos1(void)
{
  float ActPos1;
  //ActualPos0 = ...
  return ActPos1;
}

//Position sersor table serch
char transform(char hex)
{
 char data;
 switch(hex)
 {
  /*********************************
  case 0x00: data = 0xC0;break;
  ....
  ....
  case 0x0F: data = 0x8E;break;
  *********************************/
 }
 return (data);
}

//Set port number
void SetPortC(int timedelay)
{
  int pp;
  unsigned sreg;
  sreg  = SREG;
  __disable_interrupt();
  DDRC  = 0xFF;
  PORTC = 0x01;
  SREG  = sreg;
}

//Drive motor movements
void Transmit(float u)
{
 
 ;//Output the velocity to control motor velocity;
}

//Caculat the first direct control output
float ControlOut0(float Pos0,float Vel0,float TarVel0)
{
 float 	    Err0 = 0;
 float CtrlOut00 = 0;
 float CtrlOut01 = 0,CtrlOut02 = 0;
 //Caculate current control output varible
 Err0 	   	 = TarVel0 - Vel0;
 CtrlOut00   = Err0*Kp0;
 Integrator0 = Integrator0 + Err0*Ki0;
 CtrlOut01   = CtrlOut00 + Integrator0;
 CtrlOut02   = CtrlOut01*K02 + Pos0*K01;
 return CtrlOut02;
}


//Caculate the second direct control output
float ControlOut1(float Pos1,float Vel1,float TarVel1)
{
 float Err1 = 0;
 float CtrlOut10 = 0;
 float CtrlOut11 = 0,CtrlOut12 = 0;
 //Caculate current control output varible
 Err1 	   	= TarVel1 - Vel1;
 CtrlOut10   = Err1*Kp1;
 Integrator1 = Integrator1 + Err1*Ki1;
 CtrlOut11   = CtrlOut10 + Integrator1;
 CtrlOut12   = CtrlOut11*K12 + Pos1*K11;
 return CtrlOut12; 
}

//First initialize First
void ParaInit0()
{
 Integrator0 = 0;
 Integrator1 = 0;
 CtrlOut10 	 = 0;
 CtrlOut11 	 = 0;
 CtrlOut12 	 = 0;
 Pos00 		 = 0;
 Pos11 		 = 0;
 Kp0 		 = 0;
 Ki0 		 = 0;
 K01 		 = 0;
 K02 		 = 0;
 TarVel0 	 = 0;
 Kp1 		 = 0;
 Ki1 		 = 0;
 K11 		 = 0;
 K12 		 = 0;
 TarVel1 	 = 0;
}

//Parameter initialize Second
void ParaInit()
{	 
 //Init control parameters
 //Front-back direct
 Kp0 		  = 30;     //The proportion parameter
 Ki0 		  = 0.8;	//The integraor parameter
 K01 		  = 1949.1;	//The position feedback const	
 K02 		  = 16.6;	//The velocity feedback const	
 Integrator0  = 0;		//The increase integrator caculate 
 //CtrlOut00 	  = 0,CtrlOut01 = 0,CtrlOut02 = 0;
 //Err0 		  = 0; 	 	//The difference beteew target vel and feedback vel
 Pos00 		  = 0;		//The position get from position sensor 
 TarVel0 	  = 0;		//The current target velocity
 
 //direct 2 parameters
 Kp1 	  	  = 30;		//The same as above
 Ki1          = 0.8;	//	
 K11 		  = 1949.1;	//
 K12 		  = 16.6;	//
 Integrator1  = 0;		//
 CtrlOut10 	  = 0,CtrlOut11 = 0,CtrlOut12 = 0;
 //Err1 		  = 0; 	 	//
 Pos11 		  = 0;		//
 TarVel1 	  = 0;		//
 Ts  		  = 0.002;	//The sample time
 k0b 		  = 0.6;
 k1x 		  = -60;
 k1b 		  = 0.4;
 k0x 		  = -40;
}

int main( void )
{
  //Here function come from icc
  init_devices();
  
  if(MCUSR == 0x08)//watchdog reset flag
 	MCUSR = 0x00;
  if(MCUSR == 0x04)
 	MCUSR = 0x00;
  //insert your functional code here...
  __disable_interrupt();
  //Time0 and time1 init
  //Timer1Init(0x80,0x00,0x01);
  TCCR0A		= 0x00;
  TCCR0B		= 0x04;
  Timer0Init(0x07,0x3F,0x7F);    //clk/8
  WDTCSR 	  	= 0x0F; //Initialize watchdog  
 
  //Init global variables
  ParaInit0();
 
  //Init control parameters
  ParaInit();
 
  __enable_interrupt();
  
  InitUART( 25 ); /* set the baudrate to 9600 bps using a 4MHz crystal */
  //Verify parameters with difference condition
 
  while(1)
  {
  	if(index == 2)
	{
		K01 = UART_TxBuf[1];
		K02 = UART_RxBuf[2];
		K11 = UART_RxBuf[3];
		K12 = UART_RxBuf[4];
		Kp0 = UART_RxBuf[5];
		Ki0 = UART_RxBuf[6];
		Kp1 = UART_RxBuf[7];
		//Ki1 = UART_RxBuf[8];
	}
	
	__clear_watchdog_timer();// Reset watchdog
	//Deal except event
	if(Status == 1)
  		TarVel0 = 0;
	else
		TarVel1 = 0;
	//if(abs(a1) >= 10)
	//	 ;//logfile
        //if(abs(a2) >= 10)
	//         ;//logfile
	
	//Save(logfile);
  } 
  
  return 0;
}

//The next interrupt handler function is trigger by time T0;
//The control output varible is compute here.
#pragma vector=TIMER0_COMPA_vect
__interrupt void T0_CMP_A(void)
{
  __disable_interrupt();
  //Capture two direct positions 
  Pos1 = GetPos1();//The Front direct
 
  //Caculate current velocity
  Vel1 = (Pos1-Pos11)/Ts;
 
  switch(flag)
  {
    case 1:
 	   if(Pos1 >= 0.015)
	   	   TarVel1 = 0;
	   else
	   	   TarVel1 = k0x*Pos1 + k1b;
	   break;
    case 2:
  	   if(Pos1 <= -0.01)
	   	   TarVel1 = 0;
	   else
	   	   TarVel1 = -k1x*Pos1 + k1b;
	   break;
    case 3:
  	   if(Pos1>=0)
  	   	   TarVel1 = k4x*Pos1 + k4b;
	   else
	   	   TarVel1 = k4x*Pos1 + k4b;
	   break;
    default:
  	   break;
  }
 	   
  //Caculate current control output varibles
  Velocity1 = ControlOut1(Pos1,Vel1,TarVel1);
  
  //Drive 2 motors move
  Transmit(Velocity1);
 
  //Save the last time positions
  Pos11 = Pos1;
  __enable_interrupt();
}
 
#pragma vector=TIMER0_COMPB_vect
__interrupt void T0_CMP_B(void)
{
  __disable_interrupt();
  //Capture two direct positions 
  Pos0 = GetPos0();//The Vertical direct
 
  //Caculate current velocity
  Vel0 = (Pos0-Pos00)/Ts;
 
  //Get 2 direct target velocity
  switch(Status)
  {
    case 1:
 	   if(Pos0 >= 0.01)
	   	   TarVel0 = 0;
	   else
	   	   TarVel0 = k0x*Pos0 + k0b;
	   break;
    case 2:
  	   if(Pos0 <= -0.01)
	   	   TarVel0 = 0;
	   else
	   	   TarVel0 = -k0x*Pos0 + k0b;
	   break;
    case 3:
  	   if(Pos0>=0)
	   	   TarVel0 = k3x*Pos0 + k3b;
	   else
	   	   TarVel0 = -k3x*Pos0 +k3b;
	   break;
    default:
  	   break;
  }
  	   
  //Caculate current control output varibles
  Velocity0 = ControlOut0(Pos0,Vel0,TarVel0);
 
  //Drive 2 motors move
  Transmit(Velocity0);
 
  //Save the last time positions
  Pos00 = Pos0;
  __enable_interrupt();
}