32小车运动_motorpidsetspeed(1,1)什么啥意思-CSDN博客

本文链接：https://blog.csdn.net/m0_73890956/article/details/132216208

一，基本运动


extern float Motor1Speed;
extern float Motor2Speed;
extern tPid	PidMotor1Speed;
extern tPid	PidMotor2Speed;

float MotorSpeedUpCut = 0.5;
//正值向前走
void Motor_Set(int Motor1,int Motor2)
{
	Motor1 = -Motor1;
	Motor2 = -Motor2;
	//1.先根据正负设置方向GPIO 高低电平
	if(Motor1 <0) BIN1_SET;
	else  BIN1_RESET;
	
	if(Motor2 <0) AIN1_SET;
	else AIN1_RESET;
	
	//2.然后设置占空比  
	if(Motor1 <0)
	{
		if(Motor1 <-99) Motor1 =-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+Motor1));
	}
	else 
	{
		if(Motor1 >99) Motor1 = 99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,Motor1);
	}

	if(Motor2<0)
	{
		if(Motor2 <-99) Motor2=-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (100+Motor2));
	}
	else
	{
		if(Motor2 >99) Motor2 =99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, Motor2);
	}


}
/**
  * @brief    	设置Pid的目标速度
  * @param  	电机1的速度(0-10)
  * @param  	电机2的速度(0-10)
  * @retval  
	MotorPidSetSpeed(1,2);	//向右走
	MotorPidSetSpeed(2,1);	//向左走
	MotorPidSetSpeed(1,1);	//向前走
	MotorPidSetSpeed(-1,-1);//向后走
	MotorPidSetSpeed(0,0);	//停止
	MotorPidSetSpeed(-1,1);	//原地转（从上往下看顺时针）
	MotorPidSetSpeed(1,-1);	//原地转（从上往下看逆时针）
  */
void MotorPidSetSpeed(float Motor1SetSpeed,float Motor2SetSpeed)
{
	
	PidMotor1Speed.Target_val = Motor1SetSpeed;
	PidMotor2Speed.Target_val = Motor2SetSpeed;
	//PID计算控制电机
	Motor_Set(PID_realize(&PidMotor1Speed,Motor1Speed),PID_realize(&PidMotor2Speed,Motor2Speed));
}

1，前进

略

2，后退

略

3，左转

略

4，右转

略

二，OLED显示直线速度，路程，和ADC采样的电压

1，速度

        sprintf((char *)OledString1,"V1:%.2fV2:%.2f",Motor1Speed,Motor2Speed);
        //显示两个电机转速 单位：转/秒
		OLED_ShowString(0,0,OledString1,12);

2，路程

        sprintf((char *)OledString1,"Mileage%.2f",Mileage);//计算小车里程 单位cm
	    OLED_ShowString(0,1,OledString1,12);

3，电压

		sprintf((char *)OledString1,"U:%.2fV",AdcGetBatteryVoltage());//显示电池电压
		OLED_ShowString(0,2,OledString1,12);

ADC采样的电压话没有验证（万用表测量）

三，小车红外循迹


	g_ucaHW_Read[0] = READ_HW_OUT_1;
	g_ucaHW_Read[1] = READ_HW_OUT_2;	
	g_ucaHW_Read[2] = READ_HW_OUT_3;
	g_ucaHW_Read[3] = READ_HW_OUT_4;
		  
	if(g_ucaHW_Read[0]== 0&&g_ucaHW_Read[1]==0&&g_ucaHW_Read[2]==0&&g_ucaHW_Read[3]== 0)
	{
		g_cThisState = g_cThisState;	//前运动
	}
		
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 1 && g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3]== 0 )
	{
		g_cThisState = -0.5;	//右边运动
	}
		else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 0 && g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3]== 0 )
	{
		g_cThisState = -1;	//快速右转
	}
	else if(g_ucaHW_Read[0] == 1&&g_ucaHW_Read[1] == 1 && g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3]== 0 )
	{
		g_cThisState = -1.5; 	//更快速右转
	}
		
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0 && g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3]== 0 )
	{
		g_cThisState = 0.5;	//左边运动
	}
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0 && g_ucaHW_Read[2] == 0&&g_ucaHW_Read[3]== 1 )
	{
		g_cThisState = 1;	//快速左转
	}
	else if(g_ucaHW_Read[0] == 0&&g_ucaHW_Read[1] == 0 && g_ucaHW_Read[2] == 1&&g_ucaHW_Read[3]== 1 )
	{
		g_cThisState = 1.5;	//更快速左转
	}

	g_fHW_PID_Out = PID_realize(&PidHW_Tracking,g_cThisState);	
