main.c

超声波的哦

#define PI 3.1415926535
/******************陀螺仪相关*************************/
float   theta         = 0.0;        // 校正后的陀螺仪角度
float   theta1        = 0.0;        // 校正前的陀螺仪角度
float   Zero          = 0.0;        // 陀螺仪的相对零值
const float	Adjust_L = 360 / 358.4;	// 左转校正值
const float	Adjust_R = 360 / 360;	// 右转校正值

/******************PID控制相关*************************/
fp32   adjust             = 0;		// 调整量
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;		// 当前左轮应设速度
INT32S RightMotorSpeed    = 0;		// 当前右轮应设速度
INT32S MaxSpeed    = 7500;
INT32S MinSpeed    = 500;//500;

/******************各目标角度**************************/
float TargetAngle1 = -27;			// 起始目标角度	
float TargetAngle2 = -180;  		// 到位目标角度

float firstAngle  = -25.5;//-26.5;	// 方案 0，1，2 的第一个角度
float secondAngle = -58;	// 方案 0 的第二个角度
   
/***************************************************************************
***复位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;
}
	
/*********************************************************************
***小车右转弯***
**********************************************************************/
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) // 设置到位的时间
		{
			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  到要转过的角度则停止转动
		{
			Epos_SetSpeed(&eposL, 0);
			Epos_SetSpeed(&eposR, 0);
			
			break;
		}
		else						// 往右转不够
		{
			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);	
	}
}

/*********************************************************************
***小车左转弯***
**********************************************************************/
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) // 设置到位的时间
		{
			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)	// 到要转过的角度则停止转动
		{
			Epos_SetSpeed(&eposL, 0);
			Epos_SetSpeed(&eposR, 0);
			
			break;
		}
		else						// 往右转不够
		{
			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);	
	}
}

/*********************************************************************
***小车去取对方白块时的左转弯：左轮不动，右轮正转***
**********************************************************************/
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) // 设置到位的时间
		{
			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)	// 到要转过的角度则停止转动
		{
			Epos_SetSpeed(&eposL, 0);
			Epos_SetSpeed(&eposR, 0);
			
			break;
		}
		else						// 往右转不够
		{
			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);	
	}
}

/*********************************************************************
***小车差速原地转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) // 设置到位的时间
		{
			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)	// 到要转过的角度则停止转动
		{
			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) // 设置到位的时间
		{
			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) // 设置到位的时间
		{
			Epos_SetSpeed(&eposL, 0);
			Epos_SetSpeed(&eposR, 0);
			
			break;
		}
		
		temp = gpio_get(SWITCH_Arrive);
		
		if (temp == 0)				  // 检测到限位开关立即停
		{
			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);
	}
}