balance

//-----------------------------------------------------------------------------
// PWM計算函數
//-----------------------------------------------------------------------------
void PWM_calculate(void)
{	
	if(fabs(angle)>0.17)	HB_Angle = 0;			//如果車體角度大於10°,禁止前後移動

	Wheel_angle_dot = (Lwheel_Counter + Rwheel_Counter)*0.5;		//取編碼平均值
	Wheel_angle_dot_filter *= 0.85;									//	 0.85-|
	Wheel_angle_dot_filter += Wheel_angle_dot*0.15;					//	 0.15-|-編碼濾波

	Wheel_angle += Wheel_angle_dot_filter;			//統計編碼器數值，爲下面的計算做準備
	Wheel_angle_dot = Wheel_angle_dot_filter;		//賦值給車輪角速度變量，爲下面的計算做準備

	Wheel_angle += HB_Angle/9.5;					//根據接收到無線模塊的數據改變車的前後移動
				
	if (Wheel_angle<-768)		Wheel_angle=-768;	//
	else if  (Wheel_angle>768)	Wheel_angle=768;	//防止位置誤差過大導致的不穩定

	if(fabs(angle)<0.78)							//控制在車體角度小於45°纔有PWM輸出
	{
		PWM_cal = kf_1*Wheel_angle*0.008*21.25		+	//車輪角度						  -|
				  kf_2*angle*21.25					+ 	//車體角度(加速度測量值處理後)	  -|
				  kf_3*Wheel_angle_dot*0.818*21.25	+	//車輪角速度					  -|
				  kf_4*angle_dot*21.25;					//車體角速度(陀螺儀測量值處理後)  -|-弧度制單位
	}
	else	
	{
		PWM_cal = 0;
		LR_Angle= 0;
	}	

/**********************************************************************************************************/
//****		上式中		Wheel_angle * 0.008;		//計算車輪角度(0.008 = 2π/(64*12))				  ****//
//****					Wheel_angle_dot*0.818		//計算車輪角速度(0.818 = 100*2*π/(64*12))		  ****//
//****		分別乘以21.25	原因如下：																  ****//
//****					PWM_cal*= 255/12;			//PWM輸出爲255時 對應 的輸出電壓值12			  ****//
//****												//PWM輸出爲X  時 對應 的輸出值爲PWM_cal，求X	  ****//
/**********************************************************************************************************/
	LW_PWM	=  PWM_cal - LR_Angle;					//根據接收到無線模塊的數據改變車的轉彎
	RW_PWM	= -PWM_cal - LR_Angle;					//由於電機安裝是一正一反，故PWM輸出相反
}

ISR(TIMER0_COMP_vect)//10ms
{
	//TIMSK &=~0x02;//輸出比較匹配使能
	sei();

	gyro=ADport(0);	
	acc=ADport(1);			//角度校正
		
	//n0=648000/period0;//rp60s
	//n1=648000/period1;
	
	//n0=1080/period0;//rp100ms
	//n1=1080/period1;
	if(f_refresh0 || f_refresh1)
	{
		f_refresh0=FALSE;
		f_refresh1=FALSE;
		speed=(1080.0/period0+1080.0/period1)*0.5;//最大15
		if(dir0==POS)
		{
			speed=speed;
			//PORTD &=0x7f;
		}
		else
		{
			speed=-speed;
			//PORTD |=0x80;
		}
	}
	else
		speed=0;
	speed_filter*=0.8282;		//車輪速度濾波0.7304
	speed_filter+=speed*0.1718;//0.2696  

	position+=speed_filter;

	if(!f_ok)
	{
		if(f_forward)
			position+=0.4;
		else if(f_backward)
			position-=0.6;
	}
	else
	{
		f_ok=FALSE;
		f_forward=FALSE;
		f_backward=FALSE;
	}

	if(position>300)
		position=300;
	else if(position<-300)
		position=-300;
	/*
	if(++ctr_pos==2)
	{
		ctr_pos=0;

		temp1=speed_filter*10+150;
		Putc(0xfd);
		Putc(temp1/256);
		Putc(temp1%256);
	}
	*/
	acc=acc-1650;
	acc=acc/800;//*100 求出多少g
	if(acc>1)
		acc=1;
	else if(acc<-1)
		acc=-1;
	acc=180/3.1415*asin(acc);	
	w=(gyro-1178)/0.67;//1169	
	Kalman_Filter(acc,w);

	if(angle>30 || angle<-30)
	{
		PORTB |=1<<4;
		PORTB |=1<<5;
		PORTB |=1<<6;
		PORTB |=1<<7;
		OCR1A=0;
		OCR1B=0;
		return;
	}
	
	ut=Kp_angle*angle + Kd_angle*angle_dot - Kp_speed*speed_filter - Ki_speed*position;
	left_sp=ut;
	right_sp=ut;

	if(f_right)
	{
		PORTA &= ~(1<<4);
		left_sp+=30;
		right_sp-=35;
	}
	else if(f_left)
	{
		PORTA &= ~(1<<4);
		right_sp+=35;
		left_sp-=30;
	}

	if(left_sp>PER)
		left_sp=PER;
	else if(left_sp<-PER)
		left_sp=-PER;

	if(right_sp>PER)
		right_sp=PER;
	else if(right_sp<-PER)
		right_sp=-PER;

	if(left_sp>=0)
	{
		PORTB &=~(1<<4);//正轉,窗戶
		PORTB |=1<<5;
	}
	else
	{
		left_sp=-left_sp;

		PORTB &=~(1<<5);//反轉，人
		PORTB |=1<<4;
	}

	if(right_sp>=0)
	{
		PORTB &=~(1<<6);
		PORTB |=1<<7;
	}
	else
	{
		right_sp=-right_sp;

		PORTB &=~(1<<7);
		PORTB |=1<<6;
	}

	OCR1B=FRICTION+left_sp;
	OCR1A=FRICTION+right_sp;
}

