选用材料:主控板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, ¶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;
}
}
//发送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文章来源:https://www.toymoban.com/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文章来源地址https://www.toymoban.com/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
到了这里,关于小车跟随行驶系统(基于MSP-EXP430F5529LP系统板)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!