注册会员
- 积分
- 105
- 威望
- 57
- 贡献
- 34
- 兑换币
- 36
- 注册时间
- 2018-8-9
- 在线时间
- 7 小时
|
直立车,不用printf就会死 while(1)
{
if(flag==4)
{
MPU6050ReadAcc(Accel);
MPU6050ReadGyro(Gyro);
acceleration0=acosf(((double)Accel[2])/15788.0)*57.3;
acceleration1=(double)Accel[0]/(double)15788*165.6/3.1415+11;
if(acceleration1<45.0)acceleration2=acceleration1;
else acceleration2=acceleration0;
angular_acceleration=angular-((double)Gyro[1]*2000.0/32763.0*0.005);
angular=acceleration2*0.05+angular_acceleration*0.95;
rIn = angular; // Read Input
rOut = PIDCalc ( &sPID,rIn ); // Perform PID Interation
;
if(rOut>0)
{
if(rOut>6400)rOut=6400;
left_b=0;
left_a=450+(int16_t)rOut;
right_b=500+(int16_t)rOut;
right_a=0;
}
else
{
if(rOut<-6400)rOut=-6400;
left_a=0;
left_b=450-(int16_t)rOut;
right_a=500-(int16_t)rOut;
right_b=0;
}
printf("\r\n");
TIM3_Mode_Config(left_a,left_b,right_a,right_b);
flag=0;
}
|
|