我感觉你完全忘记了我前面给你的代码，我再给你一次你不要搞错了/*
* Copyright (c) 2021, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in sour
ce and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of sour
ce code must retain the above copyright
* noti
ce, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reprodu
ce the above copyright
* noti
ce, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, IN
DIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVI
CES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGEN
CE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "ti_msp_dl_config.h"
#include "
motor.h"
#include "PID.h"
#include "xunji.h"
#include "atk_ms901m.h"
int16_t baseSpeed = 65;
//int16_t SpeedL = 0, SpeedR = 0;
float Kp = 0.5;
//uint8_t key1_flag;
volatile int32_t enco_l;
volatile int32_t enco_r;
volatile uint32_t gpioA,gpioB;
void GROUP1_IRQHandler(void);
uint8_t A;
void encoder_update(void);
volatile int32_t enco_l_last,enco_r_last;
float speed_l,speed_r;
uint32_t gEchoData;
uint32_t date[20];
uint8_t date_index;
atk_ms901m_attitude_data_t attitude_dat;
// 全局变量
PID pid_left; // 左轮PID控制器
PID pid_right; // 右轮PID控制器
float target_speed_l = 200.0f; // 左轮目标速度（mm/s）
float target_speed_r = 200.0f; // 右轮目标速度（mm/s）
// 初始化PID（示例参数，需调试）
void PID_Init_Config(void) {
PID_Init(&pid_left, 2.0f, 0.5f, 0.1f, 100.0f); // P=2, I=0.5, D=0.1，输出限幅±100
PID_Init(&pid_right, 2.0f, 0.5f, 0.1f, 100.0f); // 左右轮参数可相同或独立调整
}
int main(void)
{
SYSCFG_DL_init();
NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
NVIC_EnableIRQ(ENCODER_GPIOA_INT_IRQN);
NVIC_EnableIRQ(ENCODER_GPIOB_INT_IRQN);
NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN);
NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
PID_Init_Config(); // 添加PID初始化
//if(!DL_GPIO_readPins(GPIO_KEY_PORT,GPIO_KEY_PIN_KEY1_PIN))
//{
//delay_cycles(32000000);
Set_Speed(0, baseSpeed);
Set_Speed(1, baseSpeed);
Motor_On();
//}
//DL_GPIO_clearPins(GPIO_LED_PORT,GPIO_LED_PIN_0_PIN);
atk_ms901m_uart_init();//初始化函数
atk_ms901m_get_attitude(&attitude_dat, 100);//获取参数，第一个参数是结构体，第二个参数是out time(s失联时间)
while (1) {
xunji();
//if(gEchoData==49)
//DL_GPIO_clearPins(GPIO_LED_PORT,GPIO_LED_PIN_0_PIN);
}
}
void encoder_update(void)
{
// 关闭全局中断（或仅关闭编码器中断）
__disable_irq();
enco_l_last = enco_l;
enco_r_last = enco_r;
enco_l = 0;
enco_r = 0;
// 恢复全局中断
__enable_irq();
// 计算速度（
单位：mm/s，假设车轮周长=π*直径=3.14*65mm=204.1mm）
// 公式：速度 = (脉冲数 / 每圈脉冲数) * 周长 / 时间周期
// 每圈脉冲数 = 编码器线数 × 减速比 = 13 × 28 = 364脉冲/圈
// 时间周期=20ms（假设定时器中断周期为20ms，即0.02秒）
float per_round_pulse = 13.0f * 28.0f; // 364脉冲/圈
float wheel_circumferen
ce = 3.14159f * 65.0f; // 车轮周长（mm）
float time_period = 0.02f; // 20ms = 0.02秒
speed_l = (enco_l_last / per_round_pulse) * wheel_circumferen
ce / time_period; // 左轮速度（mm/s）
speed_r = (enco_r_last / per_round_pulse) * wheel_circumferen
ce / time_period; // 右轮速度（mm/s）
}
void speed_
control(void)
{
//zuolun pid
float duty_l = PID_Realize(&pid_left, speed_l, target_speed_l);
//youlun pid
float duty_r = PID_Realize(&pid_right, speed_r, target_speed_r);
//通过Set_Speed输出PWM占空比（duty已由PID限幅为-100~100）
Set_Speed(0, (int8_t)duty_l); // 0=左轮，传递计算的duty值
Set_Speed(1, (int8_t)duty_r); // 1=右轮
}
void GROUP1_IRQHandler(void)
{
//获取中断信号
gpioA = DL_GPIO_getEnabledInterruptStatus(GPIOA,ENCODER_E2A_PIN);
gpioB = DL_GPIO_getEnabledInterruptStatus(GPIOB,ENCODER_E1A_PIN);
//ENCODER RIGHT
if((gpioA&ENCODER_E2A_PIN)!=0)
{
if(!DL_GPIO_readPins(ENCODER_E2B_PORT,ENCODER_E2B_PIN))
{
enco_r++;
}
else
{
enco_r--;
}
DL_GPIO_clearInterruptStatus(GPIOA, ENCODER_E2A_PIN);
}
//ENCODER LEFT
if((gpioB&ENCODER_E1A_PIN)!=0)
{
DL_GPIO_clearPins(GPIO_LED_PORT,GPIO_LED_PIN_0_PIN);
if(!DL_GPIO_readPins(ENCODER_E1B_PORT,ENCODER_E1B_PIN))
{
enco_l++;
}
else
{
enco_l--;
}
DL_GPIO_clearInterruptStatus(GPIOB,ENCODER_E1A_PIN);
}
}
void TIMER_0_INST_IRQHandler(void)
{
switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST))
{
case DL_TIMERA_IIDX_ZERO:
// 处理计数到零的中断
encoder_update();
speed_
control();
break;
default:
break;
}
}
void UART_0_INST_IRQHandler(void) {
switch (DL_UART_Main_getPendingInterrupt(UART_0_INST)) {
/*! UART interrupt index for UART re
ceive */
case DL_UART_MAIN_IIDX_RX:
gEchoData = DL_UART_Main_re
ceiveData(UART_0_INST);
DL_GPIO_clearPins(GPIO_LED_PORT,GPIO_LED_PIN_0_PIN);
//DL_UART_Main_transmitData(UART_0_INST, gEchoData);
break;
default:
break;
}
}
void
Motor_On(void)
{
DL_GPIO_setPins(GPIO_
MOTOR_PIN_STBY_PORT, GPIO_
MOTOR_PIN_STBY_PIN);
//DL_GPIO_clearPins(GPIO_LED_PORT,GPIO_LED_PIN_0_PIN);
}
void
Motor_Off(void)
{
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_STBY_PORT, GPIO_
MOTOR_PIN_STBY_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_L1_PORT, GPIO_
MOTOR_PIN_L1_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_L2_PORT, GPIO_
MOTOR_PIN_L2_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_R1_PORT, GPIO_
MOTOR_PIN_R1_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_R2_PORT, GPIO_
MOTOR_PIN_R2_PIN);
}
void Set_Speed(uint8_t side, int8_t duty)
{
uint32_t compareValue = 0;
if(!side)
{
if(duty<0)
{
compareValue = 3199 - 3199 * (-duty/100.0);
DL_TimerA_setCaptureCompareValue(PWM_
MOTOR_INST, compareValue, DL_TIMER_CC_0_INDEX);
DL_GPIO_setPins(GPIO_
MOTOR_PIN_L1_PORT, GPIO_
MOTOR_PIN_L1_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_L2_PORT, GPIO_
MOTOR_PIN_L2_PIN);
}
else if(duty>0)
{
compareValue = 3199 - 3199 * (duty/100.0);
DL_TimerA_setCaptureCompareValue(PWM_
MOTOR_INST, compareValue, DL_TIMER_CC_0_INDEX);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_L1_PORT, GPIO_
MOTOR_PIN_L1_PIN);
DL_GPIO_setPins(GPIO_
MOTOR_PIN_L2_PORT, GPIO_
MOTOR_PIN_L2_PIN);
}
else
{
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_L1_PORT, GPIO_
MOTOR_PIN_L1_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_L2_PORT, GPIO_
MOTOR_PIN_L2_PIN);
}
}
else
{
if(duty<0)
{
compareValue = 3199 - 3199 * (-duty/100.0);
DL_TimerA_setCaptureCompareValue(PWM_
MOTOR_INST, compareValue, DL_TIMER_CC_1_INDEX);
DL_GPIO_setPins(GPIO_
MOTOR_PIN_R1_PORT, GPIO_
MOTOR_PIN_R1_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_R2_PORT, GPIO_
MOTOR_PIN_R2_PIN);
}
else if(duty>0)
{
compareValue = 3199 - 3199 * (duty/100.0);
DL_TimerA_setCaptureCompareValue(PWM_
MOTOR_INST, compareValue, DL_TIMER_CC_1_INDEX);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_R1_PORT, GPIO_
MOTOR_PIN_R1_PIN);
DL_GPIO_setPins(GPIO_
MOTOR_PIN_R2_PORT, GPIO_
MOTOR_PIN_R2_PIN);
}
else
{
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_R1_PORT, GPIO_
MOTOR_PIN_R1_PIN);
DL_GPIO_clearPins(GPIO_
MOTOR_PIN_R2_PORT, GPIO_
MOTOR_PIN_R2_PIN);
}
}
}#include "PID.h"
/*!
* @brief PID
* @sin
ce v1.0
* *pid: PID参数
* NowPla
ce：当前位置
* Point： 预期位置
*/
void PID_Init(PID *pid, float p, float i, float d, float limit){
pid->P = p;
pid->I = i;
pid->D = d;
pid->limit = limit;
pid->realize = 0.0f;
pid->Current_Error = 0;
pid->Last_Error = 0;
pid->Previous_Error = 0;
pid->Increase = 0;
}
// 位置式PID控制
float PID_Realize(PID *pid, float NowPla
ce, float Point)
{
float iError; // 当前误差
float Realize; //实际输出
iError = Point - NowPla
ce; // 计算当前误差
pid->Current_Error += pid->I * iError; // 误差积分
pid->Current_Error = pid->Current_Error > pid->limit?pid->limit:pid->Current_Error;//积分限幅
pid->Current_Error = pid->Current_Error <-pid->limit?-pid->limit:pid->Current_Error;
Realize = pid->P * iError //比例P
+ pid->Current_Error //积分I
+ pid->D * (iError - pid->Last_Error); //微分D
pid->Last_Error = iError; // 更新上次误差
pid->realize = Realize;
if(pid->limit<pid->realize) pid->realize = pid->limit;
else if (-pid->limit>pid->realize) pid->realize = -pid->limit;
return pid->realize; // 返回实际值
}
/*!
* @brief 增量式PID
* @sin
ce v1.0
* *pid ：误差参数
* *pid: PID参数
* NowPla
ce：实际值
* Point： 期望值
*/
//增量式PID电机控制
float PID_Increase(PID *pid, float NowPla
ce, float Point)
{
float iError; //当前误差
float Increase; //最后得出的实际增量
iError = Point - NowPla
ce; // 计算当前误差
Increase = pid->P * (iError - pid->Last_Error) //比例P
+ pid->I * iError //积分I
+ pid->D * (iError - 2 * pid->Last_Error + pid->Previous_Error); //微分D
pid->Previous_Error = pid->Last_Error; // 更新前次误差
pid->Last_Error = iError; // 更新上次误差
pid->Increase = Increase;
pid->realize = pid->realize + Increase;
if(pid->limit<pid->realize) pid->realize = pid->limit;
else if (-pid->limit>pid->realize) pid->realize = -pid->limit;
return pid->realize; // 返回实际值
}
本文通过两个实验探讨了不同放大倍数(Kp)对直流电机(DCMotor)调速性能的影响。实验中使用了额定参数为220V55A1000r/min的电机，并设置了不同的电枢电流加载方式。结果显示，放大倍数增加能够有效减小转速降，提高系统硬度。
扫一扫
1634
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
