效果图：

波形图软件是VOFA，B站有教程 ，虽然有缺点但是非常简单。

视频效果：

之前发过只有超声波测距的代码，MSPM0G3507（三十二）——超声波模块移植代码-CSDN博客

 SYSCFG配置：

 ultrasonic_two.c

#include "ti_msp_dl_config.h"
#include "Delay_two.h"
#include "motor_two.h"
int distance=0;
int overcount=0;      //记录定时器溢出次数  
int  Senor_Using();
extern int MOTO1,MOTO2;
int left_pwm=0;
    int right_pwm=0;
//PID控制距离      (速度环)             第二题的PID
int Senor_control_PID(int true_distance,int Target_distance)
{ 	
	 float Position_KP=40,Position_KI=0.2,Position_KD=1.2;
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Target_distance-true_distance;                                  //计算偏差
	 Integral_bias+=Bias;	                                 //求出偏差的积分
	 Pwm=-(Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias));       //位置式PID控制器
	 Last_Bias=Bias;                                       //保存上一次偏差 
	 return Pwm;                                           //增量输出


}


void Senor_control()
{
    
    distance=Senor_Using();
   
    left_pwm= Senor_control_PID(distance,15);;
    right_pwm= Senor_control_PID(distance,15);;
MOTO1=left_pwm;
MOTO2= right_pwm;

    Limit(&MOTO1,&MOTO2);	//速度限幅
    Load(MOTO1,MOTO2);		 //加载到电机上。

}



//测距函数
int  Senor_Using() 
{
		unsigned int sum=0;
		unsigned int tim;
		unsigned int i=0;
		unsigned int length;
		int cnt_i=0;
		while(i!=3)        
		{
			DL_GPIO_setPins( ultrasonic_Port_PORT ,ultrasonic_Port_TRIG_Pin_PIN );                     //TRIG=1;              //给发送端高电平
			delay_us(20);    
			 DL_GPIO_clearPins(ultrasonic_Port_PORT ,ultrasonic_Port_TRIG_Pin_PIN );               //TRIG=0;              //给发送端低电平
			cnt_i=0;

          //DL_GPIO_readPins(xunji_PORT_PIN1_PORT                 , xunji_PORT_PIN1_PIN       )==xunji_PORT_PIN1_PIN
			while(DL_GPIO_readPins(ultrasonic_Port_PORT,ultrasonic_Port_MCHO_Pin_PIN)!=ultrasonic_Port_MCHO_Pin_PIN)    //当接收端为0，一直循环      while(ECHO==0)
			{
				cnt_i++;
				delay_us(1);
				if(cnt_i>300)
				{	
                                             
                      DL_GPIO_setPins( ultrasonic_Port_PORT ,ultrasonic_Port_TRIG_Pin_PIN );        // TRIG=1;        发送端给高电平  
					delay_us(20);


					 DL_GPIO_clearPins(ultrasonic_Port_PORT ,ultrasonic_Port_TRIG_Pin_PIN );                //TRIG=0;    发送端给低电平
                    
                    cnt_i=0;          
				}
			} 

                //跳出循环说明已经接收到信号，开启定时器记录时间
			DL_Timer_startCounter(TIMER_ultrasonic_INST    );     //  TIM_Cmd(TIM3,ENABLE);      使能定时器                                         
			
			i+=1;                     
			
			while(DL_GPIO_readPins(ultrasonic_Port_PORT,ultrasonic_Port_MCHO_Pin_PIN)==ultrasonic_Port_MCHO_Pin_PIN)	          //这里是知道信号结束，等待定时器计时      while(ECHO==1);	     
            {

            }



			 DL_Timer_stopCounter(TIMER_ultrasonic_INST) ;                  //TIM_Cmd(TIM3,DISABLE);         失能定时器                     
			
			 tim=DL_Timer_getTimerCount(TIMER_ultrasonic_INST) ;                  //tim=TIM_GetCounter(TIM3);         得到时计单元个数
			length=(tim*50)/58.0;                                               //微秒计算法求距离
			sum=length+sum;
			DL_Timer_setTimerCount(TIMER_ultrasonic_INST,0) ;                          //TIM3->CNT=0;     将计数值清零
			overcount=0;                                        //一般来说没有超过一个定时器周期该值不需要调用，一直给0即可
			//delay_ms(10);
		}
		length=sum/3;                                           //三次作为平均值
		return length; 
}

//测距定时器中断服务函数
void TIMER_ultrasonic_INST_IRQHandler (void)
{
        switch (DL_TimerA_getPendingInterrupt(TIMER_ultrasonic_INST))
    {
        case DL_TIMER_IIDX_ZERO:
                overcount++;




            break;
        default:
            break;
    }




}

ultrasonic_two.h

#ifndef __ultrasonic_two_H
#define	__ultrasonic_two_H




#include "ti_msp_dl_config.h"

int Senor_Using() ;
void TIMER_ultrasonic_INST_IRQHandler (void);
int Senor_control_PID(int true_distance,int Target_distance);
void Senor_control();

#endif

主函数中while循环调用  Senor_control();即可

这里只列出了超声波的所有代码

有些函数没有列出来，大家可以自己给他加上去，就是驱动电机的程序，非常简单。比如limit和load 一个是速度限幅一个是给电机加载PWM，每个人有每个人的写法，实在不会在评论区回复就行。