一,基本运动
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);