小车跟随行驶系统（基于MSP-EXP430F5529LP系统板）

选用材料：主控板MSP-EXP430F5529LP、陀螺仪、直流减速电机（可以选用光电编码器，霍尔电机不好调节PID）、TB6612电机驱动、超声波测距模块、灰度传感器、无线透传/蓝牙模块（便于两辆小车相互发送信息）、OLED屏等。

总体思路：使用灰度传感器巡线，超声波检测前后车距，通过调节PID的位置环，控制两辆小车前后的距离，运用JY901进行陀螺仪矫正。

2022TI_C1_JY901.c

#include "2022TI_C1_JY901.h"

struct SAngle Mpu_angle;

//串口0初始化
void Usart0Init(void)
{
    GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P3, GPIO_PIN4);
    GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P3, GPIO_PIN3);

    //Baudrate = 115200, clock freq = 25MHz
    //在线计算器
    //https://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html
    USCI_A_UART_initParam param = {0};
    param.selectClockSource = USCI_A_UART_CLOCKSOURCE_SMCLK;
    param.clockPrescalar = 13;
    param.firstModReg = 9;
    param.secondModReg = 0;
    param.parity = USCI_A_UART_NO_PARITY;
    param.msborLsbFirst = USCI_A_UART_LSB_FIRST;
    param.numberofStopBits = USCI_A_UART_ONE_STOP_BIT;
    param.uartMode = USCI_A_UART_MODE;
    param.overSampling = USCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION;


    if (STATUS_FAIL == USCI_A_UART_init(USCI_A0_BASE, &param))
    {
        return;
    }

    //Enable UART module for operation
    USCI_A_UART_enable(USCI_A0_BASE);

    //Enable Receive Interrupt
    USCI_A_UART_clearInterrupt(USCI_A0_BASE,USCI_A_UART_RECEIVE_INTERRUPT);
    USCI_A_UART_enableInterrupt(USCI_A0_BASE,USCI_A_UART_RECEIVE_INTERRUPT);
}

#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=USCI_A0_VECTOR
__interrupt
#elif defined(__GNUC__)
__attribute__((interrupt(USCI_A0_VECTOR)))
#endif
void USCI_A0_ISR (void)
{
//    uint8_t receivedData = 0;

    switch (__even_in_range(UCA0IV,4))
    {
        //Vector 2 - RXIFG
        case 2:
            CopeSerial2Data(USCI_A0_BASE);
//            receivedData = USCI_A_UART_receiveData(USCI_A0_BASE);
            break;
        default: break;
    }
}

//发送N个字节长度的数据
void USART0_Send(uint8_t *pui8Buffer, uint32_t ui32Count)
{
  while(ui32Count--)
  {
      USCI_A_UART_transmitData(USCI_A0_BASE, *pui8Buffer++);
  }
}

void CopeSerial2Data(uint16_t baseAddress)
{
    static unsigned char ucRxBuffer[250];
    static unsigned char ucRxCnt = 0;
    unsigned char i,sum;

    ucRxBuffer[ucRxCnt++]=USCI_A_UART_receiveData(baseAddress);;   //将收到的数据存入缓冲区中

    if (ucRxBuffer[0]!=0x55) //数据头不对，则重新开始寻找0x55数据头
    {
        ucRxCnt=0;
        return;
    }
    if (ucRxCnt<11) {return;}//数据不满11个，则返回
    else
    {
        switch(ucRxBuffer[1])//三轴角度
        {
            case 0x53:
                memcpy(&Mpu_angle,&ucRxBuffer[2],8);
                for(i=0; i<10; i++)
                    sum += ucRxBuffer[i];

                if(sum == ucRxBuffer[10])
                {
                    Mpu_angle.angle_z=  (float)Mpu_angle.Angle[2]/32768*180;//Z
                    Mpu_angle.angle_y = (float)Mpu_angle.Angle[0]/32768*180;//Y
                    Mpu_angle.angle_x = (float)Mpu_angle.Angle[1]/32768*180;//X

                }
//                memcpy(&Mpu_angle,&ucRxBuffer[2],8);
//                Mpu_angle.angle_z=  (float)Mpu_angle.Angle[2]/32768*180;//Z
//                Mpu_angle.angle_y = (float)Mpu_angle.Angle[0]/32768*180;//Y
//                Mpu_angle.angle_x = (float)Mpu_angle.Angle[1]/32768*180;//X
            break;
            default:break;
        }
        ucRxCnt=0;//清空缓存区
    }
}

2022TI_C1_JY901.h

#ifndef __2022TI_C1_JY901_H_
#define __2022TI_C1_JY901_H_

#include <includes.h>


struct SAngle
{
    short Angle[3];
    short T;
    float angle_x;
    float angle_y;
    float angle_z;
};

extern struct SAngle Mpu_angle;

//串口0初始化
void Usart0Init(void);
void USART0_Send(uint8_t *pui8Buffer, uint32_t ui32Count);

void CopeSerial2Data(uint16_t baseAddress);

#endif /* JY901_H_ */

2022TI_C1_SR04.c

#include "2022TI_C1_SR04.h"

float sr04_dist = 0.0;
uint32_t Sign_Counts = 0;//脉冲宽度(高) us

//启动测量
void SR04_Start(void)
{
    GPIO_setOutputHighOnPin(GPIO_PORT_P7, GPIO_PIN4);
    delay_us(50);
    GPIO_setOutputLowOnPin(GPIO_PORT_P7, GPIO_PIN4);
}

//SR04 trig p7.4
//SR04 echo p2.5
void Timer_A2_Capture_Init(void)
{
    Timer_A_initContinuousModeParam htim = {0};
    htim.clockSource = TIMER_A_CLOCKSOURCE_SMCLK;
    htim.clockSourceDivider = TIMER_A_CLOCKSOURCE_DIVIDER_1;
    htim.timerInterruptEnable_TAIE = TIMER_A_TAIE_INTERRUPT_ENABLE;
    htim.timerClear = TIMER_A_DO_CLEAR;
    htim.startTimer = true;
    Timer_A_initContinuousMode(TIMER_A2_BASE, &htim);

    GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P2, GPIO_PIN5);
    Timer_A_initCaptureModeParam capture_htim = {0};
    capture_htim.captureRegister = TIMER_A_CAPTURECOMPARE_REGISTER_2;
    capture_htim.captureMode = TIMER_A_CAPTUREMODE_RISING_AND_FALLING_EDGE;
    capture_htim.captureInputSelect = TIMER_A_CAPTURE_INPUTSELECT_CCIxA;
    capture_htim.synchronizeCaptureSource = TIMER_A_CAPTURE_SYNCHRONOUS;
    capture_htim.captureInterruptEnable = TIMER_A_CAPTURECOMPARE_INTERRUPT_ENABLE;
    capture_htim.captureOutputMode = TIMER_A_OUTPUTMODE_OUTBITVALUE;
    Timer_A_initCaptureMode(TIMER_A2_BASE,&capture_htim);

    GPIO_setAsOutputPin(GPIO_PORT_P7, GPIO_PIN4);
    GPIO_setOutputLowOnPin(GPIO_PORT_P7, GPIO_PIN4);
    GPIO_setOutputLowOnPin(GPIO_PORT_P2, GPIO_PIN5);
}

#pragma vector=TIMER2_A1_VECTOR
__interrupt
void TIMER2_A1_ISR (void)
{
    static uint16_t Overflow_Times = 0;
    static uint16_t Sign_Begin = 0, Sign_End = 0;

    switch(TA2IV)
    {
        case TA2IV_TACCR2:
            if(GPIO_getInputPinValue(GPIO_PORT_P2,GPIO_PIN5))
            {
                Sign_Begin = Timer_A_getCaptureCompareCount(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2);
            }
            else
            {
                Sign_End = Timer_A_getCaptureCompareCount(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2);
                if(!Overflow_Times)
                    Sign_Counts = Sign_End - Sign_Begin;
                else
                {
                    Sign_Counts = (uint32_t)65536 * Overflow_Times + Sign_End - Sign_Begin;
                    Overflow_Times = 0;
                }

                //25MHZ
                //计数周期 1/25 us 340M/S
                sr04_dist = 0.04 *0.34 * Sign_Counts / 2.0;//mm
            }
            Timer_A_clearCaptureCompareInterrupt(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2);
            break;
        case TA2IV_TAIFG:
            if(GPIO_getInputPinValue(GPIO_PORT_P2,GPIO_PIN5))
            {
                ++Overflow_Times;
            }
            else
                Overflow_Times = 0;
            Timer_A_clearTimerInterrupt(TIMER_A2_BASE);
            break;
        default:
            break;
    }
}

2022TI_C1_SR04.h

#ifndef __2022TI_C1_SR04__H
#define __2022TI_C1_SR04__H

#include <includes.h>

extern float sr04_dist;
extern uint32_t Sign_Counts;

void SR04_Start(void);

void Timer_A2_Capture_Init(void);

#endif 

2022TI_C1_PID.c

#include "2022TI_C1_PID.h"

PID M3508_spid[2];
PID ANGLE;
PID distance;
float abs_limit(float a, float ABS_MAX)
{
	if(a > ABS_MAX)
		a = ABS_MAX;

	if(a < -ABS_MAX)
		a = -ABS_MAX;
	return a;
}

void PID_Position_Calc( PID *pp,  float  CurrentPoint,  float NextPoint )  
{   
	pp->Error =  NextPoint -  CurrentPoint;          
	pp->SumError += pp->Error;                      
	pp->DError = pp->Error - pp->LastError;

	pp->output =  pp->Proportion * pp->Error +   \
								abs_limit(pp->Integral * pp->SumError, pp->Integralmax ) +   \
								pp->Derivative * pp->DError ;  

	if(pp->output > pp->outputmax )  pp->output = pp->outputmax;
	if(pp->output < - pp->outputmax )  pp->output = -pp->outputmax;
//	pp->PrevError = pp->LastError;  
	pp->LastError = pp->Error;
}

void PID_Incremental_Calc( PID *pp,  float  CurrentPoint,  float NextPoint )  
{  
	pp->Error =  NextPoint -  CurrentPoint;          
	pp->SumError += pp->Error;                      
	pp->DError = pp->Error - pp->LastError;

	pp->output +=  pp->Proportion * ( pp->Error - pp->LastError )+   \
								 abs_limit(pp->Integral * pp->Error, pp->Integralmax ) +   \
								 pp->Derivative * ( pp->Error +  pp->PrevError - 2*pp->LastError);  

	if(pp->output > pp->outputmax )  pp->output = pp->outputmax;
	if(pp->output < - pp->outputmax )  pp->output = -pp->outputmax;
	pp->PrevError = pp->LastError;  
	pp->LastError = pp->Error;
}

void PIDInit(PID *pp, float Kp , float Ki , float Kd ,  float outputmax, float Integralmax)  
{  
	pp->Integralmax = Integralmax;
	pp->outputmax  = outputmax;
	pp->Proportion = Kp;
	pp->Integral   = Ki;
	pp->Derivative = Kd;
	pp->DError = pp->Error = pp->output = pp->LastError = pp->PrevError = 0; 
}  

2022TI_C1_PID.h

#ifndef __2022TI_C1_PID_H
#define __2022TI_C1_PID_H

typedef struct PID {
  float  Proportion;         //  Proportional Const  
  float  Integral;           //  Integral Const  
  float  Derivative;         //  Derivative Const  
	float  PrevError;          //  Error[-2]
  float  LastError;          //  Error[-1]  
	float  Error;
	float  DError;
  float  SumError;           //  Sums of Errors  
	float  Integralmax;
	float  output;
	float  outputmax;
} PID;

extern PID M3508_spid[2];
extern PID ANGLE;
extern PID distance;

float abs_limit(float a, float ABS_MAX);
void PID_Position_Calc( PID *pp,  float  CurrentPoint,  float NextPoint);
void PID_Incremental_Calc( PID *pp,  float  CurrentPoint,  float NextPoint);
void PIDInit(PID *pp, float Kp , float Ki , float Kd ,  float outputmax, float Integralmax);

#endif