?? main.c
字號(hào):
#define PI 3.1415926535
/******************陀螺儀相關(guān)*************************/
float theta = 0.0; // 校正后的陀螺儀角度
float theta1 = 0.0; // 校正前的陀螺儀角度
float Zero = 0.0; // 陀螺儀的相對(duì)零值
const float Adjust_L = 360 / 358.4; // 左轉(zhuǎn)校正值
const float Adjust_R = 360 / 360; // 右轉(zhuǎn)校正值
/******************PID控制相關(guān)*************************/
fp32 adjust = 0; // 調(diào)整量
fp32 erro_l_r_d = 0;
fp32 erro_l_r_d_last = 0;
fp32 erro_l_r = 0;
fp32 erro_l_r_last = 0;
fp32 erro_l_r_last_last = 0;
INT32S LeftMotorSpeed = 0; // 當(dāng)前左輪應(yīng)設(shè)速度
INT32S RightMotorSpeed = 0; // 當(dāng)前右輪應(yīng)設(shè)速度
INT32S MaxSpeed = 7500;
INT32S MinSpeed = 500;//500;
/******************各目標(biāo)角度**************************/
float TargetAngle1 = -27; // 起始目標(biāo)角度
float TargetAngle2 = -180; // 到位目標(biāo)角度
float firstAngle = -25.5;//-26.5; // 方案 0，1，2 的第一個(gè)角度
float secondAngle = -58; // 方案 0 的第二個(gè)角度
/***************************************************************************
***復(fù)位PID控制的全局變量***
****************************************************************************/
void ClearData(void)
{
adjust = 0;
erro_l_r_d = 0;
erro_l_r_d_last = 0;
erro_l_r = 0;
erro_l_r_last = 0;
erro_l_r_last_last = 0;
RightMotorSpeed = 0;
LeftMotorSpeed = 0;
}
/*********************************************************************
***小車右轉(zhuǎn)彎***
**********************************************************************/
void TurnToRight(int32 time, float angle)
{
float Angle = 0.0;
int32 CurrentTime = 0;
int32 timedifference = 0;
ClearData();
Angle = theta - angle;
if (Angle < -180)
{
Angle += 360;
}
Angle = fabs(Angle) ;//* 0.5;
CurrentTime = nowtime;
while (1)
{
timedifference = nowtime - CurrentTime;
if(timedifference >= time) // 設(shè)置到位的時(shí)間
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
erro_l_r = theta - angle;
if (erro_l_r < -180)
{
erro_l_r += 360;
}
if (erro_l_r > 25)//30
{
Epos_SetSpeed(&eposL, MaxSpeed*0.27);//0.4
Epos_SetSpeed(&eposR, 0);
}
else if (erro_l_r <= 1.0) // 1.0 到要轉(zhuǎn)過的角度則停止轉(zhuǎn)動(dòng)
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
else // 往右轉(zhuǎn)不夠
{
LeftMotorSpeed = MaxSpeed * 0.25 * erro_l_r / 25;//30;//MaxSpeed * 0.4 * erro_l_r / 30;
if (LeftMotorSpeed > MaxSpeed * 0.25)//MaxSpeed*0.4)
{
LeftMotorSpeed = MaxSpeed * 0.25;//MaxSpeed*0.4;
}
else if (LeftMotorSpeed < MinSpeed*0.1)
{
LeftMotorSpeed = MinSpeed*0.1;
}
Epos_SetSpeed(&eposL, LeftMotorSpeed);
Epos_SetSpeed(&eposR, 0);
}
erro_l_r_last = erro_l_r;
OSTimeDly(1);
}
}
/*********************************************************************
***小車左轉(zhuǎn)彎***
**********************************************************************/
void TurnToLeft(int32 time, float angle)
{
float Angle = 0.0;
int32 CurrentTime = 0;
int32 timedifference = 0;
ClearData();
Angle = -(theta - angle);
if (Angle < -180)
{
Angle += 360;
}
Angle = fabs(Angle) ;
CurrentTime = nowtime;
while (1)
{
timedifference = nowtime - CurrentTime;
if(timedifference >= time) // 設(shè)置到位的時(shí)間
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
erro_l_r = -(theta - angle);
if (erro_l_r <= -180)
{
erro_l_r += 360;
}
if (erro_l_r > 25)
{
Epos_SetSpeed(&eposL, -MaxSpeed*0.27);
Epos_SetSpeed(&eposR, 0);
}
else if (erro_l_r <= 1.0) // 到要轉(zhuǎn)過的角度則停止轉(zhuǎn)動(dòng)
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
else // 往右轉(zhuǎn)不夠
{
LeftMotorSpeed = MaxSpeed * 0.25 * erro_l_r / 25;
if (LeftMotorSpeed > MaxSpeed*0.25)
{
LeftMotorSpeed = MaxSpeed*0.25;
}
else if (LeftMotorSpeed < MinSpeed*0.1)
{
LeftMotorSpeed = MinSpeed*0.1;
}
{
Epos_SetSpeed(&eposL, -LeftMotorSpeed);
Epos_SetSpeed(&eposR, 0);
}
}
erro_l_r_last = erro_l_r;
OSTimeDly(1);
}
}
/*********************************************************************
***小車去取對(duì)方白塊時(shí)的左轉(zhuǎn)彎：左輪不動(dòng)，右輪正轉(zhuǎn)***
**********************************************************************/
void TurnToLeft1(int32 time, float angle)
{
float Angle = 0.0;
int32 CurrentTime = 0;
int32 timedifference = 0;
ClearData();
Angle = -(theta - angle);
if (Angle < -180)
{
Angle += 360;
}
Angle = fabs(Angle) ;//* 0.5;
CurrentTime = nowtime;
while (1)
{
timedifference = nowtime - CurrentTime;
if(timedifference >= time) // 設(shè)置到位的時(shí)間
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
erro_l_r = -(theta - angle);
if (erro_l_r <= -180)
{
erro_l_r += 360;
}
if (erro_l_r > 25)
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, -MaxSpeed*0.27);
}
else if (erro_l_r <= 1.0) // 到要轉(zhuǎn)過的角度則停止轉(zhuǎn)動(dòng)
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
else // 往右轉(zhuǎn)不夠
{
LeftMotorSpeed = MaxSpeed * 0.25 * erro_l_r / 25;
if (LeftMotorSpeed > MaxSpeed*0.25)
{
LeftMotorSpeed = MaxSpeed*0.25;
}
else if (LeftMotorSpeed < MinSpeed*0.1)
{
LeftMotorSpeed = MinSpeed*0.1;
}
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, -LeftMotorSpeed);
}
}
erro_l_r_last = erro_l_r;
OSTimeDly(1);
}
}
/*********************************************************************
***小車差速原地轉(zhuǎn)180度***
**********************************************************************/
void TurnToBack(int32 time)
{
float Angle = 0.0;
int32 CurrentTime = 0;
int32 timedifference = 0;
int8 turnflag = 0;
ClearData();
Angle =-180;
if (theta >= -180 && theta <0)
{
turnflag = 1;
}
else
{
turnflag = -1;
}
CurrentTime = nowtime;
while (1)
{
timedifference = nowtime - CurrentTime;
if(timedifference >= time) // 設(shè)置到位的時(shí)間
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
erro_l_r = fabs(theta - Angle);
if (erro_l_r > 30)
{
Epos_SetSpeed(&eposL, turnflag*MaxSpeed*0.125);
Epos_SetSpeed(&eposR, turnflag*MaxSpeed*0.125);
}
else if (erro_l_r <= 1.0) // 到要轉(zhuǎn)過的角度則停止轉(zhuǎn)動(dòng)
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
else
{
LeftMotorSpeed = MaxSpeed * 0.125 * erro_l_r / 30;
if (LeftMotorSpeed > MaxSpeed*0.125)
{
LeftMotorSpeed = MaxSpeed*0.125;
}
else if (LeftMotorSpeed < MinSpeed*0.1)
{
LeftMotorSpeed = MinSpeed*0.1;
}
Epos_SetSpeed(&eposL, turnflag*LeftMotorSpeed);
Epos_SetSpeed(&eposR, turnflag*LeftMotorSpeed);
}
erro_l_r_last = erro_l_r;
OSTimeDly(1);
}
}
/*********************************************************************
***小車后退***
**********************************************************************/
void BackCar(int time)
{
uint32 CurrentTime = 0;
uint32 timedifference =0;
CurrentTime = nowtime;
while (1)
{
timedifference = nowtime - CurrentTime;
if(timedifference >= time) // 設(shè)置到位的時(shí)間
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
Epos_SetSpeed(&eposL,-MaxSpeed*0.36);
Epos_SetSpeed(&eposR,MaxSpeed*0.36);
}
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
}
/*********************************************************************
***小車走直線***
**********************************************************************/
void GoStraight(int32 time1, float angle1)
{
fp32 Ud = 0;
fp32 Ud_last = 0;
uint8 coefficient = 1;
int32 CurrentTime1 = 0;
int32 timedifference1 = 0;
uint8 temp = 0;
uint8 flag_adjust = 0;
ClearData();
CurrentTime1 = nowtime;
while (1)
{
timedifference1 = nowtime - CurrentTime1;
if (timedifference1 >= time1) // 設(shè)置到位的時(shí)間
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
temp = gpio_get(SWITCH_Arrive);
if (temp == 0) // 檢測(cè)到限位開關(guān)立即停
{
Epos_SetSpeed(&eposL, 0);
Epos_SetSpeed(&eposR, 0);
break;
}
erro_l_r_last_last = erro_l_r_last;
erro_l_r_last = erro_l_r;
erro_l_r = theta - angle1;
// 修正erro_l_r
if (erro_l_r < -180)
{
erro_l_r += 360;
}
erro_l_r_d_last = erro_l_r_d;
erro_l_r_d = erro_l_r - erro_l_r_last;
Ud = 0.5 * Ud_last + 10 * 0.5 * erro_l_r_d;
if (RightMotorSpeed >= MaxSpeed || LeftMotorSpeed >= MaxSpeed)
{
if(erro_l_r >= 0)
coefficient = 0;
else
coefficient = 1;
}
else if (RightMotorSpeed <= MinSpeed || LeftMotorSpeed <= MinSpeed)
{
if(erro_l_r <= 0)
coefficient = 0;
else
coefficient = 1;
}
else
{
coefficient = 1;
}
if (erro_l_r * erro_l_r_d > 0 || erro_l_r_d == 0)
{
adjust += MaxSpeed * 0.018 * (erro_l_r - erro_l_r_last)+ (Ud - Ud_last);
}
else
{
adjust = adjust;
}
Ud_last = Ud;
//最后1.3秒到0.5秒減速
if(timedifference1 < (time1 - 120) && timedifference1 >= (time1-130))
{
if (flag_adjust == 0)
{
flag_adjust = 1;
}
Epos_SetSpeed(&eposL, (MaxSpeed + adjust)*0.922);//
Epos_SetSpeed(&eposR, -(MaxSpeed - adjust)*0.937);//
continue;
}
else if(timedifference1 < (time1 - 110) && timedifference1 >= (time1-120))
{
Epos_SetSpeed(&eposL, (MaxSpeed + adjust)*0.844);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.859);//
continue;
}
else if(timedifference1 < (time1 - 100) && timedifference1 >= (time1-110))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.766);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.781);//
continue;
}
else if(timedifference1 < (time1 - 90) && timedifference1 >= (time1-100))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.688);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.703);//
continue;
}
else if(timedifference1 < (time1 - 80) && timedifference1 >= (time1-90))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.61);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.625);//
continue;
}
else if(timedifference1 < (time1 - 70) && timedifference1 >= (time1-80))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.532);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.547);//
continue;
}
else if(timedifference1 < (time1 - 60) && timedifference1 >= (time1-70))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.454);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.469);//
continue;
}
else if(timedifference1 < (time1 - 50) && timedifference1 >= (time1-60))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.376);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.391);//
continue;
}
else if(timedifference1 < (time1 - 40) && timedifference1 >= (time1-50))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.298);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.313);//
continue;
}
else if(timedifference1 < (time1 - 30) && timedifference1 >= (time1-40))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.22);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.235);//
continue;
}
else if(timedifference1 < (time1 - 20) && timedifference1 >= (time1-30))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.142);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.157);//
continue;
}
else if(timedifference1 < (time1 - 10) && timedifference1 >= (time1-20))
{
Epos_SetSpeed(&eposL, (MaxSpeed+ adjust)*0.064);//
Epos_SetSpeed(&eposR, -(MaxSpeed-adjust)*0.079);//
continue;
}
else if(timedifference1 < time1 && timedifference1 >= (time1-10))
{
Epos_SetSpeed(&eposL, 0);//+ adjust
Epos_SetSpeed(&eposR, 0);//-adjust
continue;
}
if ((fabs(adjust) > MaxSpeed) && (adjust < 0))
{
LeftMotorSpeed = 0;
RightMotorSpeed = -(MaxSpeed - adjust);
}
else if ((fabs(adjust) > MaxSpeed*0.99) && (adjust > 0))
{
LeftMotorSpeed = (MaxSpeed + adjust)*0.99;
RightMotorSpeed = 0;
}
else
{
LeftMotorSpeed = (MaxSpeed + adjust)*0.99;
RightMotorSpeed = -(MaxSpeed - adjust);
}
if (LeftMotorSpeed > MaxSpeed)
{
LeftMotorSpeed = MaxSpeed;
}
else if (LeftMotorSpeed < MinSpeed)
{
LeftMotorSpeed = MinSpeed;
}
if (RightMotorSpeed < -MaxSpeed)
{
RightMotorSpeed = -MaxSpeed;
}
else if (RightMotorSpeed > -MinSpeed)
{
RightMotorSpeed = -MinSpeed;
}
Epos_SetSpeed(&eposL, LeftMotorSpeed);
Epos_SetSpeed(&eposR, RightMotorSpeed);
OSTimeDly(1);
}
}?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -