效果图：

波形图软件是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_PINwhile(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*100)/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，每个人有每个人的写法，实在不会在评论区回复就行。