//PID计算输出目标速度这个速度，会和基础速度加减
	g_fHW_PID_Out1 = 1.5 + g_fHW_PID_Out;							
//电机1速度=基础速度+循迹PID输出速度
	g_fHW_PID_Out2 = 1.5 - g_fHW_PID_Out;							
//电机2速度=基础速度-循迹PID输出速度
	if(g_fHW_PID_Out1 >2.5) g_fHW_PID_Out1 =2.5;					
//进行限幅 限幅速度在0-5之间
	if(g_fHW_PID_Out1 <0) g_fHW_PID_Out1 =0;
	if(g_fHW_PID_Out2 >2.5) g_fHW_PID_Out2 =2.5;
	if(g_fHW_PID_Out2 <0) g_fHW_PID_Out2 =0;
		
	if(g_cThisState != g_cLastState)
	{	
//如何这次状态不等于上次状态、就进行改变目标速度和控制电机、在定时器中依旧定时控制电机
		MotorPidSetSpeed(g_fHW_PID_Out1,g_fHW_PID_Out2);		//通过计算的速度控制电机
	}
	g_cLastState = g_cThisState;//保存上次红外对管状态

增加运行速度的方向

1，时时改变小车基本速度（根据不同情况，细分状态）

2，增加红外对管数码（增加不同情况，加快小车状态的改变，细分状态）

四，蓝牙调试

先用电脑连接（用串口），然后蓝牙连接手机，测试出口收发信号。

之后用电脑直接连接单片机（用串口），发送信号，看是否可以控制小车。

五，蓝牙控制小车运动

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if( huart == &huart3)//判断中断源
	{
		if(g_ucUsart3ReceiveData == 'A') MotorPidSetSpeed(1,1);		//前运动
		if(g_ucUsart3ReceiveData == 'B') MotorPidSetSpeed(-1,-1);	//后运动
		if(g_ucUsart3ReceiveData == 'C') MotorPidSetSpeed(0,0);		//停止
		if(g_ucUsart3ReceiveData == 'D') MotorPidSetSpeed(1,2);		//右边运动
		if(g_ucUsart3ReceiveData == 'E') MotorPidSetSpeed(2,1);		//左边运动
		if(g_ucUsart3ReceiveData == 'F') MotorPidSpeedUp();			//加速
		if(g_ucUsart3ReceiveData == 'G') MotorPidSpeedCut();		//减速
		
		if(g_ucUsart3ReceiveData == 'H')//转向90度
		{
			if(PidMPU6050Yaw.Target_val <= 180)PidMPU6050Yaw.Target_val += 90;//目标值
		}
		if(g_ucUsart3ReceiveData == 'I')//转回90度
		{
			if(PidMPU6050Yaw.Target_val >= -180)PidMPU6050Yaw.Target_val -= 90;//目标值
		}
		
		if(g_ucUsart3ReceiveData == 'J') {Car_Mode++;}			//改变模式
		if(g_ucUsart3ReceiveData == 'K') {Car_Mode = 0;}		//设置为显示模式
		
		HAL_UART_Receive_IT( &huart3, &g_ucUsart3ReceiveData, 1);	//继续进行中断接收
	}
}

六，超声波小车

1，避障

if(HC_SR04_Read() > 25)//前方无障碍物
		{
			MotorPidSetSpeed(1,1);//前运动
			HAL_Delay(100);
		}
		else//前方有障碍物
		{	
			MotorPidSetSpeed(-0.5,0.5);//右边运动 原地	
			HAL_Delay(500);
			if(HC_SR04_Read() > 25)//右边无障碍物
			{
				MotorPidSetSpeed(1,1);//前运动
				HAL_Delay(100);
			}
			else//右边有障碍物
			{
				MotorPidSetSpeed(0.5,-0.5);//左边运动 原地
				HAL_Delay(1000);
				if(HC_SR04_Read() >25)//左边无障碍物
				{
					 MotorPidSetSpeed(1,1);//前运动
					HAL_Delay(100);
				}
				else
				{
					MotorPidSetSpeed(-1,-1);//后运动
					HAL_Delay(1000);
					MotorPidSetSpeed(-0.5,0.5);//右边运动
					HAL_Delay(50);
				}
			}
		}

2，跟随

        g_fHC_SR04_Read = HC_SR04_Read();
		if(g_fHC_SR04_Read < 40)
		{
			g_fFollow_PID_Out = PID_realize(&PidFollow,g_fHC_SR04_Read);
			if(g_fFollow_PID_Out > 4){g_fFollow_PID_Out = 4;}
			if(g_fFollow_PID_Out < -4){g_fFollow_PID_Out = -4;}
			MotorPidSetSpeed(g_fFollow_PID_Out,g_fFollow_PID_Out);
		}
		else
		{
			MotorPidSetSpeed(0,0);
		}
		
		HAL_Delay(10);//读取超声波传感器不能过快
		
		sprintf((char *)OledString1,"Mode:%d",Car_Mode);//显示小车模式
		OLED_ShowString(0,7,OledString1,12);

七，MPU6050实现闭环控制

A = 0x21;          //A = 0010 0001
A &= 0x0F;         //A = A & (0000 1111) = 0000 0001
A |= 1 << 4;       //A = A | (0000 0001 << 4) = A | 0001 0000 = 0x11(0001 0001)

c语言知识：

&:都1为1，|:都0为0 , <<的优先级比 & 和 | 大。

        sprintf((char *)Usart3String,"pitch:%.2f roll:%.2f yaw:%.2f\r\n",Pitch,Roll,Yaw);
        //显示6050数据 俯仰角 横滚角 航向角
		HAL_UART_Transmit(&huart3,( uint8_t *)Usart3String,strlen(( const  char  *)Usart3String),0xFFFF);
        //通过串口三输出字符 strlen:计算字符串大小	
	   
	   //mpu_dmp_get_data(&pitch,&roll,&yaw);//返回值:0,DMP成功解出欧拉角
		while(mpu_dmp_get_data(&Pitch,&Roll,&Yaw)!=0){}  //这个可以解决经常读不出数据的问题
		
		g_fMPU6050YawPidOut = PID_realize(&PidMPU6050Yaw,Yaw);
        //PID计算输出目标速度 这个速度，会和基础速度加减
		g_fMPU6050YawPidOut1 = 1.5 + g_fMPU6050YawPidOut;//基础速度加减PID输出速度
		g_fMPU6050YawPidOut2 = 1.5 - g_fMPU6050YawPidOut;
		if(g_fMPU6050YawPidOut1 >3.5) g_fMPU6050YawPidOut1 =3.5;//进行限幅
		if(g_fMPU6050YawPidOut1 <0) g_fMPU6050YawPidOut1 =0;
		if(g_fMPU6050YawPidOut2 >3.5) g_fMPU6050YawPidOut2 =3.5;
		if(g_fMPU6050YawPidOut2 <0) g_fMPU6050YawPidOut2 =0;
		MotorPidSetSpeed(g_fMPU6050YawPidOut1,g_fMPU6050YawPidOut2);