選用材料：主控板MSP-EXP430F5529LP、陀螺儀、直流減速電機(jī)（可以選用光電編碼器，霍爾電機(jī)不好調(diào)節(jié)PID）、TB6612電機(jī)驅(qū)動(dòng)、超聲波測(cè)距模塊、灰度傳感器、無線透?jìng)?藍(lán)牙模塊（便于兩輛小車相互發(fā)送信息）、OLED屏等。
總體思路：使用灰度傳感器巡線，超聲波檢測(cè)前后車距，通過調(diào)節(jié)PID的位置環(huán)，控制兩輛小車前后的距離，運(yùn)用JY901進(jìn)行陀螺儀矯正。
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
//在線計(jì)算器
//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, ¶m))
{
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;
}
}
//發(fā)送N個(gè)字節(jié)長(zhǎng)度的數(shù)據(jù)
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);; //將收到的數(shù)據(jù)存入緩沖區(qū)中
if (ucRxBuffer[0]!=0x55) //數(shù)據(jù)頭不對(duì)，則重新開始尋找0x55數(shù)據(jù)頭
{
ucRxCnt=0;
return;
}
if (ucRxCnt<11) {return;}//數(shù)據(jù)不滿11個(gè)，則返回
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;//清空緩存區(qū)
}
}
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
//啟動(dòng)測(cè)量
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
//計(jì)數(shù)周期 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文章來源：http://www.zghlxwxcb.cn/news/detail-522684.html
#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文章來源地址http://www.zghlxwxcb.cn/news/detail-522684.html
#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
到了這里，關(guān)于小車跟隨行駛系統(tǒng)（基于MSP-EXP430F5529LP系統(tǒng)板）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
