예제 #1
0
파일: app.c 프로젝트: FanHongchuang/Tina
//static int Line=0;
static void App_GLWPTask(void *pdata)
{
	(void)pdata;
	
	//开机校准
	if((Get_Angle(adc_10,ADC_Channel_10,&LeftKnee) != 0) || (Get_Angle(adc_11,ADC_Channel_11,&RightKnee) != 0))
		{
				  //开机初始化状态
// 				robotMode=Forward;
// 				actionFlag = 1;//1:执行摆臂动作 0:不执行摆臂动作
// 			
// 				robotMode=ToStop;
// 				calibrationLFlag = 0;
// 				calibrationRFlag = 0;
// 			
//   			OSSemPost(ActionSem);
// 			  robotMode=ToStopBackward;
//			  OSSemPost(BackwardSem);
			
			
		}
		
	while (1)
	{		
		//获取左膝当前位置
		positionArryR[i++] = myRobot.actionctrl.leftLeg.positionNow_Float  = Get_Angle(adc_10,ADC_Channel_10,&LeftKnee);
		
		//获取右膝当前位置
		positionArryL[j++] = myRobot.actionctrl.rightLeg.positionNow_Float = Get_Angle(adc_11,ADC_Channel_11,&RightKnee);
		
		//获取腰部当前位置
		WaistPosition = myRobot.actionctrl.waist.positionNow_Float = Get_Angle(adc_12,ADC_Channel_12,&Waist);
		
// 		printf("%d:  L: %f  R: %f \n",Line++,positionArryR[i-1],positionArryL[j-1]);
		if(i == 500)
		{
			i = 0;
		}
		if(j == 500)
		{
			j = 0;
		}
		
		//腰部复位
   	resetWaist(WaistPosition);
		
		//每2ms检测一次
 		OSTimeDlyHMSM(0,0,0,2);
	}
}
예제 #2
0
int TIM1_UP_IRQHandler(void)  
{
	static int SysTime = 0;
	if(TIM1->SR&0X0001)//5ms定时中断
	{   
		  TIM1->SR&=~(1<<0);                                       //===清除定时器1中断标志位		 
		  Flag_Target=!Flag_Target;
		  if(Flag_Target==1)                                       //5ms读取一次陀螺仪和加速度计的值,更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
			{
			Get_Angle(Way_Angle);                                    //===更新姿态	
			return 0;	
			}                                                        //10ms控制一次,为了保证M法测速的时间基准,首先读取编码器数据
		//	Encoder_Left=Read_Encoder(2);                           //===读取编码器的值,
		//	Encoder_Right=Read_Encoder(3);                           //===读取编码器的值
	  	Get_Angle(Way_Angle);                                    //===更新姿态	
  		Led_Flash(100);                                          //===LED闪烁;指示单片机正常运行	
			Key();                                                   //===扫描按键状态 单击双击可以改变小车运行状态
 	//		Balance_Pwm =balance(Angle_Balance,Gyro_Balance);        //===平衡PID控制	
	//	  Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);       //===速度环PID控制	 记住,速度反馈是正反馈,就是小车快的时候要慢下来就需要再跑快一点
 	//    Turn_Pwm    =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制     
 	//	  Moto1=Balance_Pwm+Velocity_Pwm+Turn_Pwm;                 //===计算左轮电机最终PWM
 	//  	Moto2=Balance_Pwm+Velocity_Pwm-Turn_Pwm;                 //===计算右轮电机最终PWM
   		SysTime += 10;
		//	Moto1 = PWMOut(waveOut( 0.5, 1, 0,  0, SysTime) );
		//	Moto2 = PWMOut(waveOut( 0.5, 1, (30/180*PI),  0, SysTime));
			Moto1 = PWMOut(&motorCMD[0],SysTime);
			Moto2 = PWMOut(&motorCMD[1],SysTime);
			Moto3 = PWMOut(&motorCMD[2],SysTime);
			Moto4 = PWMOut(&motorCMD[3],SysTime);
		//	Xianfu_Pwm();                                            //===PWM限幅
  //    if(Turn_Off(Angle_Balance,Voltage)==0)   //===如果不存在异常
//printf("PWMOUT A : %d",Moto1 );	
//printf("PWMOUT B : %d",Moto2 );
		//	if(SysTime%1000==0)printf("PWMOUT A : %d",Moto1 );
 			Set_Pwm(Moto1,Moto2,Moto3,Moto4);                                    //===赋值给PWM寄存器  
	}       	
	 return 0;	  
}