//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);
}
}
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;
}