//-------------------------------------------------------
//計算PWM輸出值
//車輛直徑：76mm; 12*64pulse/rev; 1m=3216pulses;
//-------------------------------------------------------	
//static int speed_diff,speed_diff_all,speed_diff_adjust;
//static float K_speed_P,K_speed_I;
static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
static float position,position_dot;
static float position_dot_filter;
static float PWM;
static int speed_output_LH,speed_output_RH;
static int Turn_Need,Speed_Need;
//-------------------------------------------------------	
void PWM_calculate(void)	
{
	if ( 0==(~PINA&BIT(1)) )	//左轉
	{
		Turn_Need=-40;
	}
	else if ( 0==(~PINB&BIT(2)) ) 	//右轉
	{
		Turn_Need=40;
	}
	else	//不轉
	{
		Turn_Need=0;
	}
	
	if ( 0==(~PINC&BIT(0)) )	//前進
	{
		Speed_Need=-2;
	}
	else if ( 0==(~PINC&BIT(1)) )	//後退 
	{
		Speed_Need=2;
	}
	else	//不動
	{
		Speed_Need=0;
	}
	
	K_angle_AD=ADport(4)*0.007;
	K_angle_dot_AD=ADport(5)*0.007;
	K_position_AD=ADport(6)*0.007;
	K_position_dot_AD=ADport(7)*0.007;

	position_dot=(speed_real_LH+speed_real_RH)*0.5;
	
	position_dot_filter*=0.85;		//車輪速度濾波
	position_dot_filter+=position_dot*0.15;	
	
	position+=position_dot_filter;
	//position+=position_dot;	
	position+=Speed_Need;
	
	if (position<-768)		//防止位置誤差過大導致的不穩定
	{
		position=-768;
	}
	else if  (position>768)
	{
		position=768;
	}
	
	PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD + 
					K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;
	
	
	speed_output_RH = PWM - Turn_Need;
	speed_output_LH = - PWM - Turn_Need ;
	
	/*
	speed_diff=speed_real_RH-speed_real_LH;			//左右輪速差PI控制；
	speed_diff_all+=speed_diff;
	speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2;
	*/
		
	PWM_output (speed_output_LH,speed_output_RH);	
}

/*
-----------------------------------------------------------------------------
Function           : Set_PWM()
limiting and Setting the PWM Signals, and Set the Direction Pins
Date               : 01.08.2006


-----------------------------------------------------------------------------
*/
void Set_PWM()
{
    if(Drive_A >  max_PWM)Drive_A =  max_PWM;       // limiting PWM Signal A
    if(Drive_A < -max_PWM)Drive_A = -max_PWM;       // limiting PWM Signal A

    if(Drive_A < 0)                                 // Check direction
    {
        DriveA = Drive_A * -1;                      // only positive
        CW_CCW_A = 1;                               // CW
    }
    else
    {
        DriveA = Drive_A;
        CW_CCW_A = 0;                               // CCW
    }
    PWM_A = 255 - DriveA;                           // inverse Signal to have a Phase shift from 180?to Signal PWM B

    if(Drive_B >  max_PWM)Drive_B =  max_PWM;       // limiting PWM Signal B
    if(Drive_B < -max_PWM)Drive_B = -max_PWM;       // limiting PWM Signal B
    if(Drive_B < 0)                                 // Check direction
    {
        DriveB = Drive_B * -1;                      // only positive
        CW_CCW_B = 1;                               // CW
    }
    else
    {
        DriveB = Drive_B;
        CW_CCW_B = 0;                               // CCW
    }
    PWM_B = DriveB;                                 // Set PWM
}


/*
-----------------------------------------------------------------------------
Function           : Algorythmus()
Limit top speed by tilting back, Calculate the Driving speed
Date               : 01.08.2006

-----------------------------------------------------------------------------
*/
void Algorythmus()

{        
    Balance_moment =  Angle_Rate*4 +   5 * (Tilt_ANGLE + Anglecorrection);      // calculate balance moment

    Overspeed = lmax(0, Drive_sum - Overspeedlimit);                            // get over speed

    if (Overspeed > 0)
    {
        if(MODE == Run)Overspeed_flag = 1;

        Overspeed_integral = lmin(1000, Overspeed_integral + min(500, Overspeed + 125));   // calculate over speed integral
    }
    else
    {
        Overspeed_flag = 0;
        Overspeed_integral = lmax(0, Overspeed_integral - 100);                 // calculate over speed integral
    }

    Anglecorrection = Overspeed / 200 + Overspeed_integral / 125;               // calculate Angle correction

    Drive_sum += Balance_moment;                                                // calculate Drive_sum

    // limitting
    if(Drive_sum >  Drivesumlimit)Drive_sum = Drivesumlimit;
    if(Drive_sum <- Drivesumlimit)Drive_sum =-Drivesumlimit;

    Drivespeed = Drive_sum / 250 + Balance_moment / 40;                         // calculate Drive speed

}